I am trying to make a autonomous rover for an upcoming competition for my school, and am having trouble with some of my code. At least I think it's the code that Is the problem.
Simply, there are three ultrasound sensors that get their distances reported ever 1/10 of a second. These values then go through a few "if" statements to see what motors should/should not be enabled. The sensors work fine, but none of my motors seem to be working. Any advice is greatly appreciated!
#define trigPin1 13
#define echoPin1 12
#define trigPin2 11
#define echoPin2 10
#define trigPin3 9
#define echoPin3 8
#define FrontRightMotor 2
#define FrontLeftMotor 3
#define BackRightMotor 4
#define BackLeftMotor 5
long duration, distance, RightSensor,BackSensor,FrontSensor,LeftSensor;
void setup()
{
Serial.begin (9600);
pinMode(trigPin1, OUTPUT);
pinMode(echoPin1, INPUT);
pinMode(trigPin2, OUTPUT);
pinMode(echoPin2, INPUT);
pinMode(trigPin3, OUTPUT);
pinMode(echoPin3, INPUT);
pinMode(FrontRightMotor, OUTPUT);
pinMode(FrontLeftMotor, OUTPUT);
pinMode(BackRightMotor, OUTPUT);
pinMode(BackLeftMotor, OUTPUT);
}
void loop()
{
SonarSensor(trigPin1, echoPin1);
RightSensor = distance;
SonarSensor(trigPin2, echoPin2);
LeftSensor = distance;
SonarSensor(trigPin3, echoPin3);
FrontSensor = distance;
Serial.print(LeftSensor);
Serial.print(" - ");
Serial.print(FrontSensor);
Serial.print(" - ");
Serial.println(RightSensor);
if (RightSensor < 5)
{
digitalWrite(FrontRightMotor, HIGH);
digitalWrite(BackRightMotor, HIGH);
digitalWrite(FrontLeftMotor, LOW);
digitalWrite(BackLeftMotor, LOW);
}
else if (LeftSensor < 5)
{
digitalWrite(FrontRightMotor, LOW);
digitalWrite(BackRightMotor, LOW);
digitalWrite(FrontLeftMotor, HIGH);
digitalWrite(BackLeftMotor, HIGH);
}
else if (FrontSensor < 5)
{
if(LeftSensor < 5)
{
digitalWrite(FrontRightMotor, LOW);
digitalWrite(BackRightMotor, LOW);
digitalWrite(FrontLeftMotor, HIGH);
digitalWrite(BackLeftMotor, HIGH);
}
else if(RightSensor < 5)
{
digitalWrite(FrontRightMotor, HIGH);
digitalWrite(BackRightMotor, HIGH);
digitalWrite(FrontLeftMotor, LOW);
digitalWrite(BackLeftMotor, LOW);
}
}
else
{
digitalWrite(FrontRightMotor, HIGH);
digitalWrite(BackRightMotor, HIGH);
digitalWrite(FrontLeftMotor, HIGH);
digitalWrite(BackLeftMotor, HIGH);
}
}
void SonarSensor(int trigPin,int echoPin)
{
digitalWrite(trigPin, LOW);
delayMicroseconds(2);
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance = (duration/2) / 29.1;
}
I am using a modified version of the code that is listed here: Interfacing of Multiple Ultrasonic Sensor With Arduino - The Engineering Projects