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!
#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.