I´m trying now to follow Paul's advise but the code is still not doing what I want.
This is the new code:
#include <Servo.h>
Servo motorLeft; // Define left motor
Servo motorRight; // Define right motor
#define trigPin A2
#define echoPin A3
int i;
boolean turnLeft = true;
void setup() {
Serial.begin (9600);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
motorLeft.attach(A5); // Set left motor to digital pin 4
motorRight.attach(A4); // Set right motor to digital pin 7
}
void loop() {
int duration, distance;
digitalWrite(trigPin, HIGH);
delayMicroseconds(1000);
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance = (duration/2) / 29.1;
if (distance >= 30 || distance <= 0){
Serial.println("Out of range");
motorLeft.write(20);
motorRight.write(20);
}
if (distance <= 30){
if(turnLeft){
Serial.print(distance);
Serial.println( "cm");
motorLeft.write(170);
motorRight.write(10);
delay (1500);
turnLeft = false;
}
else {
Serial.print(distance);
Serial.println( "cm");
motorLeft.write(10);
motorRight.write(170);
delay (1500);
turnLeft = true;
}
}
delay(100);
}
The problem is that now when the robot finds and obstacule it turns right for 1500ms and then left for 1500ms in a loop. But what I want is for robot turn right when it finds and obstacule for as long as it takes to get it sorted. And then when it finds the next obstacule to do the same but now turning left. I have tried many variations on the code, but I still can get the outcome wanted.