Pages: [1]   Go Down
Author Topic: Someone please check this out and tell me what can I do to perfect it.  (Read 732 times)
0 Members and 1 Guest are viewing this topic.
Offline Offline
Newbie
*
Karma: 0
Posts: 2
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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:
#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.
« Last Edit: November 11, 2011, 02:31:06 am by AWOL » Logged

nr Bundaberg, Australia
Offline Offline
Tesla Member
***
Karma: 129
Posts: 8594
Scattered showers my arse -- Noah, 2348BC.
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

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

One problem might be this

Code:
myservo.write(75);

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

Code:
  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
Logged

Rob Gray aka the GRAYnomad www.robgray.com

Melbourne, Australia
Offline Offline
Newbie
*
Karma: 0
Posts: 4
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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 smiley
Logged

Global Moderator
UK
Offline Offline
Brattain Member
*****
Karma: 309
Posts: 26488
I don't think you connected the grounds, Dave.
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

This code
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.
Logged

"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.

Pages: [1]   Go Up
Jump to: