loading

robot bumper code for arduino?

I have a two wheeled robot with three bumpers and 5 ir sensors. I have completed my code to run the motors. I'm new to this it took four weeks to get where I am. I don't know how to add the avoidence code to make the robot stop and turn when faced with an object. Can anyone help me add this feature?


The robot is a DFrobot HCR
The microcontoller is the Arduino Romeo All in One 328
My code is working but maybe not so good. I am open to suggestions.

Here is my code: 

int E1 = 5;
int E2 = 6;
int M1 = 4;
int M2 = 7;

void forward(char a,char b) //Move forward
{
analogWrite(E1,a);
digitalWrite(M1,HIGH);
analogWrite(E2,b);
digitalWrite(M2,HIGH);
}

void stop(void) //Stop
{
digitalWrite(E1,LOW);
digitalWrite(E2,LOW);
}

void backward (char a,char b) //Move backward
{
analogWrite (E1,a);
digitalWrite(M1,LOW);
analogWrite (E2,b);
digitalWrite(M2,LOW);
}


void Left (char a,char b) //Turn left
{
analogWrite (E1,a);
digitalWrite(M1,LOW);
analogWrite (E2,b);
digitalWrite(M2,HIGH);
}
void Right (char a,char b) //Turn right
{
analogWrite (E1,a);
digitalWrite(M1,HIGH);
analogWrite (E2,b);
digitalWrite(M2,LOW);
}

void setup()

{
for(int i=4;i<=7;i++)
pinMode(i, OUTPUT);
}

void loop()

{

forward(100,100); //Forward at 50% full speed
delay(10000); //delay for 10 seconds
stop(); //Stop
delay(6000000); //delay for 1 hour
backward(100,100); //backward at 50% full speed
delay(10000); //delay for 10 seconds
stop(); //Stop
delay(6000000); //delay for 1 hour
Left(100,100); //Left at 50% full speed
delay(10000); //delay for 10 seconds
stop(); //Stop
delay(6000000); //delay for 1 hour
Right(100,100); //Left at 50% full speed
delay(10000); //delay for 10 seconds
stop(); //Stop
delay(6000000); //delay for 1 hour

}

sort by: active | newest | oldest
astroboy9076 years ago
What exactly is wrong with it? So far the only problem I can find is the delay(6000000); function. I believe that delay uses and int, which can only hold values up to 32,767, before rolling over and starting over at the negative maximum value (-32,7680) and doing that until it has "counted" to 6,000,000. I suggest creating another variable for the time like so:

unsigned long hourDelay = 6000000;

and for your hour delay change it from delay(6000000); to

delay(hourDelay);

and it should work better. Also you can use just a long instead of an unsigned long, and unsigned long just means it cant hold negative numbers. One more question, what are your variables changing? (E1, E2, M1, M2) Motors? Sensors? Nice code though so far btw, its hard to get functions working right like it looks like youve done :)
Moses58 (author)  astroboy9076 years ago
Hi here are the spec. and photos

Thank you so much for your help

Aluminum power-coated orange
Two 17w DC motors with 64:1 Gearbox 120 rpm
Two (2 Phases) encoders with 12 PPR
2 wheels (and one rotating third wheel)
Associated plates and hardware
Three bumper sensors
Five Sharp GP2D12 infrared sensors
Romeo Controller board (Arduino compatible)
Weight: 5.2kg
Dimensions: 34.2cmx31.3cmx26.6cm

I tried to ad photos but don't seem to get them uploaded.
I could send them in a email.
ah its ok, I found some example code for your IR sensors online. Will look at it and try to implement it in code sometime tomorrow. I hope it doesnt need to be done today though.. For images try uploading again maybe and drag and drop them to the box under "Attach images" - sometimes instructables whacks out when adding photos. If you cant get them uploaded, no worry, I think i can get enough by your part #'s. I did see that you got your profile pic to an image of it tho.. nice :)

P.S are your bumper sensors just mostly a switch, or do they have some fancy sensor on them?

It really helps that you got the motors working... I would have no idea how to connect to them if you didnt do that :)
Ok here's some code, found it here (off of this 'ible), so all credit goes to the owner. If possible disconnect all the motors and sensors from your board, and then connect the power line of one sensor to +5v, the ground to one of the ground pins on your Romeo, and the output pin to pin Analog 0; then run this code:

const int IR_pin=A0;//analog

int IR_STATE;
int val;
void setup() {
Serial.begin(9600); // set up Serial library at 9600 bps
Serial.println("IR test!");
IR_STATE=A0;
pinMode(A0,INPUT);
}

void loop() {
val = analogRead(IR_pin); // read the value from the sensor
Serial.println(val);
delay(100);
}




leave the board connected to USB, as we will need the serial outputs its going to give. In Arduino, click serial output (next to upload). It should be displaying a list of values anywhere between 1 and 1024 (or possibly 255, i cant remember which). This represents the distance between the sensor and and object. Try using your hand (or move the robot in front of an wall) in frost of the sensor, and when you reach the distance you want the robot to be before it would turn, leave it there, and look at the value in the serial output. The number should refreshing in the serial monitor, but it should be a constant value now (or changing very little). Write the number down, and put it in the comment below. Hope the code works, good luck!
Moses58 (author)  astroboy9076 years ago
Thank you for your help. I spent most of the day studying code. I can now report i have tested all five ir sensors with the serial monitor. And all three bumper switchs with the digital led 13 on the Romeo All in One. So I'm getting close to figuring this out. I have spent the last three hours trying to get one ir sensor or one digital bumper to start and stop one motor. So far no success but i will keep trying. I am a little concerned that i will burn up my Arduino if I make a mistake. This is very hard for me since I am so new at this.
try this code, it might work, might not. Im not sure what A and B are supposed to equal, but there are variables for them. In mine i have the sensors from left to right, with A0 being the leftmost sensor and the A4 is the rightmost sensor.

int a;
int b;
int E1 = 5;
int E2 = 6;
int M1 = 4;
int M2 = 7;
int S1 = A0;
int S2 = A1;
int S3 = A2;
int S4 = A3;
int S5 = A4;

int forward() //Move forward
{
analogWrite(E1,a);
digitalWrite(M1,HIGH);
analogWrite(E2,b);
digitalWrite(M2,HIGH);
}

int halt() //Stop (I capitalized as there is a function called stop)
{
digitalWrite(E1,LOW);
digitalWrite(E2,LOW);
}

int backward() //Move backward
{
analogWrite (E1,a);
digitalWrite(M1,LOW);
analogWrite (E2,b);
digitalWrite(M2,LOW);
}


int Left() //Turn left
{
analogWrite (E1,a);
digitalWrite(M1,LOW);
analogWrite (E2,b);
digitalWrite(M2,HIGH);
}
int Right() //Turn right
{
analogWrite (E1,a);
digitalWrite(M1,HIGH);
analogWrite (E2,b);
digitalWrite(M2,LOW);
}

void setup()

{
for(int i=4;i<=7;i++)

pinMode(i, OUTPUT);
pinMode(A0, INPUT);
pinMode(A1, INPUT);
pinMode(A2, INPUT);
pinMode(A3, INPUT);
pinMode(A4, INPUT);
}


void loop(){
if(analogRead(A0) <= 340){
int STOP = halt();
int right = Right();
}
if(analogRead(A1) <= 340){
int STOP = halt();
int right = Right();
}
if(analogRead(A2) <= 340){
int STOP = halt();
int left = Left();
}
if(analogRead(A3) <= 340){
int STOP = halt();
int left = Left();
}
if(analogRead(A4) <= 340){
int STOP = halt();
int left = Left();
}
}
Moses58 (author)  astroboy9076 years ago
Hi, thank you for the sketch. I'm not sure why but there is no responce. It compiles and uploads ok.

try this. i forgot to add forward (oops)!

int a;
int b;
int E1 = 5;
int E2 = 6;
int M1 = 4;
int M2 = 7;
int S1 = A0;
int S2 = A1;
int S3 = A2;
int S4 = A3;
int S5 = A4;

int forward() //Move forward
{
analogWrite(E1,a);
digitalWrite(M1,HIGH);
analogWrite(E2,b);
digitalWrite(M2,HIGH);
}

int halt() //Stop (I capitalized as there is a function called stop)
{
digitalWrite(E1,LOW);
digitalWrite(E2,LOW);
}

int backward() //Move backward
{
analogWrite (E1,a);
digitalWrite(M1,LOW);
analogWrite (E2,b);
digitalWrite(M2,LOW);
}


int Left() //Turn left
{
analogWrite (E1,a);
digitalWrite(M1,LOW);
analogWrite (E2,b);
digitalWrite(M2,HIGH);
}
int Right() //Turn right
{
analogWrite (E1,a);
digitalWrite(M1,HIGH);
analogWrite (E2,b);
digitalWrite(M2,LOW);
}

void setup()

{
for(int i=4;i<=7;i++)

pinMode(i, OUTPUT);
pinMode(A0, INPUT);
pinMode(A1, INPUT);
pinMode(A2, INPUT);
pinMode(A3, INPUT);
pinMode(A4, INPUT);
}


void loop(){
int Forward = forward();
if(analogRead(A0) <= 340){
int STOP = halt();
int right = Right();
}
if(analogRead(A1) <= 340){
int STOP = halt();
int right = Right();
}
if(analogRead(A2) <= 340){
int STOP = halt();
int left = Left();
}
if(analogRead(A3) <= 340){
int STOP = halt();
int left = Left();
}
if(analogRead(A4) <= 340){
int STOP = halt();
int left = Left();
}
}

Moses58 (author)  astroboy9076 years ago
Hi I'm sorry to say there is no responce. It compiles and up-loads fine. But the motors do not move. I know everything is connected right because the origibal code works fine. It is only when the sensors are added that it stops working.
I wish I could understand this code better. I have spent another day trying many different ways to make it work with no luck.

I know there are many robots wirh sensors that work. They are on Youtube. The code seems to be the biggest problem. Why doesn't Arduino offer an easier way to write the code. The humaniods have programs to get them going right away, why not platforms.

Well sorry I'm rambling due to my frustration.
Ok- will check around through the code and revamp it. Idk why they dont offer an easy way to write the code. Some people actually wrote a program i think that makes it more drag and drop though... Idk where
Its ok, ive done the same thing. Multiple times actually.

int E1 = 5;
int E2 = 6;
int M1 = 4;
int M2 = 7;

void forward(char a,char b) //Move forward
{
analogWrite(E1,a);
digitalWrite(M1,HIGH);
analogWrite(E2,b);
digitalWrite(M2,HIGH);
}

void stop(void) //Stop
{
digitalWrite(E1,LOW);
digitalWrite(E2,LOW);
}

void backward (char a,char b) //Move backward
{
analogWrite (E1,a);
digitalWrite(M1,LOW);
analogWrite (E2,b);
digitalWrite(M2,LOW);
}


void Left (char a,char b) //Turn left
{
analogWrite (E1,a);
digitalWrite(M1,LOW);
analogWrite (E2,b);
digitalWrite(M2,HIGH);
}
void Right (char a,char b) //Turn right
{
analogWrite (E1,a);
digitalWrite(M1,HIGH);
analogWrite (E2,b);
digitalWrite(M2,LOW);
}

void setup(){
for(int i=4;i<=7;i++)

pinMode(i, OUTPUT);
pinMode(A0, INPUT);
pinMode(A1, INPUT);
pinMode(A2, INPUT);
pinMode(A3, INPUT);
pinMode(A4, INPUT);
}


void loop(){
forward(100, 100);
if(analogRead(A0) <= 340){
stop();
Right(100, 100);
}
if(analogRead(A1) <= 340){
stop();
Right(100, 100);
}
if(analogRead(A2) <= 340){
stop();
Left(100, 100);
}
if(analogRead(A3) <= 340){
stop();
Left(100, 100);
}
if(analogRead(A4) <= 340){
stop();
Left(100, 100);
}
}


try that :)
Moses58 (author)  astroboy9076 years ago
Hi I unpluged two sensors but that did not change anything.

Is there supposed to be some code at the begining for the sensors?

An interesting note when my hand is close to a sensor the motor works fine. Seems like a reverse effect.

Thanks
the code should still work if things end up unplugged- though not all the way because then i think it would return 0. If its a reverse effect, try turning all the <= s to >= s . That might be the problem :) Good luck!

Btw in the code it might just be easier to use find (Edit>Find) and use find <= and replace with >=
Moses58 (author)  astroboy9076 years ago
Hi, It works the delays made a hugh difference. I am going to try it out on the floor when my helper comes back. I am very weak due to a rare neurological condition. I can only lift a few pounds. So i need someone to put it on the floor.

I have been testing some code for the three bumpers. With what you have taught me i should be able to incorparate it into my code.

Thank you so much. I could not have done this myself.
Moses58 (author)  astroboy9076 years ago
Hi, I have been trying different things until the Romeo was no longer recognized This has happened twice in the past. I had to uninstall and re-install Arduino again. The software is working fine now.

Thank you for the sketch.
Good news it does work with all five sensors.

Bad news the motors make a load whine with slow rotation. If I put it on the floor there is no power to move it. I tried a few modifications but no change. I wonder if the ir sensors are so close that they over ride each other?

Any thoughts?

The originsl code works fine still. Forward, reverse, left & right.
Seems like the board is a little warm under the analog conectors too.


Thanks
Hmm.. idk why. What do they do with the orig. code? No whine? It might hae overloaded. Heres code with delays


int E1 = 5;
int E2 = 6;
int M1 = 4;
int M2 = 7;

void forward(char a,char b) //Move forward
{
analogWrite(E1,a);
digitalWrite(M1,HIGH);
analogWrite(E2,b);
digitalWrite(M2,HIGH);
}

void stop(void) //Stop
{
digitalWrite(E1,LOW);
digitalWrite(E2,LOW);
}

void backward (char a,char b) //Move backward
{
analogWrite (E1,a);
digitalWrite(M1,LOW);
analogWrite (E2,b);
digitalWrite(M2,LOW);
}


void Left (char a,char b) //Turn left
{
analogWrite (E1,a);
digitalWrite(M1,LOW);
analogWrite (E2,b);
digitalWrite(M2,HIGH);
}
void Right (char a,char b) //Turn right
{
analogWrite (E1,a);
digitalWrite(M1,HIGH);
analogWrite (E2,b);
digitalWrite(M2,LOW);
}

void setup(){
for(int i=4;i<=7;i++)

pinMode(i, OUTPUT);
pinMode(A0, INPUT);
pinMode(A1, INPUT);
pinMode(A2, INPUT);
pinMode(A3, INPUT);
pinMode(A4, INPUT);
}


void loop(){
forward(100, 100);
if(analogRead(A0) <= 340){
stop();
Right(100, 100);
delay(1000);
stop();
}
if(analogRead(A1) <= 340){
stop();
Right(100, 100);
delay(1000);
stop();
}
if(analogRead(A2) <= 340){
stop();
Left(100, 100);
delay(1000);
stop();
}
if(analogRead(A3) <= 340){
stop();
Left(100, 100);
delay(1000);
stop();
}
if(analogRead(A4) <= 340){
stop();
Left(100, 100);
delay(1000);
stop();
}
}

might keep it from overloading. Will detect an object, and go left/right for 1sec, then run through the loop again.
Moses58 (author)  astroboy9076 years ago
Hi, I'm in no hurry.
The bumpers are simple digital switchs.
I tried to add images but it erased my first reply.
I found one person who has made the same robot.
He is from another country and is to busy to help.
Last week he sent his sketch.
It makes my robot jittery and if a bumper is pressed it stops completely.
I could send it to you if you feel it could be worked with.
It is very compllcated I don;t understand what is happening.
He did not use all the ir sensors. Only 3

Thanks
Moses58 (author)  astroboy9076 years ago
Thank you both for advice and corrections.
I am not sure where to put the changes.

void loop() {
checkSensors();
driveMotors();
}

I think I have to tell it what pins are used.
The bumpers are digital & the sensors are analog.

&

unsigned long hourDelay = 6000000;
delay(hourDelay);


I'm very new at this and after weeks of trying to make the motors run I'm afraid I will mess it up.
int E1 = 5;
int E2 = 6;
int M1 = 4;
int M2 = 7;
unsigned long hourDelay = 3600000;

void forward(char a,char b) //Move forward
{
analogWrite(E1,a);
digitalWrite(M1,HIGH);
analogWrite(E2,b);
digitalWrite(M2,HIGH);
}

void stop(void) //Stop
{
digitalWrite(E1,LOW);
digitalWrite(E2,LOW);
}

void backward (char a,char b) //Move backward
{
analogWrite (E1,a);
digitalWrite(M1,LOW);
analogWrite (E2,b);
digitalWrite(M2,LOW);
}


void Left (char a,char b) //Turn left
{
analogWrite (E1,a);
digitalWrite(M1,LOW);
analogWrite (E2,b);
digitalWrite(M2,HIGH);
}
void Right (char a,char b) //Turn right
{
analogWrite (E1,a);
digitalWrite(M1,HIGH);
analogWrite (E2,b);
digitalWrite(M2,LOW);
}

void setup()

{
for(int i=4;i<=7;i++)
pinMode(i, OUTPUT);
}

void loop()

{

forward(100,100); //Forward at 50% full speed
delay(10000); //delay for 10 seconds
stop(); //Stop
delay(hourDelay); //delay for 1 hour
backward(100,100); //backward at 50% full speed
delay(10000); //delay for 10 seconds
stop(); //Stop
delay(hourDelay); //delay for 1 hour
Left(100,100); //Left at 50% full speed
delay(10000); //delay for 10 seconds
stop(); //Stop
delay(hourDelay); //delay for 1 hour
Right(100,100); //Left at 50% full speed
delay(10000); //delay for 10 seconds
stop(); //Stop
delay(hourDelay); //delay for 1 hour

}


 If possible, could you add some pics of your IR sensors? It would help with the code. I'd be glad to help write it. The IR sensors could be something like the types here (random googled page), or they could be an IR emitter/receiver combo like this  (another random googled page). At the very least the number of pins is needed. Also, user joshnuss below posted a method where it would read your sensors, and choose the appropriate movement constantly, rather than every 10 secs or hour
kind of a tangent, note that 6,000,000 milliseconds does not equal 1 hour. An hour is 60*60*1000=3600000 milliseconds.

a simple way to do avoidance testing and correction is by usingpolling.  In the loop() function constantly check your sensors and then adjust you direction (turn specific motors on/off)  based on the sensory input

void loop() {
   checkSensors();
   driveMotors();
}

a more advanced way would be to use interrupts
Moses58 (author) 6 years ago
Well I have tried all day to come up with a working code for the 5 Sharp GP2D12 infrared sensors. I used analog pin0 for the first input this will give me the high and low values from the sensor on the serial port as well. I tested all 5 sensors on pins 0 through 4 on my Romeo All in One controller from DFrobots. I decided to focus on the analog ir sensors first rather then try to do it all with the digital bumpers too.

The code that I have here may not be what I need to make the robot change direction when an object is in the way. One thing I’m sure of is this code needs more instructions. Can you help me get this going?

byte sensorPin=0;
int latestVal=0;
int maxVal=0;
int minVal=1024;

void setup() {
Serial.begin(9600);
Serial.println("in setup");
}

void loop() {
//Serial.println("In loop");
latestVal = analogRead(sensorPin);
if(latestVal < minVal) {
minVal = latestVal;
}
if(latestVal > maxVal) {
maxVal = latestVal;
}
Serial.print("Max: ");
Serial.print(maxVal);
Serial.print(" Min: ");
Serial.print(minVal);
Serial.print(" Latest: ");
Serial.println(latestVal);
delay(1000);
}

My working motor code with the suggestions you made:

int E1 = 5;
int E2 = 6;
int M1 = 4;
int M2 = 7;
unsigned long hourDelay = 3600000;

void forward(char a,char b) //Move forward
{
analogWrite(E1,a);
digitalWrite(M1,HIGH);
analogWrite(E2,b);
digitalWrite(M2,HIGH);
}

void stop(void) //Stop
{
digitalWrite(E1,LOW);
digitalWrite(E2,LOW);
}

void backward (char a,char b) //Move backward
{
analogWrite (E1,a);
digitalWrite(M1,LOW);
analogWrite (E2,b);
digitalWrite(M2,LOW);
}


void Left (char a,char b) //Turn left
{
analogWrite (E1,a);
digitalWrite(M1,LOW);
analogWrite (E2,b);
digitalWrite(M2,HIGH);
}
void Right (char a,char b) //Turn right
{
analogWrite (E1,a);
digitalWrite(M1,HIGH);
analogWrite (E2,b);
digitalWrite(M2,LOW);
}

void setup()

{
for(int i=4;i<=7;i++)
pinMode(i, OUTPUT);
}

void loop()

{

forward(100,100); //Forward at 50% full speed
delay(10000); //delay for 10 seconds
stop(); //Stop
delay(hourDelay); //delay for 1 hour
backward(100,100); //backward at 50% full speed
delay(10000); //delay for 10 seconds
stop(); //Stop
delay(hourDelay); //delay for 1 hour
Left(100,100); //Left at 50% full speed
delay(10000); //delay for 10 seconds
stop(); //Stop
delay(hourDelay); //delay for 1 hour
Right(100,100); //Left at 50% full speed
delay(10000); //delay for 10 seconds
stop(); //Stop
delay(hourDelay); //delay for 1 hour

}
Oh its ok Ill send over one just as soon as im done with this. I actually searched your board and i *think* have a good idea of the pins. Pretty much ill need to know any of the special pin numbers. Like on the arduino the analog pins are like A1 and A1. Sending PM
Moses58 (author)  astroboy9076 years ago
If you magnify the picture I just uploaded all the pins are numbered. I use this picture all the time because the Romeo all in one has so many pins it is easy to make a mistake. The pictures are in my library

Thanks
Im gonna try to add more to the code, but I need to know what distance you want objects to be before the robot turns. Like instead of it turning when the object is 80cm away, have it turn when its 20cm away. To do this I just need the value from the serial test of the IR sensor, when the robot is placed as far away from the wall as you want it to turn. So like place the robot anywhere between 10 and 30 cm from the wall and give me any one of the serial values from there. Apart from that theres not much more I need to build your code. As for your comment below, theres not much you can do to fry the chip itself. I once had like 14 pins of my arduino shorting to ground (not good) and it survived. Yeah, it would have eventually killed it, but its easy to catch most problems. A big one would be if the atmega chip on your board heats up exponentially. I might not worry so much about your motor controllers as they have to output quite a lot more than your atmega, so its ok if they get warm. Also do you want to PM me so the question doesnt get longer and longer? I'm fine either way.

Good Luck!-Astroboy907
Moses58 (author)  astroboy9076 years ago
Hi and thanks.

Sorry I have not found the PM feature my email is billvogt@juno.com

I decided on 15cm as a good distance to stop and turn. That gives me a value of about 340 on the serial monitor.

I tested analogpin 5

There are five ir sensors pins 1 through 5

I also posted several pictures of my robot. I will thy to add a picture of the board diagram with pin numbers.

My working code.

int E1 = 5;
int E2 = 6;
int M1 = 4;
int M2 = 7;
unsigned long hourDelay = 3600000;

void forward(char a,char b) //Move forward
{
analogWrite(E1,a);
digitalWrite(M1,HIGH);
analogWrite(E2,b);
digitalWrite(M2,HIGH);
}

void stop(void) //Stop
{
digitalWrite(E1,LOW);
digitalWrite(E2,LOW);
}

void backward (char a,char b) //Move backward
{
analogWrite (E1,a);
digitalWrite(M1,LOW);
analogWrite (E2,b);
digitalWrite(M2,LOW);
}


void Left (char a,char b) //Turn left
{
analogWrite (E1,a);
digitalWrite(M1,LOW);
analogWrite (E2,b);
digitalWrite(M2,HIGH);
}
void Right (char a,char b) //Turn right
{
analogWrite (E1,a);
digitalWrite(M1,HIGH);
analogWrite (E2,b);
digitalWrite(M2,LOW);
}

void setup()

{
for(int i=4;i<=7;i++)
pinMode(i, OUTPUT);
}

void loop()

{

forward(100,100); //Forward at 50% full speed
delay(10000); //delay for 10 seconds
stop(); //Stop
delay(hourDelay); //delay for 1 hour
backward(100,100); //backward at 50% full speed
delay(10000); //delay for 10 seconds
stop(); //Stop
delay(hourDelay); //delay for 1 hour
Left(100,100); //Left at 50% full speed
delay(10000); //delay for 10 seconds
stop(); //Stop
delay(hourDelay); //delay for 1 hour
Right(100,100); //Left at 50% full speed
delay(10000); //delay for 10 seconds
stop(); //Stop
delay(hourDelay); //delay for 1 hour
}