Homework SourseI need help with my Arduino Walking Robot I want it to avoid
I need help with my Arduino Walking Robot. I want it to avoid obstacles and i am facing a problem right now with my code. Whenever my robot detects a wall in front of it I programmed it to go backwards and turn. But the problem with this is that the robot immediately after sensing the front wall and turning it automatically goes back to the middle because it doesent detect a wall anymore. I need to program the function so that it turns back(completes a function) for a certain amount time eg. turns back for 5 seconds.
I NEED THE SERVO MOTOR TO MOVE BACKWARDS FOR 5 SECONDS AND THEN TURN AND MOVE FORWARDS. servoDrive.write(0) is backwards and servoDrive.write(180) is forwards. I Need this to happen for the 2nd 3rd and 4th if statement functions. I was advised to you millis() but i do not know how to implement that in this scenario
i want it to be so that if the front sensor detects a wall it moves backwards and gets out of range of the front wall....after that action is complete it will turn and move forwards going away from the front wall .it can move to the right or left. It doesent matter to me. Or if it could decide to go left or right depending on how close the left and right wall is. For example if the left wall is 25cm away and the right wall is 20cm away it will go left instead of right. Here is an older video of the machine. you can see that there are two motors one controls movement the other controls direction. The robot walks much better now than it did on the video but this is for you to get a better idea of the system i am working with. https://vid.me/1Ift
HERE IS MY CODE SO FAR IF YOU CAN EDIT THIS CODE THAT WOULD BE GREAT
#include
int trigPin1 = 11;
int echoPin1 = 10;
int trigPin2 = 7;
int echoPin2 = 6;
int trigPin3 = 42;
int echoPin3 = 43;
int distance1, distance2, distance3;
long duration1, duration2, duration3;
Servo servoDrive; // Define left servo
Servo servoDirection; // Define right servo
boolean isFrontWallThere = false;
boolean isARightWallThere = false;
boolean isALeftWallThere = false;
void setup() {
pinMode(trigPin1, OUTPUT);
pinMode(echoPin1, INPUT);
pinMode(trigPin2, OUTPUT);
pinMode(echoPin2, INPUT);
pinMode(trigPin3, OUTPUT);
pinMode(echoPin3, INPUT);
servoDrive.attach(18); // Set left servo to digital pin 18
servoDirection.attach(14); // Set right servo to digital pin 14
}
void loop() {
digitalWrite (trigPin1, LOW);
delayMicroseconds(2);
digitalWrite (trigPin1, HIGH);
delayMicroseconds (5);
duration1 = pulseIn (echoPin1, HIGH);
distance1 = (duration1 / 2) / 29.1;
if ((distance2 > 0 && distance2 < 15)) {
isFrontWallThere = true;
}
else {
isFrontWallThere = false;
}
digitalWrite (trigPin2, LOW);
delayMicroseconds(2);
digitalWrite (trigPin2, HIGH);
delayMicroseconds (5);
duration2 = pulseIn (echoPin2, HIGH);
distance2 = (duration2 / 2) / 29.1;
if ((distance2 > 0 && distance2 < 15)) {
isALeftWallThere = true;
}
else {
isALeftWallThere = false;
}
digitalWrite (trigPin3, LOW);
delayMicroseconds(2);
digitalWrite (trigPin3, HIGH);
delayMicroseconds (5);
duration3 = pulseIn (echoPin3, HIGH);
distance3 = (duration3 / 2) / 29.1;
if ((distance3 > 0 && distance3 < 15)) {
isARightWallThere = true;
}
else {
isARightWallThere = false;
}
Serial.print(\"Front Sensor: \");
Serial.print(distance1);
Serial.print(\"cm \");
Serial.print(\"Left Sensor: \");
Serial.print(distance2);
Serial.print(\"cm \");
Serial.print(\"Right Sensor: \");
Serial.print(distance3);
Serial.print(\"cm\");
Serial.println(\"\ \");
//delay(100);
//moving to forward
if (isFrontWallThere == false && isARightWallThere == false && isALeftWallThere == false)
{
servoDrive.write(180);
servoDirection.write(180);
delay(300);
//the motor will rotate backwards.
Serial.println(\"\ \");
Serial.println(\"All Clear! going forwards\");
}
//moving to back
else if (isFrontWallThere == true && isARightWallThere == false && isALeftWallThere == false)
{
servoDrive.write(0);
servoDirection.write(0);
delay(500);
Serial.println(\"\ \");
Serial.println(\"Object detected in front of me.....moving back\");
}
//moving to right
else if (isFrontWallThere == false && isARightWallThere == false && isALeftWallThere == true)
{
servoDrive.write(180);
servoDirection.write(0);
delay(5000);
Serial.println(\"\ \");
Serial.println(\"Object detected to the LEFT.....moving to the right\");
}
//moving to left
else if (isFrontWallThere == false && isARightWallThere == true && isALeftWallThere == false) {
servoDrive.write(0);
servoDirection.write(180);
delay(500);
Serial.println(\"\ \");
Serial.println(\"Object detected to the RIGHT.....moving to the left\");
}
}
Solution
#include <Servo.h>
int trigPin1 = 11;
int echoPin1 = 10;
int trigPin2 = 7;
int echoPin2 = 6;
int trigPin3 = 42;
int echoPin3 = 43;
int distance1, distance2, distance3;
long duration1, duration2, duration3;
Servo servoDrive; // Define left servo
Servo servoDirection; // Define right servo
boolean isFrontWallThere = false;
boolean isARightWallThere = false;
boolean isALeftWallThere = false;
void setup() {
pinMode(trigPin1, OUTPUT);
pinMode(echoPin1, INPUT);
pinMode(trigPin2, OUTPUT);
pinMode(echoPin2, INPUT);
pinMode(trigPin3, OUTPUT);
pinMode(echoPin3, INPUT);
servoDrive.attach(18); // Set left servo to digital pin 18
servoDirection.attach(14); // Set right servo to digital pin 14
}
void loop() {
digitalWrite (trigPin1, LOW);
delayMicroseconds(2);
digitalWrite (trigPin1, HIGH);
delayMicroseconds (5);
duration1 = pulseIn (echoPin1, HIGH);
distance1 = (duration1 / 2) / 29.1;
if ((distance1 > 0 && distance1 < 15)) {
isFrontWallThere = true;
}
else {
isFrontWallThere = false;
}
digitalWrite (trigPin2, LOW);
delayMicroseconds(2);
digitalWrite (trigPin2, HIGH);
delayMicroseconds (5);
duration2 = pulseIn (echoPin2, HIGH);
distance2 = (duration2 / 2) / 29.1;
if ((distance2 > 0 && distance2 < 15)) {
isALeftWallThere = true;
}
else {
isALeftWallThere = false;
}
digitalWrite (trigPin3, LOW);
delayMicroseconds(2);
digitalWrite (trigPin3, HIGH);
delayMicroseconds (5);
duration3 = pulseIn (echoPin3, HIGH);
distance3 = (duration3 / 2) / 29.1;
if ((distance3 > 0 && distance3 < 15)) {
isARightWallThere = true;
}
else {
isARightWallThere = false;
}
Serial.print(\"Front Sensor: \");
Serial.print(distance1);
Serial.print(\"cm \");
Serial.print(\"Left Sensor: \");
Serial.print(distance2);
Serial.print(\"cm \");
Serial.print(\"Right Sensor: \");
Serial.print(distance3);
Serial.print(\"cm\");
Serial.println(\"\ \");
//delay(100);
//moving to forward
if (isFrontWallThere == false && isARightWallThere == false && isALeftWallThere == false)
{
servoDrive.write(180);
servoDirection.write(180);
delay(300);
Serial.println(\"\ \");
Serial.println(\"All Clear! going forwards\");
}
//moving to back
else if (isFrontWallThere == true && isARightWallThere == false && isALeftWallThere == false)
{
servoDrive.write(0);
servoDirection.write(0);
delay(500);
if(distance2 >distance 3){ //move left since distance from left wall to the robot is high
servoDrive.write(0);
servoDirection.write(180);
}
else{ //move right
servoDrive.write(180);
servoDirection.write(0);
}
Serial.println(\"\ \");
Serial.println(\"Object detected in front of me.....moving back\");
}
//moving to right
else if (isFrontWallThere == false && isARightWallThere == false && isALeftWallThere == true)
{
servoDrive.write(180);
servoDirection.write(0);
delay(5000);
Serial.println(\"\ \");
Serial.println(\"Object detected to the LEFT.....moving to the right\");
}
//moving to left
else if (isFrontWallThere == false && isARightWallThere == true && isALeftWallThere == false) {
servoDrive.write(0);
servoDirection.write(180);
delay(500);
Serial.println(\"\ \");
Serial.println(\"Object detected to the RIGHT.....moving to the left\");
}
}
