Go Down

Topic: Someone please check this out and tell me what can I do to perfect it. (Read 791 times) previous topic - next topic

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! :smiley-red:


Code: [Select]
#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.

Graynomad

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

One problem might be this

Code: [Select]
myservo.write(75);

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

Code: [Select]
  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
Rob Gray aka the GRAYnomad www.robgray.com

sLoki

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 :)

AWOL

This code
Code: [Select]
  // 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.
"Pete, it's a fool looks for logic in the chambers of the human heart." Ulysses Everett McGill.
Do not send technical questions via personal messaging - they will be ignored.

Go Up