as my fourth real program (not counting blink etc) Ive written a program to control a basic wall avoiding robot. The robot (bill) has two drive wheels and a rolling castor on the front. the sensor consists of a ping)) on a servo mounted to the front of the robot.
the code Ive written works, but as a super green programmer Im looking for ways to improve my coding skills and to make my avoidance algorithm(if you can even call it that) more efficient. if anyone would be willing to critique my code that would be great.
i couldnt paste it into the forum for some reason so here it is on google code
http://code.google.com/p/billbot/source/browse/BillBot_v0_4.ino
or you can download the code below
EDIT: it was a problem with my clipboard fixed now
/**********************************************
* *
* bill the robot *
* v 0.4 *
* Jack Murray *
* *
**********************************************/
// Libraries
#include <Servo.h>
// Setup the servo
Servo pingServo;
// Setup Pins
const int pingPin = 7;
const int directionA = 12;
const int directionB = 13;
const int speedA = 3;
const int speedB = 11;
const int brakeA = 9;
const int brakeB = 8;
const int greenLed = 2;
const int redLed = 3;
// setup variables
int pingDistance;
int pingDelay = 100;
int pingLeft;
int pingRight;
int pingForward;
int time;
void setup()
{
//attach servos
pingServo.attach(5);
//setup LEDs
pinMode(greenLed, OUTPUT);
pinMode(redLed, OUTPUT);
digitalWrite(greenLed, HIGH);
//setup motorshield
pinMode(directionA, OUTPUT);
pinMode(directionB, OUTPUT);
pinMode(speedA, OUTPUT);
pinMode(speedB, OUTPUT);
pinMode(brakeA, OUTPUT);
pinMode(brakeB, OUTPUT);
Serial.begin(9600);
// set initial servo positions
pingServo.write(100);
//delay after power applied
delay(5000);
}
void loop()
{
directionalPing();
if(pingLeft <= 6)
{
reverse(50);
turnRight(600);
return;
}
if(pingRight <= 6)
{
reverse(50);
turnLeft(600);
return;
}
if(pingForward <= 6)
{
reverse(200);
turnLeft(600);
return;
}
if(pingForward > 6)
{
reverse(50);
forward(150);
return;
}
}
long turnLeft(long time)
{
digitalWrite(redLed, HIGH);
Serial.println("turning left");
digitalWrite(directionA, LOW);
digitalWrite(directionB, LOW);
digitalWrite(brakeB, LOW);
digitalWrite(brakeA, LOW);
digitalWrite(speedA, HIGH);
digitalWrite(speedB, HIGH);
delay(time);
digitalWrite(redLed, LOW);
}
long turnRight(long time)
{
digitalWrite(redLed, HIGH);
Serial.println("turning right");
digitalWrite(directionA, HIGH);
digitalWrite(directionB, HIGH);
digitalWrite(brakeB, LOW);
digitalWrite(brakeA, LOW);
digitalWrite(speedA, HIGH);
digitalWrite(speedB, HIGH);
delay(time);
digitalWrite(redLed, LOW);
}
long forward(long time)
{
digitalWrite(redLed, HIGH);
Serial.println("driving forward");
digitalWrite(directionA, LOW);
digitalWrite(directionB, HIGH);
digitalWrite(brakeB, LOW);
digitalWrite(brakeA, LOW);
digitalWrite(speedA, HIGH);
digitalWrite(speedB, HIGH);
delay(time);
digitalWrite(redLed, LOW);
}
long reverse(long time)
{
digitalWrite(redLed, HIGH);
Serial.println("driving backwards");
digitalWrite(directionA, HIGH);
digitalWrite(directionB, LOW);
digitalWrite(brakeB, LOW);
digitalWrite(brakeA, LOW);
digitalWrite(speedA, HIGH);
digitalWrite(speedB, HIGH);
delay(time);
digitalWrite(redLed, LOW);
}
long brake(long time)
{
digitalWrite(speedA, LOW);
digitalWrite(speedB, LOW);
digitalWrite(brakeB, HIGH);
digitalWrite(brakeA, HIGH);
delay(time);
}
void directionalPing()
{
pingServo.write(150);
delay(500);
ping();
pingLeft = pingDistance;
pingServo.write(100);
delay(500);
ping();
pingForward = pingDistance;
pingServo.write(50);
delay(500);
ping();
pingRight = pingDistance;
Serial.print("left ");
Serial.println(pingLeft);
Serial.print("forward ");
Serial.println(pingForward);
Serial.print("right ");
Serial.println(pingRight);
Serial.println();
}
void ping()
{
long duration, inches;
//trigger the ping)) sensor
pinMode(pingPin, OUTPUT);
digitalWrite(pingPin, LOW);
delayMicroseconds(2);
digitalWrite(pingPin, HIGH);
delayMicroseconds(5);
digitalWrite(pingPin, LOW);
//read the ping)) sensor
pinMode(pingPin, INPUT);
duration = pulseIn(pingPin, HIGH);
// convert the time into a distance
inches = microsecondsToInches(duration);
//output PingDistance
pingDistance = inches;
}
long microsecondsToInches(long microseconds)
{
return microseconds / 74 / 2;
}
BillBot_v0_4.ino (4.05 KB)