I used 3 hc-sr04 , Adafruit Motor Shield and arduino mega 2569.
One of hc-sr04 can not work.
But them can work without Adafruit Motor Shield.
#include <NewPing.h>
#include <SoftwareSerial.h>
#include <AFMotor.h>
//SoftwareSerial mySerial(32,33); // RX, TX
#define SONAR_NUM 3 // Number or sensors.
#define MAX_DISTANCE 300 // Max distance in cm.
#define PING_INTERVAL 66// Milliseconds between pings.
char val;
byte dir = 0;
unsigned long pingTimer[SONAR_NUM]; // When each pings.
unsigned int cm[SONAR_NUM]; // Store ping distances.
uint8_t currentSensor = 0; // Which sensor is active.
NewPing sonar[SONAR_NUM] = { // Sensor object array.
NewPing (30, 31, MAX_DISTANCE),
NewPing (40, 41, MAX_DISTANCE),
NewPing (50, 51, MAX_DISTANCE)
};
AF_DCMotor leftMotor2(1, MOTOR12_64KHZ); // create motor #1 using M1 output on Motor Drive Shield, set to 1kHz PWM frequency
AF_DCMotor rightMotor2(2, MOTOR12_64KHZ); // create motor #2, using M2 output, set to 1kHz PWM frequency
AF_DCMotor rightMotor1(3, MOTOR34_1KHZ);// create motor #3, using M3 output, set to 1kHz PWM frequency
AF_DCMotor leftMotor1(4, MOTOR34_1KHZ);// create motor #4, using M4 output, set to 1kHz PWM frequency
String motorSet = "";
const int dangerThresh = 15;
void setup() {
Serial.begin(115200);
//mySerial.begin(9600);
leftMotor1.setSpeed(200);
rightMotor2.setSpeed(200);
rightMotor1.setSpeed(200);
leftMotor2.setSpeed(200);//可調轉速約150~到255
//pinMode(IA, OUTPUT);
//pinMode(IB, OUTPUT);
pingTimer[0] = millis() + 75; // First ping start in ms.
for (uint8_t i = 1; i < SONAR_NUM; i++) // Set the starting time for each sensor.
pingTimer[i] = pingTimer[i - 1] + PING_INTERVAL;
}
//-------------------------------------------------------------------------------------------------------------------------------------
void stop() {leftMotor1.run(RELEASE); leftMotor2.run(RELEASE); rightMotor1.run(RELEASE); rightMotor2.run(RELEASE);} // stop the motors.
//-------------------------------------------------------------------------------------------------------------------------------------
void forward() {
motorSet = "FORWARD";
leftMotor1.run(FORWARD); // turn it on going forward
leftMotor2.run(FORWARD); // turn it on going forward
rightMotor1.run(FORWARD); // turn it on going forward
rightMotor2.run(FORWARD); // turn it on going forward
}
//-------------------------------------------------------------------------------------------------------------------------------------
void backward() {
motorSet = "BACKWARD";
leftMotor1.run(BACKWARD); // turn it on going backward
leftMotor2.run(BACKWARD); // turn it on going backward
rightMotor1.run(BACKWARD); // turn it on going backward
rightMotor2.run(BACKWARD); // turn it on going backward
}
//-------------------------------------------------------------------------------------------------------------------------------------
void turnRight() {
motorSet = "RIGHT";
leftMotor1.run(BACKWARD); // turn motor 1 forward
leftMotor2.run(BACKWARD); // turn motor 2 forward
rightMotor1.run(FORWARD); // turn motor 3 backward
rightMotor2.run(FORWARD); // turn motor 4 backward
}
//-------------------------------------------------------------------------------------------------------------------------------------
void turnLeft() {
motorSet = "LEFT";
leftMotor1.run(FORWARD); // turn motor 1 backward
leftMotor2.run(FORWARD); // turn motor 2 backward
//leftMotor1.setSpeed(speedSet+MAX_SPEED_OFFSET);
//leftMotor2.setSpeed(speedSet+MAX_SPEED_OFFSET);
rightMotor1.run(BACKWARD); // turn motor 3 forward
rightMotor2.run(BACKWARD); // turn motor 4 forward
}
//-------------------------------------------------------------------------------------------------------------------------------------
void turnAround() {
motorSet = "RIGHT";
leftMotor1.run(BACKWARD); // turn motor 1 forward
leftMotor2.run(BACKWARD); // turn motor 2 forward
rightMotor1.run(FORWARD); // turn motor 3 backward
rightMotor2.run(FORWARD); // turn motor 4 backward
}
void echoCheck() { // If ping echo, set distance to array.
// Serial.println("echoCheck");
// Serial.println(currentSensor);
if (sonar[currentSensor].check_timer())
cm[currentSensor] = sonar[currentSensor].ping_result / US_ROUNDTRIP_CM;
// Serial.print("cm x ");
// Serial.println(cm[currentSensor]);
}
void oneSensorCycle() { // Do something with the results.
for (uint8_t i = 0; i < SONAR_NUM; i++) {
Serial.print("ping");
Serial.print(i);
Serial.print("=");
Serial.print(cm[i]);
Serial.print("cm ");
//Serial.print(sonar[currentSensor].ping_result / US_ROUNDTRIP_CM);
}
//Serial.print(cm[currentSensor]);
Serial.println();
//------------------------------------------------------------------------------------------
if (cm[0] > dangerThresh && cm[1] > dangerThresh && cm[2] > dangerThresh){ //0 0 0
Serial.println("0 0 0");
if (dir != 0) {
dir = 0;
stop();
delay(50);
}
forward();
delay(100);
}
else if(cm[0] > dangerThresh && cm[1] > dangerThresh && cm[2] < dangerThresh){ //0 0 1
Serial.println("0 0 1");
if (dir != 1) {
dir = 1;
stop();
delay(50);
}
turnLeft();
delay(100);
}
else if(cm[0] > dangerThresh && cm[1] < dangerThresh && cm[2] < dangerThresh){ //0 1 1
Serial.println("0 1 1");
if (dir != 1) {
dir = 1;
stop();
delay(50);
}
turnLeft();
delay(100);
}
else if(cm[0] < dangerThresh && cm[1] > dangerThresh && cm[2] > dangerThresh){ //1 0 0
Serial.println("1 0 0");
if (dir != 1) {
dir = 1;
stop();
delay(50);
}
turnRight();
delay(100);
}
else if(cm[0] < dangerThresh && cm[1] < dangerThresh && cm[2] > dangerThresh){ //1 1 0
Serial.println("1 1 0");
if (dir != 1) {
dir = 1;
stop();
delay(50);
}
turnRight();
delay(100);
}
else if(cm[0] > dangerThresh && cm[1] < dangerThresh && cm[2] > dangerThresh){ //0 1 0
Serial.println("0 1 0");
if (dir != 1) {
dir = 1;
stop();
delay(50);
}
turnLeft();
delay(100);
}
else if(cm[0] > dangerThresh && cm[1] < dangerThresh && cm[2] < dangerThresh){ //1 0 1
Serial.println("1 0 1");
if (dir != 0) {
dir = 0;
stop();
delay(50);
}
forward();
delay(100);
}
else if(cm[0] < dangerThresh && cm[1] < dangerThresh && cm[2] < dangerThresh){ //1 1 1
Serial.println("1 1 1");
if (dir != 1) {
dir = 1;
stop();
delay(50);
}
backward();
delay(100);
}
//--------------------------------------------------------------------------------------------
}
void loop() {
for (uint8_t i = 0; i < SONAR_NUM; i++) {
if (millis() >= pingTimer[i]) { // Is it this sensor's time to ping?
pingTimer[i] += PING_INTERVAL * SONAR_NUM; // Set next time this sensor will be pinged.
if (i == 0 && currentSensor == SONAR_NUM - 1) oneSensorCycle(); // Sensor ping cycle complete, do something with the results.
sonar[currentSensor].timer_stop(); // Make sure previous timer is canceled before starting a new ping (insurance).
currentSensor = i; // Sensor being accessed.
//cm[currentSensor] = 0; // Make distance zero in case there's no ping echo for this sensor.
sonar[currentSensor].ping_timer(echoCheck); // Do the ping (processing continues, interrupt will call echoCheck to look for echo).
// Serial.print("i = ");
// Serial.println(i);
// Serial.print("currentSensor =");
// Serial.println(currentSensor);
//Serial.println("currentSensor = %d",currentSensor);
}
}
}