I am trying to implement a bot which can sense obstacle in front, left or right and post detection send interrupts. Which in turn will perform the appropriate motor function to avoid these obsticles. Any help would be highly appreciated. It would be great if you could make changes in this code and post it so that I can actually run, simulate and find out where actually I have gone wrong.
My code
#define trigPin 14
#define echoPin 12
#define echoPin1 13
#define echoPin2 15
//Initialize Digital Pins
const int leftForward = 2;
const int leftBackward = 16;
const int rightForward = 5;
const int rightBackward = 4;
// Define variables:
long duration;
long duration1;
long duration2;
int distance;
int distance1;
int distance2;
boolean goesForward = false;
bool flag = false;
bool flag1 = false;
bool flag2 = false;
void f5(void){
flag = true;
}
void f2(void){
flag1 = true;
}
void f3(void){
flag2 = true;
}
void inter1(){
Serial.print("Distance1 = ");
Serial.print(distance2);
Serial.println(" cm");
moveStop();
delay(3000);
moveBackward();
delay(4000);
moveStop();
delay(3000);
turnRight();
moveStop();
delay(3000);
}
void inter2(){
Serial.print("Distance2 = ");
Serial.print(distance2);
Serial.println(" cm");
moveStop();
delay(3000);
moveBackward();
delay(4000);
moveStop();
delay(3000);
turnRight();
moveStop();
delay(3000);
}
void inter3(){
Serial.print("Distance2 = ");
Serial.print(distance2);
Serial.println(" cm");
moveStop();
delay(3000);
moveBackward();
delay(4000);
moveStop();
delay(3000);
turnRight();
moveStop();
delay(3000);
}
void setup() {
// Define inputs and outputs:
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
pinMode(echoPin1, INPUT);
pinMode(echoPin2, INPUT);
// set control pins as Output
pinMode(leftForward,OUTPUT);
pinMode(leftBackward,OUTPUT);
pinMode(rightForward,OUTPUT);
pinMode(rightBackward,OUTPUT);
attachInterrupt(digitalPinToInterrupt(echoPin), f5, RISING);
attachInterrupt(digitalPinToInterrupt(echoPin1), f2, RISING);
attachInterrupt(digitalPinToInterrupt(echoPin2), f3, RISING);
//Begin Serial communication at a baudrate of 9600:
Serial.begin(9600);
}
void loop() {
// Clear the trigPin by setting it LOW:
digitalWrite(trigPin, LOW);
delayMicroseconds(5);
// Trigger the sensor by setting the trigPin high for 10 microseconds:
digitalWrite(trigPin, HIGH);
delayMicroseconds(10);
digitalWrite(trigPin, LOW);
// Read the echoPin, pulseIn() returns the duration (length of the pulse) in microseconds:
duration = pulseIn(echoPin, HIGH);
// Calculate the distance:
distance= duration*0.034/2;
duration1 = pulseIn(echoPin1, HIGH);
// Calculate the distance:
distance1= duration1*0.034/2;
duration2 = pulseIn(echoPin2, HIGH);
// Calculate the distance:
distance2= duration2*0.034/2;
// Print the distance on the Serial Monitor (Ctrl+Shift+M):
if (distance <= 20){
attachInterrupt(digitalPinToInterrupt(12), f5, RISING);
}
else{
moveForward();
}
if (distance1 <= 20){
attachInterrupt(digitalPinToInterrupt(13), f2, RISING);
}
else{
moveForward();
}
if (distance2 <= 20){
attachInterrupt(digitalPinToInterrupt(15), f3, RISING);
}
else{
moveForward();
}
if(flag)
{
inter1();
flag = false;
}
if(flag1)
{
inter2();
flag1 = false;
}
if(flag2)
{
inter3();
flag2 = false;
}
delay(1000);
/* // run forward
digitalWrite(leftForward,HIGH);
digitalWrite(leftBackward,LOW);
digitalWrite(rightForward,HIGH);
digitalWrite(rightBackward,LOW);
delay(1000);
// run left
digitalWrite(leftForward,LOW);
digitalWrite(leftBackward,LOW);
digitalWrite(rightForward,HIGH);
digitalWrite(rightBackward,LOW);
delay(1000);
// run forward
digitalWrite(leftForward,HIGH);
digitalWrite(leftBackward,LOW);
digitalWrite(rightForward,LOW);
digitalWrite(rightBackward,LOW);
delay(1000);
// run backward
digitalWrite(leftForward,LOW);
digitalWrite(leftBackward,HIGH);
digitalWrite(rightForward,LOW);
digitalWrite(rightBackward,HIGH);
delay(1000);
delay(50);
*/
}
void moveStop(){
digitalWrite(rightForward, LOW);
digitalWrite(leftForward, LOW);
digitalWrite(rightBackward, LOW);
digitalWrite(leftBackward, LOW);
}
void moveForward(){
if(!goesForward){
goesForward=true;
digitalWrite(leftForward, HIGH);
digitalWrite(rightForward, HIGH);
digitalWrite(leftBackward, LOW);
digitalWrite(rightBackward, LOW);
}
}
void moveBackward(){
goesForward=false;
digitalWrite(leftBackward, HIGH);
digitalWrite(rightBackward, HIGH);
digitalWrite(leftForward, LOW);
digitalWrite(rightForward, LOW);
}
void turnRight(){
digitalWrite(leftForward, HIGH);
digitalWrite(rightBackward, HIGH);
digitalWrite(leftBackward, LOW);
digitalWrite(rightForward, LOW);
delay(500);
digitalWrite(leftForward, HIGH);
digitalWrite(rightForward, HIGH);
digitalWrite(leftBackward, LOW);
digitalWrite(rightBackward, LOW);
}
void turnLeft(){
digitalWrite(leftBackward, HIGH);
digitalWrite(rightForward, HIGH);
digitalWrite(leftForward, LOW);
digitalWrite(rightBackward, LOW);
delay(500);
digitalWrite(leftForward, HIGH);
digitalWrite(rightForward, HIGH);
digitalWrite(leftBackward, LOW);
digitalWrite(rightBackward, LOW);
}