Obstacle avoiding arduino robot uno for biginner

unni · March 22, 2015, 1:21pm

#include <Servo.h>
// --------------------------------------------------------------------------- Motors

int motor_left[] = {2, 3};

int motor_right[] = {4, 5};

const int trigPin = A4;

const int echoPin = A5;

int Servopos=0;

int Increment=-1;

Servo myservo;  // create servo object to control a servo
// --------------------------------------------------------------------------- Setup

void setup() {

Serial.begin(9600);

myservo.attach(9);  // attaches the servo on pin 9 to the servo object

// Setup motors

int i;

for(i = 0; i < 2; i++){

pinMode(motor_left[i], OUTPUT);

pinMode(motor_right[i], OUTPUT);

}

myservo.write(90);

}
// --------------------------------------------------------------------------- Loop

void loop() {

//------------------------------

long duration,  cm;
pinMode(trigPin, OUTPUT);

digitalWrite(trigPin, LOW);

delayMicroseconds(2);

digitalWrite(trigPin, HIGH);

delayMicroseconds(10);

digitalWrite(trigPin, LOW);
pinMode(echoPin, INPUT);

duration = pulseIn(echoPin, HIGH);
cm = microsecondsToCentimeters(duration);
Serial.print(cm);Serial.println();

if (Servopos <=10)

{Increment=1;}

if (Servopos  >=170)

{Increment=-1;}
Servopos<span style="color: #333333;">=</span>Servopos<span style="color: #333333;">+</span>Increment;

myservo.write(Servopos);
if (Servopos<=90)

{

if (cm<15)

{

turn_right();
<span style="color: #888888;">//  Serial.print("right");Serial.println();Serial.print(cm);Serial.println();</span>
}
<span style="color: #008800; font-weight: bold;">else</span>{
drive_forward();}

}
<span style="color: #008800; font-weight: bold;">if</span> (Servopos<span style="color: #333333;">&gt;</span><span style="color: #0000dd; font-weight: bold;">90</span>) 

{

if (cm<15)

{

turn_left();
 <span style="color: #888888;">// Serial.print("left");Serial.println();Serial.print(cm);Serial.println();</span>
  
}
<span style="color: #008800; font-weight: bold;">else</span>{
drive_forward();
}

}

}

// --------------------------------------------------------------------------- Drive
void motor_stop(){

digitalWrite(motor_left[0], LOW);

digitalWrite(motor_left[1], LOW);
digitalWrite(motor_right[0], LOW);

digitalWrite(motor_right[1], LOW);
}
void drive_forward(){

digitalWrite(motor_left[0], HIGH);

digitalWrite(motor_left[1], LOW);
digitalWrite(motor_right[0], HIGH);

digitalWrite(motor_right[1], LOW);

}
void drive_backward(){

digitalWrite(motor_left[0], LOW);

digitalWrite(motor_left[1], HIGH);
digitalWrite(motor_right[0], LOW);

digitalWrite(motor_right[1], HIGH);

}
void turn_left(){

digitalWrite(motor_left[0], LOW);

digitalWrite(motor_left[1], HIGH);
digitalWrite(motor_right[0], HIGH);

digitalWrite(motor_right[1], LOW);
}
void turn_right(){

digitalWrite(motor_left[0], HIGH);

digitalWrite(motor_left[1], LOW);
digitalWrite(motor_right[0], LOW);

digitalWrite(motor_right[1], HIGH);
}
long microsecondsToCentimeters(long microseconds)

{

return microseconds / 29 / 2;
}

dangerousthing · March 22, 2015, 1:45pm

And your question is…
what?

Is the code working on your robot? Does it compile?

If would be nice to see your code formmated nicely so it could be read and understood. If your code is already nicely formatted you can copy the code and paste it into a box at http://hilite.me and with the right settings your code will come out in HTML so you can paste it back into your original post.

Good luck!

birdmun · March 23, 2015, 5:38am

A scanning ultrasonic sensor would be cool.

I will admit … nothing. Put your ping code in a function that also moves a servo. Every time the scan function is called increment/decrement the value written to the servo. The sensor can return a value from the function. I believe the value for the servo position must be made static to persist between calls.

After thinking about it some more, the code to move the servo should be its own function/procedure. Not because it must be, but, because it should be. You shouldn’t force yourself to rewrite code over and over again for each new project.

dangerousthing · March 23, 2015, 7:23am

One suggestion: find the
One suggestion: find the range of the servo first. Some have made the mistake of assuming that a servo will go from -90 to 90 degrees. Many servos have a much shorter span, depending on their intended use.

unni · March 23, 2015, 9:48am

Dngerousthig This code is

Dngerousthig This code is working

nemesis i added some some comments

Birdmun let me try ishall ask u if I have doubts

jscottb · March 23, 2015, 2:21pm

.

dangerousthing · March 23, 2015, 2:30pm

Nice code
Jscottb,

This is how nicely formatted Arduino code should look.

I’m about to put the electronics together for my Runt Rover, and would like to run this code, if it’s alright. Eventually I would like to write my own code, but this way I can get it running faster. Strange, I’m a programmer asking for code.

I will say that you’ve done the reading of the sonars very elegantly.

I wish that the OP could have formatted his code as nicely, because then I might be able to read it.

jscottb · March 23, 2015, 2:58pm

Sure!

Thanks! I will repost the code again tonight. I wanted to put a few more comments in before I let it loose, but if you have it already, then no problem. I like code you can read as well!

dannyv · March 23, 2015, 5:33pm

just like to know…

How do you post code that way? Looks really neat and readable this way.

jscottb · March 23, 2015, 6:43pm

See…

https://www.robotshop.com/letsmakerobots/code-html-syntax-hi-liter

jscottb · March 23, 2015, 7:00pm

Try this code

Here is the code re-posted.

This is code I am working on for a new SHR I will be posting soon. It’s not fully tested and the pin assignments may need changing to use. The above bot code did not use PWM, so I tell my library to do the same. Install the library like the code says to use it. You can change the reaction distance and turn delays to suit your needs.

I fashioned the motor libraries after the original Adafruit motor shield V1 library. They are just light wrappers around the wiring code, and do not play with the timers. I have a few different motor libraries like this so I could switch around driver boards and shields with little to few code changes. I will post the final code when I release my SHR.

/*

   Basic Arduino obstacle avoider.
   Goal in life…

      Moves forward looking for obstacles to avoid 
   Use to get started for a SHR (Start here robot) (https://www.robotshop.com/letsmakerobots/)
   Written by Scott Beasley - 2015

   Free to use or modify. Enjoy.

*/
/*

   Uses the L29NDriver library. It works for the L293 and L298 series chips.
   Download from https://github.com/jscottb/L29NDriver or clone the zip from

   https://github.com/jscottb/L29NDriver/archive/master.zip

   Unzip and copy to your Arduino library folder or follow the instructions

   here: http://arduino.cc/en/guide/libraries

*/
#include <l29NDriver.h>

#include <Servo.h>
// Defines for the distance reading function of the bot.

#define turn() (left_dist >= right_dist) ? go_left () : go_right ()

#define microsecondsToCentimeters(microseconds) (unsigned long)microseconds / 29.1 / 2.0
#define ECHO_PIN A5  // Digital pin A5

#define TRIGGER_PIN A4 // Digital pin A4

#define MIN_ACTION_DIST 5 // 5 cm
// Defines for I/O lines used for motor driver pins

#define pinI1 2 //define I1 interface

#define pinI2 3 //define I2 interface

#define pinI3 4 //define I3 interface

#define pinI4 5 //define I4 interface

#define MAXFORWARDSPEED 200 // Max speed we want moving forward

#define MAXBACKWARDSPEED 125 // Max reverse speed
/

   Globals area. Try to keep to a minimum 

/
// Create the motor and servo objects with use to interface with

L29N_Motor motor_left (pinI1, pinI2, NOPWM); // Pins to drive the Left motor

L29N_Motor motor_right (pinI3, pinI4, NOPWM); // Pins to drive the Right motor

Servo sensor_servo; // Create a servo object for our distance sensor
byte sweep_pos = 0; // Current position of the sensor servo

byte pos_index = 90;

unsigned long left_dist, right_dist; // Distance measured left and right
void setup ( )

{

Serial.begin (9600); // Set Serial monitor at 9600 baud

Serial.println (“My SHR bot is starting up!”);
// Make sure the motors are off at start

halt ( );
sensor_servo.attach (6); // Attach the servo to digital pin 6

sensor_servo.write (90); // Set the servo to the middle (neutral) pos
// Set modes for distance sensor pins

pinMode (ECHO_PIN, INPUT);  // Set echo pin as input

pinMode (TRIGGER_PIN, OUTPUT); // Set trigger pin as output

}
void loop ( )

{

unsigned long dist_fwd;
// Rotate the distance sensor as we drive along

rotate_sensor ( );
// Give the servo time to get to position and get setted

delay (100);
// Get a reading from the current sensor direction

dist_fwd = ping ( );

Serial.print ("Distance sensor reading: ");

Serial.println (dist_fwd);
// Go forward while nothing is in the distance sensors read area

if (dist_fwd > MIN_ACTION_DIST || dist_fwd == 0)

{

go_forward ( );

}

else // There is something in the sensors read area

{

halt ( ); // Stop!

go_backward ( ); // Back up a bit

delay (400);

halt ( ); // Stop!
  sensor_read <span style="color: #808030;">(</span> <span style="color: #808030;">)</span><span style="color: #800080;">;</span> <span style="color: #696969;">// Read distance left and right</span>

  turn <span style="color: #808030;">(</span> <span style="color: #808030;">)</span><span style="color: #800080;">;</span> <span style="color: #696969;">// Turn toward the clearest path</span>
  delay <span style="color: #808030;">(</span><span style="color: #008c00;">600</span><span style="color: #808030;">)</span><span style="color: #800080;">;</span>
  halt <span style="color: #808030;">(</span> <span style="color: #808030;">)</span><span style="color: #800080;">;</span>

}

}
// Read the sensor after we find something in the way. This helps find a new

// path

void sensor_read ( )

{

Serial.println (“Server at 40 deg…”);

sensor_servo.write (40);

delay (300);

right_dist = ping ( ); //Look to the right
Serial.println (“Servo at 140 deg…”);

sensor_servo.write (140);

delay (300);

left_dist = ping ( ); // Look to the left
Serial.println (“Servo at 90 deg…”);

sensor_servo.write (90);

}
// Rotate the sensor servo at 45deg increments

void rotate_sensor ( )

{

if (sweep_pos <= 0) {

pos_index = 45;

}

else if (sweep_pos >= 180) {

pos_index = -45;

}
Serial.print ("pos_index = ");

Serial.println (pos_index);

sweep_pos += pos_index;

Serial.print ("sweep_pos = ");

Serial.println (sweep_pos);

sensor_servo.write (sweep_pos);

}
// Read the HC-SR04 uSonic sensor

unsigned long ping ( )

{

// Trigger the uSonic sensor (HC-SR04) to send out a ping

digitalWrite (TRIGGER_PIN, LOW);

delayMicroseconds (5);

digitalWrite (TRIGGER_PIN, HIGH);

delayMicroseconds (10);

digitalWrite (TRIGGER_PIN, LOW);
// Measure how long the ping took and convert to cm’s

return (microsecondsToCentimeters (pulseIn (ECHO_PIN, HIGH)));

}
void go_forward ( )

{

Serial.println (“Going forward…”);

motor_left.setSpeed (MAXFORWARDSPEED);

motor_right.setSpeed (MAXFORWARDSPEED);

motor_left.run (FORWARD|RELEASE);

motor_right.run (BACKWARD|RELEASE);

}
void go_backward ( )

{

Serial.println (“Going backward…”);

motor_left.setSpeed (MAXBACKWARDSPEED);

motor_right.setSpeed (MAXBACKWARDSPEED);

motor_left.run (BACKWARD|RELEASE);

motor_right.run (FORWARD|RELEASE);

}
void go_left ( )

{

Serial.println (“Going left…”);

motor_left.setSpeed (MAXFORWARDSPEED);

motor_right.setSpeed (MAXFORWARDSPEED);

motor_left.run (BACKWARD|RELEASE);

motor_right.run (BACKWARD|RELEASE);

}
void go_right ( )

{

Serial.println (“Going right…”);

motor_left.setSpeed (MAXFORWARDSPEED);

motor_right.setSpeed (MAXFORWARDSPEED);

motor_left.run (FORWARD|RELEASE);

motor_right.run (FORWARD|RELEASE);

}
void halt ( )

{

Serial.println (“Halt!”);

motor_left.setSpeed (0);

motor_right.setSpeed (0);

motor_left.run (BRAKE);

motor_right.run (BRAKE);

}

unni · March 24, 2015, 2:35am

Dannyv @ I used hilite.me to

Dannyv @ I used hilite.me to format the code

unni · March 24, 2015, 2:41am

**Re jscottb **

jscottab ,I have n’t motor shield . I am using L293 D only

Is this algoritham is right

Go forward and Rotate servo

read servo pos

If obstecle at left side then turn right

If obstecle at right side then turn left

if at front then go back turn left or right

dangerousthing · March 24, 2015, 4:03am

Code Formatting

hilite.me doesn’t actually format code, it puts the HTML markers around it so it can be properly pasted into LMR, since the code tages don’t work here.

You still need to indent your code properly and to put the braces (curly brackets) where they belong. I suggest looking at some Arduino code like jscottb's code above, or the sample code at the Arduino.cc site.

This is for your benefit. It is much easier to debug code that it properly formatted. In addition, those of us who want to help you will have a much easier time. Most of us help others in our spare time, so the easier you make it for us to help you, the more help you'll get.

For example, a sample setup call looks like this when properly formatted.

1

2
3
4

void setup()

{

// do something

}

dangerousthing · March 24, 2015, 4:25am

Obstacle “Avoider” Algorithm
On one of my first bot’s, done years ago, I made sort of an obstacle avoider. The only sensors it had were switches so it had to run into the object first. The algorithm was (and yes, this is very simple, I was using Lego Mindstorms):

Go forward at highest speed
If switched hit, then got to 3, otherwise continue 1
Back up for 1 second at full speed
Choose randomly clockwise or counter-clockwise
Choose a random number between 90 and 720.
Turn in place at maximum speed in the direction choosen for the degrees choosen
Go to 1

This only works if your robot can’t cause damage to your house, your pets, your SO, etc.

It ran like a rabid chipmonk on speed. It was hilarious, but I’m glad I didn’t break the robot. After that I built more serious bots, but that is a basic avoider algorithm.