Could someone explain why what's wrong with the code? (dist3=distance forward, dist1=distance left, dist2=distance right)
#include <Servo.h>
#define trig 13
#define echo 12
Servo myservo;
int servoLeft = 0; //angle of microservo rotation to scan on left
int servoForward = 90;
int servoRight = 180; //angle of microservo rotation to scan on right
int motorA1 = 2;
int motorA2 = 4;
int motorB1 = 6;
int motorB2 = 7;
int val, dist1, dist2, dist3;
void setup() {
Serial.begin(9600);
myservo.attach(9);
pinMode(trig, OUTPUT);
pinMode(echo, INPUT);
pinMode(motorA1, OUTPUT);
pinMode(motorA2, OUTPUT);
pinMode(motorB1, OUTPUT);
pinMode(motorB2, OUTPUT);
}
void loop() {
scan();
Serial.println(dist3);
if(dist3>40){
do{
forward();
scanForwardServo();
}while(dist3>35);
stop();
}
else if(dist1>dist2){
backward();
delay(300);
left();
delay(1200);
stop();
scanForwardServo();
if(dist3>40){
do{
forward();
scanForwardServo();
}while(dist3>35);
stop();
}
}
else if(dist2>dist1){
backward();
delay(300);
right();
delay(1200);
stop();
scanForwardServo();
if(dist3>40){
do{
forward();
scanForwardServo();
}while(dist3>35);
stop();
}
}
}
void scanForwardServo(){
myservo.write(servoForward);
digitalWrite(trig, HIGH);
delay(500);
digitalWrite(trig, LOW);
val = pulseIn(echo, HIGH);
dist3 = (val/2)/29.1;
}
void scanRightServo(){
myservo.write(servoRight);
delay(50);
digitalWrite(trig, HIGH);
delay(500);
digitalWrite(trig, LOW);
val = pulseIn(echo, HIGH);
dist2 = (val/2)/29.1;
}
void scanLeftServo(){
myservo.write(servoLeft);
delay(50);
digitalWrite(trig, HIGH);
delay(500);
digitalWrite(trig, LOW);
val = pulseIn(echo, HIGH);
dist1 = (val/2)/29.1;
}
void scan(){
scanLeftServo();
delay(150);
scanRightServo();
delay(150);
scanForwardServo();
delay(150);
}
void forward(){
digitalWrite(motorA1, HIGH);
digitalWrite(motorA2, LOW);
digitalWrite(motorB1, HIGH);
digitalWrite(motorB2, LOW);
}
void backward(){
digitalWrite(motorA1, LOW);
digitalWrite(motorA2, HIGH);
digitalWrite(motorB1, LOW);
digitalWrite(motorB2, HIGH);
}
void left(){
digitalWrite(motorA1, HIGH);
digitalWrite(motorA2, LOW);
digitalWrite(motorB1, LOW);
digitalWrite(motorB2, LOW);
}
void right(){
digitalWrite(motorA1, LOW);
digitalWrite(motorA2, LOW);
digitalWrite(motorB1, HIGH);
digitalWrite(motorB2, LOW);
}
void stop(){
digitalWrite(motorA1, LOW);
digitalWrite(motorA2, LOW);
digitalWrite(motorB1, LOW);
digitalWrite(motorB2, LOW);
}
Sometimes the robot car turns in the right direction (depends on distance, comparing left and right side), but sometimes in goes in the opposite direction (the direction with less distance). Why does this occur and how can I fix it? As you can see in the code I did use a serial monitor to read the distances to insure that everything is ok with the sensor.