A few questions about my basic wall avoiding robot

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)

30 views and not a single reply?

The arbitrary order of your tests means that the robot will prefer a right turn over other operations even if they would be more appropriate. Consider checking all of the ping results before you decide what to do.

Your movement functions don't return anything - they should be void.

The movement functions are all very similar. I would make a single function move that takes parameters for the two motor directions and the length of delay and call it from the other movement functions with apropriate parameters.

so i changed all the movement functions to void, and I'm starting on the writing of a new function "movement" I don't know how to go about turning the motor parameters to HIGH and LOW in the code. The only way i can think of at the moment is Boolean if statements looking for 0 or 1, but im sure there has to be a simpler method, I just cant think of it...

EDIT: example of boolean movement statement

/**********************************************
 *                                            *
 * 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;
    }
}

void turnLeft(int time)
{
  movement(1, 0, time);
  Serial.println("turn left");
  Serial.println(time);
  Serial.println();
}

void turnRight(int time)
{
  movement(0, 1, time);
  Serial.println("turn right");
  Serial.println(time);
  Serial.println();
}

void forward(int time)
{
  movement(1, 1, time);
  Serial.println("drive forward");
  Serial.println(time);
  Serial.println();
}

void reverse(int time)
{
  movement(0, 0, time);
  Serial.println("drive reverse");
  Serial.println(time);
  Serial.println();
}

void movement(int dirA, int dirB, long time)  // dirA and dirB 0=reverse 1=forward
{
  digitalWrite(brakeB, LOW);
  digitalWrite(brakeA, LOW);
  
  if(dirA == 0)
    {
      digitalWrite(directionA, HIGH);
      digitalWrite(speedA, HIGH);
    }
  if(dirA == 1)
    {
      digitalWrite(directionA, LOW);
      digitalWrite(speedA, HIGH);
    }
  if(dirB == 0)
    {
      digitalWrite(directionA, LOW);
      digitalWrite(speedA, HIGH);
    }
  if(dirB == 1)
    {
      digitalWrite(directionA, HIGH);
      digitalWrite(speedA, 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;
}

Also how would I go about checking all the ping values before making a movement decision?

ffmurray:
so i changed all the movement functions to void, and I'm starting on the writing of a new function "movement" I don't know how to go about turning the motor parameters to HIGH and LOW in the code. The only way i can think of at the moment is Boolean if statements looking for 0 or 1, but im sure there has to be a simpler method, I just cant think of it...

HIGH and LOW are just defines in Arduino.h (one of the core Arduino source files) as follows:

#define HIGH 0x1
#define LOW  0x0

In other words, HIGH and LOW are just 1 and 0, respectively. So any other expression that evaluates to, or can be implicitly cast to, a 1 or 0 will work just as well as HIGH or LOW.

thanks jraskell;

How does this movement function code look? is there any better way of doing it?

void movement(int dirA, int dirB, long time)  // dirA and dirB 0=reverse 1=forward
{
  digitalWrite(brakeB, LOW);
  digitalWrite(brakeA, LOW);
  digitalWrite(directionA, dirA);
  digitalWrite(directionB, dirB);
  digitalWrite(speedA, HIGH);
  digitalWrite(speedB, HIGH);
  delay(time);
  digitalWrite(speedA, LOW);
  digitalWrite(speedB, LOW);
  digitalWrite(brakeB, HIGH);
  digitalWrite(brakeA, HIGH);
}

it seems to work more smoothly than my other code

That looks better. Although why do you stop and brake at the end? You're likely to immediately start moving again anyway. As to deciding what to do at the end, I'd go through each of your test cases and set a state variable in the ifs to indicate what your next action will be. If you also keep a variable that tells you how close the obstacle is, you can overrule the decision if the current ping data reveals it's closer. Once you've come out of these tests, the state variable will tell you which of the movement functions to call.

the servo im using is old and crappy and i couldnt get it to make the full sweep without delay(400) x 3(one per ping) so it stops so it doesnt run into the wall while its sweeping (like it did the first time and popped the ping)) of the servo)

im still trying to make the main loop more efficient and effective, i would like to take into account all the sensor data before making a decision but dont know how with if then trees. any hints?