This is my coding currently. I have changed the stop time to 30 sec and simplified step 4~5. But it does not function as well as I thought. It stops and goes smooth but it does not move away when an object is blocking. What statement should I use?
#include <SoftwareSerial.h>
#define LEFT_A1 4
#define LEFT_B1 5
#define RIGHT_A2 6
#define RIGHT_B2 7
#define IR_TRIG 9
#define IR_ECHO 8
void setup() {
Serial.begin(9600);
pinMode(LEFT_A1, OUTPUT);
pinMode(RIGHT_A2, OUTPUT);
pinMode(LEFT_B1, OUTPUT);
pinMode(RIGHT_B2, OUTPUT);
pinMode(IR_TRIG, OUTPUT);
pinMode(IR_ECHO, INPUT);
}
void loop() {
float duration, distance;
digitalWrite(IR_TRIG, HIGH);
delay(10);
digitalWrite(IR_TRIG, LOW);
duration = pulseIn(IR_ECHO, HIGH);
distance = ((float)(340 * duration) / 10000) / 2;
Serial.print("\nDistance : ");
Serial.println(distance);
int sum = 0;
if(distance < 20) {
Serial.println("stop");
stop();
sum++ ;
while (sum > 10) {
Serial.println("backward");
backward ();
Serial.println("left");
left ();
Serial.println("forwardi");
forwardi ();
Serial.println("right");
right ();
Serial.println("forwardi");
forwardi ();
Serial.println("forwardi");
forwardi ();
Serial.println("right");
right ();
Serial.println("forwardi");
forwardi ();
Serial.println("left");
left ();
Serial.println("forward");
forward();
}
}else {
Serial.println("forward");
forward();
}
}
void forward(){
digitalWrite(LEFT_A1, HIGH);
digitalWrite(LEFT_B1, LOW);
digitalWrite(RIGHT_A2, HIGH);
digitalWrite(RIGHT_B2, LOW);
}
void forwardi (){
digitalWrite(LEFT_A1, HIGH);
digitalWrite(LEFT_B1, LOW);
digitalWrite(RIGHT_A2, HIGH);
digitalWrite(RIGHT_B2, LOW);
delay (4000);
}
void backward(){
digitalWrite(LEFT_A1, LOW);
digitalWrite(LEFT_B1, HIGH);
digitalWrite(RIGHT_A2, LOW);
digitalWrite(RIGHT_B2, HIGH);
delay(1000);
}
void left(){
digitalWrite(LEFT_A1, LOW);
digitalWrite(LEFT_B1, HIGH);
digitalWrite(RIGHT_A2, HIGH);
digitalWrite(RIGHT_B2, LOW);
delay(1000);
}
void right(){
digitalWrite(LEFT_A1, HIGH);
digitalWrite(LEFT_B1, LOW);
digitalWrite(RIGHT_A2, LOW);
digitalWrite(RIGHT_B2, HIGH);
delay(1000);
}
void stop(){
digitalWrite(LEFT_A1, LOW);
digitalWrite(LEFT_B1, LOW);
digitalWrite(RIGHT_A2, LOW);
digitalWrite(RIGHT_B2, LOW);
delay(3000);
}