I am doing my final-year project of an autonomous river cleaning robot. I seem to have a problem with my code.. For now, i want to read my Ultrasonic Sensors (HCSR-04) while getting my DC motors to move and find a route once an obstacle is detected. Since my robot is driving freely in an open water, i have program my motors to run in a specific pattern (although the pattern is not complete yet). But my problem is, once my Ultrasonic detects an obstacle, my motor does not immediately stop and find another route.. Instead, it finishes my pattern first before finding another route.
Please help me with my code and if there's any suggestions.
//Initialization
#include<Servo.h>
#include <printf.h>
int trigPin1 = 22; //Front
int echoPin1 = 23;
int trigPin2 = 32; //Left
int echoPin2 = 33;
int trigPin3 = 38; //Right
int echoPin3 = 39;
int motorpin1 = 5; //right motor
int motorpin2 = 6; //left motor
int object = 20; //30cm
long duration1, duration2, duration3, cm1, cm2, cm3, inches1, inches2, inches3;
Servo m1,m2,m3;
void setup() {
Serial.begin (9600);
pinMode(trigPin1, OUTPUT);
pinMode(echoPin1, INPUT);
pinMode(trigPin2, OUTPUT);
pinMode(echoPin2, INPUT);
pinMode(trigPin3, OUTPUT);
pinMode(echoPin3, INPUT);
pinMode(motorpin1, OUTPUT);
pinMode(motorpin2, OUTPUT);
m1.attach(motorpin1);
m2.attach(motorpin2);
}
void loop() {
frontsensor();
leftsensor();
rightsensor();
if(cm1 > object){
forwardpattern();
}
if(cm1 <= object){
findroute();
}
}
void forwardpattern(){
forward();
delay(5000); //Move Forward for 5 second
halt();
turnright();
forward();
delay(8000); //Move Forward for 5 second
turnleft();
}
void forward(){
m1.write(50);
delay(5);
m2.write(50);
delay(5);
}
void findroute(){
halt();
if (cm2 < cm3){
Serial.println("Turn Right");
turnright();
}
else{
Serial.println("Turn Left");
turnleft();
}
}
void halt(){
m1.write(0);
m2.write(0);
delay(200); //wait 2 second
}
void turnright(){
m1.write(20);
m2.write(80);
delay(500); //turn right for 2 second
}
void turnleft(){
m1.write(80);
m2.write(20);
delay(500); //turn left for 2 second
}
void frontsensor(){
digitalWrite(trigPin1, LOW);
delayMicroseconds(5);
digitalWrite(trigPin1, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin1, LOW);
pinMode(echoPin1, INPUT);
duration1 = pulseIn(echoPin1, HIGH);
cm1 = ((duration1)/2) / 29.1;
}
void leftsensor(){
digitalWrite(trigPin2, LOW);
delayMicroseconds(5);
digitalWrite(trigPin2, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin2, LOW);
pinMode(echoPin2, INPUT);
duration2 = pulseIn(echoPin2, HIGH);
cm2 = ((duration2)/2) / 29.1;
}
void rightsensor(){
digitalWrite(trigPin3, LOW);
delayMicroseconds(5);
digitalWrite(trigPin3, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin3, LOW);
pinMode(echoPin3, INPUT);
duration3 = pulseIn(echoPin3, HIGH);
cm3 = ((duration3)/2) / 29.1;
}