Someone please check this out and tell me what can I do to perfect it.

Projects Discussion and Showcase Robotics
1

Im trying to make an obstacle avoidance robot with 2dc motors, parallax sonar sensor, and a servo to rotate 0-180deg. I somewhat have it working but it needs some tweeking. Especially on the turning part. Please help heres my code! :blush:

#include <TimedAction.h>
#include <AFMotor.h>
#include <Servo.h>

// create a servo variable
Servo myservo;
//create 2 motor variable's
AF_DCMotor motor2(2);
AF_DCMotor motor3(3);
// create ping sensor variable
const int pingPin = 12;

int sensor;
// other variable's
int dist = 12;
int Left = 18;
int Right = 22;

void setup() { 
 //Start serial port at 9600 buad rate 
 Serial.begin(9600);
  // what pin the servo is attached to 
  myservo.attach(9);
  //set the initial speed of the motors
  motor2.setSpeed(200); 
  motor3.setSpeed(200);
  //  motor control  Just telling the motors to be at rest. Note commands are ALL CAPS!
  motor2.run(FORWARD);
  motor3.run(FORWARD);
}


void loop()
{
  // the first  servo position 
  myservo.write(75);
  delay(300);
  
  
  // establish variables for duration of the ping, 
  // and the distance result in inches and centimeters:
  long duration, cm;

  // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
  pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);

  // The same pin is used to read the signal from the PING))): a HIGH
  // pulse whose duration is the time (in microseconds) from the sending
  // of the ping to the reception of its echo off of an object.
  pinMode(pingPin, INPUT);
  duration = pulseIn(pingPin, HIGH);


  // convert the time into a distance
  cm = microsecondsToCentimeters(duration);
// place to distance of cm into variable sensor
   sensor=cm;
   //print distance of closest object in view
  Serial.print(cm);
  Serial.print("cm");
  Serial.println();

 // this next bit of code compares the sensor data to the predetermind dist set buy the user
 //to decide to go forward  or to stop

  if (sensor >= dist)
  { 
    motor2.run(FORWARD);
    motor3.run(FORWARD);
    delay(250);
   
  }
  if (sensor <= dist)
  {  motor2.run(RELEASE); 
    motor3.run(RELEASE);
     myservo.write(20);
     delay(300);
     
  myservo.write(160);
  // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
  pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);

  // The same pin is used to read the signal from the PING))): a HIGH
  // pulse whose duration is the time (in microseconds) from the sending
  // of the ping to the reception of its echo off of an object.
  pinMode(pingPin, INPUT);
  duration = pulseIn(pingPin, HIGH);


  // convert the time into a distance

  cm = microsecondsToCentimeters(duration);
    Right = cm;
      myservo.write(20);
      pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);

  // The same pin is used to read the signal from the PING))): a HIGH
  // pulse whose duration is the time (in microseconds) from the sending
  // of the ping to the reception of its echo off of an object.
  pinMode(pingPin, INPUT);
  duration = pulseIn(pingPin, HIGH);


  // convert the time into a distance

  cm = microsecondsToCentimeters(duration);
    Left = cm;
    
    myservo.write(180);
    delay(100);
    
    if (Right <= Left)
   {  motor2.setSpeed(170); 
  motor3.setSpeed(170);
     motor2.run(FORWARD); 
    motor3.run(BACKWARD);
    delay(1);}
 
     if (Right >= Left)     
    {      motor2.setSpeed(170); 
    motor3.setSpeed(170);
      motor2.run(BACKWARD); 
    motor3.run(FORWARD);
    delay(1);}
  }

else
  { motor2.run(FORWARD); 
    motor3.run(FORWARD);
    delay(10);
  }}

                    

long microsecondsToInches(long microseconds)
{
  // According to Parallax's datasheet for the PING))), there are
  // 73.746 microseconds per inch (i.e. sound travels at 1130 feet per
  // second).  This gives the distance travelled by the ping, outbound
  // and return, so we divide by 2 to get the distance of the obstacle.
  // See: http://www.parallax.com/dl/docs/prod/acc/28015-PING-v1.3.pdf
  return microseconds / 74 / 2;
}

long microsecondsToCentimeters(long microseconds)
{
  // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the
  // object we take half of the distance travelled.
  return microseconds / 29 / 2;
}

Moderator: Code box added.

2

Can you properly format the code by editing the post, highlighting the code and clicking the # button.

One problem might be this

myservo.write(75);

It's done at the start of every loop then further down the loop you have

  if (sensor <= dist)
  {  motor2.run(RELEASE); 
    motor3.run(RELEASE);
     myservo.write(20);
     delay(300);
     
  myservo.write(160);

I didn't analyze all the code paths but that first write to the servo seems a little strange. What's the point in constantly resetting its location to 75 then changing it again to 20 or 160?

Rob

3

I've built something similar. I did some testing and the US sensor will return range readings on any object within a 20 degree (from memory) "cone". It returns the distance to the closest object within the 20 degrees.

I've done a small amount of experimenting with focusing the range by putting cardboard around the sensor, it seemed to work okay but I need to do more testing.

If you want any help on specific issues PM and I'll help if I can :slight_smile:

4

This code

  // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
  pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);

  // The same pin is used to read the signal from the PING))): a HIGH
  // pulse whose duration is the time (in microseconds) from the sending
  // of the ping to the reception of its echo off of an object.
  pinMode(pingPin, INPUT);
  duration = pulseIn(pingPin, HIGH);

is repeated three times.
A simple function would shorten your code and make your sketch easier to maintain or expand.