Step 2: PROGRAMMING

Here we assume you already are familiar with PWM or pulse width modulation for servos. If you don't understand it then please refer to this forum post. This code reads the values from the Sharp IR sensors and decides whether it should continue on it's current path or reverse it's direction of travel due to an obstacle in it's path. (Note, one of the phototransistor's wire crossed path's with another wire. I was able to put out the flames before my robot burned down or worse the place where I live ).

#include <ServoTimer1.h>

// Rigel navigation with library ServoTimer1
// by vidam

ServoTimer1 servo1; // servo instance
ServoTimer1 servo2;
int pin = 9;
int pin2 = 10;
int pulse;
int pulsemin = 500; // in microseconds -- 0.50 ms pulse -- 0 deg -- most clockwise
int pulsemax = 2500; // in microseconds -- 2.50 ms pulse -- 180 deg -- most anticlockwise
int backIR = 0; // the input pin for the front Sharp IR
int frontIR = 1; // the input pin for the back Sharp IR
int front_val = 0; // variable to store the value coming from the sensor
int back_val = 0; // variable to store the value coming from the sensor

void setup()
{
Serial.begin(9600);

// prints title with ending line break
Serial.println("Servo Command");

// wait for the long string to be sent
delay(100);

// setup servo
servo1.attach(pin);
servo2.attach(pin2);
servo1.setMinimumPulse(pulsemin);
servo1.setMaximumPulse(pulsemax);
servo2.setMinimumPulse(pulsemin);
servo2.setMaximumPulse(pulsemax);
}
// to turn left num should be a value between 0 and 90

// to turn right num should be a value between 90 and 180
void turn(int num)
{
delay(100); // allow some time for the Serial data to be sent
servo1.write(num); // write the pulse to servo
servo2.write(num); // write the pulse to servo
}

// num is some value between 0 an 90
void forward(int num)
{
delay(100); // allow some time for the Serial data to be sent
servo1.write(num); // write the pulse to servo
servo2.write(90+num); // write the pulse to servo
}

// num is some value between 0 an 90
void backward(int num)
{
delay(100); // allow some time for the Serial data to be sent
servo2.write(num); // write the pulse to servo
servo1.write(num+90); // write the pulse to servo
}
void refresh()
{
servo1.refresh();
servo2.refresh();
}

void loop()
{
front_val = analogRead(frontIR); // read the value from the sensor
back_val = analogRead(backIR);
Serial.println("Front Val 1");
Serial.println(front_val);
delay(1000);
Serial.println("Back Val 1");
Serial.println(back_val);
delay(500);

while(analogRead(backIR) < 100 & analogRead(frontIR) < 100)
{
forward(30);
delay(500);
refresh();
}
Serial.println("Front Val 2");
Serial.println(front_val);
delay(1000);
Serial.println("Back Val 2");
Serial.println(back_val);
delay(500);

while(analogRead(backIR) > 100)
{
forward(30);
delay(500);
refresh();
}
Serial.println("Front Val 3");
Serial.println(front_val);
delay(1000);
Serial.println("Back Val 3");
Serial.println(back_val);
delay(500);

while(analogRead(frontIR) > 100)
{
backward(30);
delay(500);
refresh();
}

refresh();

}