Hello 
, I am looking into a project that deals with using two motors that are controlled by 3 sonar senors (rightside,leftside,front). I am new to coding and am having a tough time troubleshooting the problem with the code. My overall goal is to have the right sonar have the robot turn left, left one having it turn right, and front one having it turn around (This will be turned off and on with the switch). I’m sure this is probably a simplistic solution but as I said I am a beginner at coding. My code is based off of sparkfuns arduino kit but trying to add those two extra sonar detectors.
Code:
int AIN1 = 13;
int AIN2 = 12;
int PWMA = 11;
int PWMB = 10;
int BIN2 = 9;
int BIN1 = 8;
int switchPin = 7;
int trigPin1=6;
int echoPin1=5;
int trigPin2=1;
int echoPin2=0;
int trigPin3=3;
int echoPin3=2;
int backupTime = 300;
int turnTime = 200;
void setup() {
Serial.begin (9600);
pinMode(trigPin1, OUTPUT);
pinMode(echoPin1, INPUT);
pinMode(trigPin2, OUTPUT);
pinMode(echoPin2, INPUT);
pinMode(trigPin3, OUTPUT);
pinMode(echoPin3, INPUT);
pinMode(switchPin, INPUT_PULLUP);
pinMode(AIN1, OUTPUT);
pinMode(AIN2, OUTPUT);
pinMode(PWMA, OUTPUT);
pinMode(BIN1, OUTPUT);
pinMode(BIN2, OUTPUT);
pinMode(PWMB, OUTPUT);
Serial.begin(9600);
Serial.print("STARTING...");
}
void rightMotor(int motorSpeed) //function for driving the right motor
{
if (motorSpeed > 0) //if the motor should drive forward (positive speed)
{
digitalWrite(AIN1, HIGH); //set pin 1 to high
digitalWrite(AIN2, LOW); //set pin 2 to low
}
else if (motorSpeed < 0) //if the motor should drive backward (negative speed)
{
digitalWrite(AIN1, LOW); //set pin 1 to low
digitalWrite(AIN2, HIGH); //set pin 2 to high
}
else //if the motor should stop
{
digitalWrite(AIN1, LOW); //set pin 1 to low
digitalWrite(AIN2, LOW); //set pin 2 to low
}
analogWrite(PWMA, abs(motorSpeed)); //now that the motor direction is set, drive it at the entered speed
}
void leftMotor(int motorSpeed) //function for driving the left motor
{
if (motorSpeed > 0) //if the motor should drive forward (positive speed)
{
digitalWrite(BIN1, HIGH); //set pin 1 to high
digitalWrite(BIN2, LOW); //set pin 2 to low
}
else if (motorSpeed < 0) //if the motor should drive backward (negative speed)
{
digitalWrite(BIN1, LOW); //set pin 1 to low
digitalWrite(BIN2, HIGH); //set pin 2 to high
}
else //if the motor should stop
{
digitalWrite(BIN1, LOW); //set pin 1 to low
digitalWrite(BIN2, LOW); //set pin 2 to low
}
analogWrite(PWMB, abs(motorSpeed)); //now that the motor direction is set, drive it at the entered speed
}
void loop() {
long duration1, distance1;
digitalWrite(trigPin1, LOW); // Added this line
delayMicroseconds(2); // Added this line
digitalWrite(trigPin1, HIGH);
delayMicroseconds(10); // Added this line
digitalWrite(trigPin1, LOW);
duration1 = pulseIn(echoPin1, HIGH);
distance1 = (duration1/2) / 29.1;
//FOR RIGHT SENSOR
if(distance1 < 10){ //if an object is detected
Serial.print(" ");
Serial.print("TURN LEFT");
rightMotor(0);
leftMotor(0);
delay(200);
rightMotor(255);
leftMotor(-255);
delay(turnTime);
}
else {
Serial.print(" ");
Serial.print("Moving...");
rightMotor(255);
leftMotor(255);
}
} else{ //if the switch is off then stop
rightMotor(0);
leftMotor(0);
}
delay(50); //wait 50 milliseconds between readings
long duration2, distance2;
digitalWrite(trigPin2, LOW); // Added this line
delayMicroseconds(2); // Added this line
digitalWrite(trigPin2, HIGH);
delayMicroseconds(10); // Added this line
digitalWrite(trigPin2, LOW);
duration2 = pulseIn(echoPin2, HIGH);
distance2= (duration2/2) / 29.1;
//FOR LEFT SENSOR
if(distance2 < 10){ //if an object is detected
Serial.print(" ");
Serial.print("TURN RIGHT");
rightMotor(0);
leftMotor(0);
delay(200);
rightMotor(-255);
leftMotor(255);
delay(turnTime);
}
else {
Serial.print(" ");
Serial.print("Moving...");
rightMotor(255);
leftMotor(255);
}
} else{ //if the switch is off then stop
rightMotor(0);
leftMotor(0);
}
delay(50); //wait 50 milliseconds between readings
long duration3, distance3;
digitalWrite(trigPin3, LOW); // Added this line
delayMicroseconds(2); // Added this line
digitalWrite(trigPin3, HIGH);
delayMicroseconds(10); // Added this line
digitalWrite(trigPin3, LOW);
duration3 = pulseIn(echoPin3, HIGH);
distance3= (duration3/2) / 29.1;
//FOR FRONT SENSOR
if(distance1 < 10){ //if an object is detected
Serial.print(" ");
Serial.print("TURN RIGHT");
rightMotor(0);
leftMotor(0);
delay(200);
rightMotor(-255);
leftMotor(-255);
delay(turnTime);
}
else {
Serial.print(" ");
Serial.print("Moving...");
rightMotor(255);
leftMotor(255);
}
} else{ //if the switch is off then stop
rightMotor(0);
leftMotor(0);
}
delay(50); //wait 50 milliseconds between readings
}