Hello,
My bot seems to have a mind of its own. code seems to be working but at times it react to an object thats not there??? its set to find a better path after coming 30 cm within an object but sometimes theres nothing there and it looks for a better path? can this be from the ping returning false values, or i think it might be from the 9v battery draining quickly. Any ideas?? Thanks
Video here: http://www.youtube.com/watch?v=SiXvlV6-0S8
Heres the code:
#define servoPin 12 // Servomotor on pin D12.
#define pingpin 13 // Ping ultrasonic sensor on pin D13.
#define redled 9 // 2 drive motors, motor driver SN754410NE
#define whiteled 8
#define leftSpeedPin 10 // Left motor speed control
#define leftmotorpin1 5 // Left motor direction 1
#define leftmotorpin2 6 // Left motor direction 2
#define rightmotorpin1 3 // Right motor direction 1
#define rightmotorpin2 4 // Right motor direction 2
#define rightSpeedPin 11 // Right motor speed control
#define SERVOCENTER 1500 // Tweak so sensor looks straight ahead.
#define SERVORIGHT 550 // Servo position when looking right.
#define SERVOLEFT 2500 // Servo position when looking left.
int numPulses; // Number of pulses to servomotor.
void setup() {
pinMode(leftmotorpin1, OUTPUT); // Set motor pins as outputs.
pinMode(leftmotorpin2, OUTPUT);
pinMode(leftSpeedPin, OUTPUT);
pinMode(rightmotorpin1, OUTPUT);
pinMode(rightmotorpin2, OUTPUT);
pinMode(rightSpeedPin, OUTPUT);
pinMode(servoPin, OUTPUT); // Servomotor as output.
pinMode(redled, OUTPUT); // leds as outputs
pinMode(whiteled, OUTPUT);
centerServo(); // Centers Servo
delay(2000); // Wait 2 seconds
Serial.begin(115200);
}
void loop() {
int leftDistance, rightDistance, currDist; // Distance readings from Ping sensor.
int dangerlevel = 30; // How far away should things be, before we react?
// int numPulses;
currDist = readDistance();
if(currDist > dangerlevel) {
nodanger(); // If no danger, drive ahead
}
else {
whichway(); // If obstacle ahead then decide which way is better
}
}
void nodanger() { // Drive forward
digitalWrite(whiteled, HIGH);
digitalWrite(redled, LOW);
analogWrite(leftSpeedPin, 100); //PWM Speed Control 0 - 255
analogWrite(rightSpeedPin, 100);
digitalWrite(leftmotorpin1, HIGH);
digitalWrite(leftmotorpin2, LOW);
digitalWrite(rightmotorpin1, HIGH);
digitalWrite(rightmotorpin2, LOW);
return;
}
void whichway() {
digitalWrite(whiteled, LOW);
digitalWrite(redled, HIGH);
totalhalt(); // First stop before looking left and right
int leftDistance, rightDistance, currDist;
for (numPulses = 0; numPulses < 20; numPulses++) // Turn servo left (look left)
{
pulseOut(servoPin, SERVOLEFT);
delay(40);
}
leftDistance = readDistance(); // Check distance
totalhalt(); // totalhalt also centers the servo
for (numPulses = 0; numPulses < 20; numPulses++) // Turn servo right (look right)
{
pulseOut(servoPin, SERVORIGHT);
delay(40);
}
rightDistance = readDistance(); // Check distance
totalhalt(); // totalhalt also centers the servo
if(leftDistance > rightDistance) { // Choose better direction. Greater space
body_leftturn();
}
else {
body_rightturn();
}
return;
}
void totalhalt() {
digitalWrite(whiteled, LOW);
digitalWrite(redled, HIGH);
digitalWrite(leftmotorpin1, LOW);
digitalWrite(leftmotorpin2, LOW);
digitalWrite(rightmotorpin1, LOW);
digitalWrite(rightmotorpin2, LOW);
digitalWrite(leftSpeedPin, LOW);
digitalWrite(rightSpeedPin, LOW);
centerServo();
delay(500);
}
void body_leftturn() {
digitalWrite(whiteled, LOW);
digitalWrite(redled, HIGH);
digitalWrite(leftmotorpin1, LOW);
digitalWrite(leftmotorpin2, HIGH);
digitalWrite(rightmotorpin1, HIGH);
digitalWrite(rightmotorpin2, LOW);
digitalWrite(leftSpeedPin, HIGH);
digitalWrite(rightSpeedPin, HIGH);
delay(200);
totalhalt();
}
void body_rightturn() {
digitalWrite(whiteled, LOW);
digitalWrite(redled, HIGH);
digitalWrite(leftmotorpin1, HIGH);
digitalWrite(leftmotorpin2, LOW);
digitalWrite(rightmotorpin1, LOW);
digitalWrite(rightmotorpin2, HIGH);
digitalWrite(leftSpeedPin, HIGH);
digitalWrite(rightSpeedPin, HIGH);
delay(200);
totalhalt();
}
void pulseOut(byte pinNumber, int duration)
{
digitalWrite(servoPin, HIGH);
delayMicroseconds(duration);
digitalWrite(servoPin, LOW);
}
void centerServo()
{
byte index;
for (index = 0; index < 20; index++)
{
pulseOut(servoPin, SERVOCENTER);
delay(20);
}}
long readDistance() {
long duration, inches, cm;
// The Ping is triggered by a HIGH pulse of 2 or more microseconds.
// We 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);
return(cm);
}
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 traveled.
return microseconds / 29 / 2;}