I have made this simple code for an obstacle avoiding robot. When the ultrasonic sensor detects an obstacle the robot turns in one direction. The mod I need is a way to make the robot turn in one direction first and then in the other direction when it detects the next obstacle.
#include <Servo.h>
Servo motorLeft; // Define left motor
Servo motorRight; // Define right motor
#define trigPin A2
#define echoPin A3
void setup() {
Serial.begin (9600);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
motorLeft.attach(A5); // Set left motor to pin A5
motorRight.attach(A4); // Set right motor to pin A4
}
void loop() {
int duration, distance; // ultrasonic sensor routine
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(85); // robot go forward routine
motorRight.write(85);
}
else
{
Serial.print(distance);
Serial.println( "cm");
motorLeft.write(170); // robot turn right routine
motorRight.write(10);
}
delay(100);
}
The robot is controlled by an Arduino UNO R3 and uses two modified 360 continuous rotation servo motors.
Wiring to analog pins: Trig to A2, Echo to A3, left motor to A5 and right motor to A4.
I don´t know how to do this modification to the code miself so please help me. Thanks!
So the first time it encounters and obstacle you want it to automatically turn right, then the next time it encounters an obstacle you want it to blindly turn left? Just alternate right and left?
One way would be to have a variable that goes from 0 to 1 in value and increments by one each time a turn decision is required. If it is 0 then it turns left, if it is 1 it runs right. If it exceeds 1 on the next increment, then it becomes 0 again and starts all over.
The use of a variable that goes from 0 to 1 sounds very good to me. However, I´m doing something wrong with the way I implement the algorithm.
This is the new code with the counter:
#include <Servo.h>
int i=0;
Servo motorLeft; // Define left motor
Servo motorRight; // Define right motor
#define trigPin A2
#define echoPin A3
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);
}
else
if (i=0);{
Serial.print(distance);
Serial.println( "cm");
motorLeft.write(170);
motorRight.write(10);
}
if (i=1);{
Serial.print(distance);
Serial.println( "cm");
motorLeft.write(10);
motorRight.write(170);
}
if (i++ == 2) i = 0;
}
The motors don´t run propertly but instead they kind of start and stop running continuously.
Thank you for the correction widbill. But I don´t know where to put the if statements either. I tried different ways to put them in but I haven´t been successful to make it work so far.
Please someone tell me what I´m doign wrong with the code.
I already changed if ( i=0) for if ( I==0) but still doesn´t work. The if statements should be in a different place or something, but I don´t know where.
biotech:
Please someone tell me what I´m doign wrong with the code.
I already changed if ( i=0) for if ( I==0) but still doesn´t work. The if statements should be in a different place or something, but I don´t know where.
Did you also remove the semicolon?
The name, i, is pretty dumb. That name tends to be used a lot for for loops, etc. Global variable names should almost never be one letter names.
A name like turnLeft, with boolean as the type, and true or false as the values, makes more sense:
bool turnLeft = true;
Then, in loop():
if(turnLeft)
{
// turn left
turnLeft = false;
}
else
{
// turn right
turnLeft = true;
}
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.
Get rid of the fixed time for the turn. Have it keep turning until the obstacle is cleared. The IF IT IS LESS AND 30 UNITS TURN part of the code will keep it turning until it is no longer less than 30 units from an obstacle. You will need to have the event that changes the variable from left to right choice, triggered by it clearing the obstacle and going straight again. As long as the obstacle is close, the main loop of the code will keep it turning. That's the way I have mine programmed and it works slick. Turns just enough to clear then goes straight.
I know that I have to take off the delays but if I do it with the code as it is the motors don´t work. That´s why I put them in even when they are not suitable for the task I'm trying to achieve.
Regarding the IF IT IS LESS AND 30 UNITS TURN Northof49 I don´t know what you mean. May I have an example please? Alongside that, even as I tried in many ways to change boolean from true to false when the event of clearing the obstacle is done I haven´t been successful so far.
Is it possible please for someone to give me a routine example? Maybe that way I can understand the concept finally and start experimenting with it.
I gathered that part of the decision making engine from the seventh line of your void loop. It has the robot going straight until something is less than 30 units in distance away.
I removed the two delays in the turn subroutines. Now the robot goes forward until it finds an obstacle. But when it finds an it instead of turning right or left continuously the wheels kind of jamm in the same place.
I think the problem is that I don´t know how to change from true to false so the routinge of turning left ( or right ) keeps going until the obstacule is sorted. I have tried variations to the if statement like do-while of for-loop. None had given any good results. That´s why an example would be great to clarify concepts.
Well I keep on trying. In this new variation of the code I introduce the boolean value change ' turnLeft = !turnLeft; ' when there is no obstacle detected. So when an obstacle is detected, the robot rotates in one direction or the other depending on that value.
At this point I think the code is ok to make the robot turn in alternating manner to one side and then when the next obstacle is detected to the other. But instead the robot turns ramdomly to one side or the other.
Variation of the code:
#include <Servo.h>
Servo motorLeft; // Define left motor
Servo motorRight; // Define right motor
#define trigPin A2
#define echoPin A3
boolean turnLeft = true;
void setup() {
Serial.begin (9600);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
motorLeft.attach(A5); // Set left motor to pin A5
motorRight.attach(A4); // Set right motor to pin A4
}
void loop() {
int duration, distance; //ultrasonic sensor routine
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);
turnLeft = !turnLeft;
}
if (distance <= 30 && turnLeft == true){
Serial.print(distance);
Serial.println( "cm");
motorLeft.write(170);
motorRight.write(10);
}
if (distance <= 30 && turnLeft == false){
Serial.print(distance);
Serial.println( "cm");
motorLeft.write(10);
motorRight.write(170);
}
}
Getting much closer but not there yet. Help please to get to understand what could still be failing.
That doesn´t work wildbill. If I move the ' turnLeft = !turnLeft; ' to the turn subroutines the wheels try to go forward and backward at the same time. I tried before that variation and now again. That´s the result.
Yup, that suggestion was rubbish. Let's try again.
You need to keep the turn left variable the same while the range is short & flip it only when the turn has brought you to a clear path again.
So, keep an OldDistance variable with the prior value of distance. When OldDistance <=30 and Distance > 30, you've just finished a turn. At that point, you can flip turnLeft.