Hi, I am controlling two motors with an arduino using ultrasonic sensors. My if and else statements work for Forward and Stop and Right but the Left function under the if statement within the first if statement is being ignored. I am only ever getting a Right even when the condition states otherwise.
Help would be greatly appreciated. Here is my code:
#include <Arduino.h>
#include <avr/wdt.h>
#define SV1 6 // Motor1 Direction
#define SV2 5 // Motor2 Direction
const int trigPin_f = 13;
const int echoPin_f = 12;
const int trigPin_r = 11;
const int echoPin_r = 10;
const int trigPin_l = 9;
const int echoPin_l = 8;
const int limitDistance = 10;
float duration_f, duration_l, duration_r, distance_f, distance_l, distance_r;
void setup() {
// Ultrasonic sensor
Serial.begin(9600);
pinMode(trigPin_f, OUTPUT);
pinMode(echoPin_f, INPUT);
pinMode(trigPin_l, OUTPUT);
pinMode(echoPin_l, INPUT);
pinMode(trigPin_r, OUTPUT);
pinMode(echoPin_r, INPUT);
// Motor Controller
pinMode(SV1, OUTPUT);
pinMode(SV2, OUTPUT);
// Display
}
void Stop() {
digitalWrite(SV1, LOW);
digitalWrite(SV2, LOW);
}
void Forward() {
digitalWrite(SV1, HIGH);
digitalWrite(SV2, HIGH);
}
void Right() { // May need to change label if reversed
digitalWrite(SV1, HIGH);
digitalWrite(SV2, LOW);
}
void Left() { // May need to change label if reversed
digitalWrite(SV1, LOW);
digitalWrite(SV2, HIGH);
}
void loop() {
digitalWrite(trigPin_f, LOW);
digitalWrite(trigPin_l, LOW);
digitalWrite(trigPin_r, LOW);
delayMicroseconds(2);
digitalWrite(trigPin_f, HIGH);
digitalWrite(trigPin_l, HIGH);
digitalWrite(trigPin_r, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin_f, LOW);
digitalWrite(trigPin_l, LOW);
digitalWrite(trigPin_r, LOW);
duration_f = pulseIn(echoPin_f, HIGH);
distance_f = (microsecondsToCentimeters(duration_f));
duration_l = pulseIn(echoPin_l, HIGH);
distance_l = (microsecondsToCentimeters(duration_l));
duration_r = pulseIn(echoPin_r, HIGH);
distance_r = (microsecondsToCentimeters(duration_r));
// Display distance
// Serial.print("Distance: ");
// Serial.println(distance);
if (distance_f < limitDistance){
Stop();
Serial.print("Stop");
if (distance_r < distance_l){
Left();
Serial.print("Left");
}
else {
Right();
Serial.print("Right");
}
}
else {
Forward();
Serial.print("Forward");
}
}
long microsecondsToCentimeters(long microseconds) {
// The speed of sound is 340 m/s or 29 microseconds per centimeter.
// The ping travels out and back, so to find the distance of the object we
// take half of the distance travelled.
return microseconds / 29 / 2;
}