Go Down

Topic: Someone please check this out and tell me what can I do to perfect it. (Read 820 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
 


Please enter a valid email to subscribe

Confirm your email address

We need to confirm your email address.
To complete the subscription, please click the link in the email we just sent you.

Thank you for subscribing!

Arduino
via Egeo 16
Torino, 10131
Italy