Now here is the code
#include <Servo.h> //includes the servo library
const int DiagDelay = 0; // To be insterted where needed.
const int rightWheelEN = 6; // H-Bride (pin 1, 1A-2A Enable)
const int rightWheel1A = 7; // H-bridge (pin 2, 1A)
const int rightWheel2A = 8; // H-bridge (pin 7, 2A)
const int leftWheelEN = 11; // H-Bride (pin 9, 3A-4A Enable)
const int leftWheel3A = 12; // H-bridge (pin 10, 3A)
const int leftWheel4A = 13; // H-bridge (pin 15, 4A)
const int rightWheelSpeed = 255; // Values used to PWM the Enable pin to control motor spped.
const int leftWheelSpeed = 230; // Values used to PWM the Enable pin to control motor spped.
const int baseServoPin = 2; // Control pin for the servo controlling horizontal basePositionition.
const int IRsensorServoPin = 4; // Control pin for the servo controlling the verticle IR sensor basePositionition.
const int IRsensorInputPin = A0; // Analog input pin for the IR sensor reading value for distance.
int obstacle = 400; // sensorValue value when a verticla object is encountered. .
int dropOff = 100; // sensorValue value when there is nothing encountered, i.e. no ground.
int basePosition = 0; // Initialize the base servo position variable.
Servo baseServo; // Define servo that horizontally (X Axis) rotates base plate.
Servo IRsensorServo; // Define servo that vertically (Y Axis) rotates the IR Sensor.
int sensorValue; // This is the averaged sensor reading returned from AvgSensor function.
int centerDistance = 0; // Placeholder for sensor reading so we can compare to decide which way to go later.
int leftDistance = 0; // Placeholder for sensor reading so we can compare to decide which way to go later.
int rightDistance = 0; // Placeholder for sensor reading so we can compare to decide which way to go later.
boolean pathClear = true; // State holder for sensorresult. True if no obstacle, false if path blocked.
boolean centerClear = true; // State holder for sensorresult. True if no obstacle, false if path blocked.
boolean leftClear = true; // State holder for sensorresult. True if no obstacle, false if path blocked.
boolean rightClear = true; // State holder for sensorresult. True if no obstacle, false if path blocked.
void setup ()
{
Serial.begin(9600);
pinMode(rightWheelEN, OUTPUT); // set all the other pins you’re using as outputs:
pinMode(rightWheel1A, OUTPUT);
pinMode(rightWheel2A, OUTPUT);
pinMode(leftWheelEN, OUTPUT);
pinMode(leftWheel3A, OUTPUT);
pinMode(leftWheel4A, OUTPUT);
pinMode(IRsensorServoPin, OUTPUT);
pinMode(baseServoPin, OUTPUT);
pinMode(IRsensorInputPin, INPUT); // Set all the pins being used as inputs:
baseServo.attach(baseServoPin); // Establish the servo connection for the servo control library.
IRsensorServo.attach(IRsensorServoPin);
baseServo.write(90); // Set the IR sensor servos to an initial basePositionition.
IRsensorServo.write(70);
delay(700); // Delay time to allow servo to move.
}
class PlatformMotors { // Class for the motors. This is to drive the L293NE Motor controller IC.
public:
void Forward() {
Serial.println(“Forward”);
analogWrite(rightWheelEN, rightWheelSpeed);
digitalWrite(rightWheel1A, LOW);
digitalWrite(rightWheel2A, HIGH);
analogWrite(leftWheelEN, leftWheelSpeed);
digitalWrite(leftWheel3A, HIGH);
digitalWrite(leftWheel4A, LOW);
return;
}
void Reverse() {
Serial.println(“Reverse”);
analogWrite(rightWheelEN, rightWheelSpeed);
digitalWrite(rightWheel1A, HIGH);
digitalWrite(rightWheel2A, LOW);
analogWrite(leftWheelEN, leftWheelSpeed);
digitalWrite(leftWheel3A, LOW);
digitalWrite(leftWheel4A, HIGH);
delay(500);
Halt();
return;
}
void Halt() {
Serial.println(“Halt”);
analogWrite(rightWheelEN, 0);
digitalWrite(rightWheel1A, LOW);
digitalWrite(rightWheel2A, LOW);
analogWrite(leftWheelEN, 0);
digitalWrite(leftWheel3A, LOW);
digitalWrite(leftWheel4A, LOW);
delay(500);
return;
}
void CounterClockWise () {
Serial.println(“Turn CounterClcokcWise”);
analogWrite(rightWheelEN, rightWheelSpeed);
digitalWrite(rightWheel1A, HIGH);
digitalWrite(rightWheel2A, LOW);
analogWrite(leftWheelEN, leftWheelSpeed);
digitalWrite(leftWheel3A, HIGH);
digitalWrite(leftWheel4A, LOW);
delay(800);
Halt();
return;
}
void ClockWise ()
{
Serial.println(“Turn ClockWise”);
analogWrite(rightWheelEN, leftWheelSpeed);
digitalWrite(rightWheel1A, LOW);
digitalWrite(rightWheel2A, HIGH);
analogWrite(leftWheelEN, leftWheelSpeed);
digitalWrite(leftWheel3A, LOW);
digitalWrite(leftWheel4A, HIGH);
delay(800);
Halt();
return;
}
};
int IRsensorRead() { // Get the IRsensor Reading function.
int sensorSample = 0; // Initialize the variable used for getting the sensor input. Not a rolling average.
int numSamples = 20;
int sensorCummulative = 0; // Initialize the variable used for averaging multiple sensor readings during the sample session.
for (int sampleCount = 0; sampleCount < numSamples; sampleCount++) // Loop through multiple sensor readings.
{
sensorSample = analogRead(IRsensorInputPin); // Get the reading from the analog input pin.
sensorCummulative = sensorCummulative + sensorSample; // Add the readings together for subsequent averaging.
sensorValue = sensorCummulative / numSamples; // Do the averaging division
}
return (sensorValue); // Return the averaged sensor reading (distance).
}
int findRoute() {
PlatformMotors PlatformDrive; // Create the object for the PaltformMotoros class.
PlatformDrive.Halt(); // For safety sake stop.
PlatformDrive.Reverse();
if (!leftClear) { // If obstacle is on the left turn right.
PlatformDrive.ClockWise();
}
else {
PlatformDrive.CounterClockWise(); // If obstacle is on th eright turn left.
}
Serial.println(pathClear);
Serial.println(leftClear);
Serial.println(rightClear);
pathClear = true; // Reset all of the path variables to true.
leftClear = true;
rightClear = true;
//delay(5000);
return 0;
}
int testPath()
{
PlatformMotors PlatformDrive; // Create the object for the class PlatformMotors.
if (sensorValue < obstacle && sensorValue > dropOff) // A high sensor value means vertical obstacle, a low sensor value means a dropoff.
{
Serial.println(“Forward”); // For diagnostic purposes.
pathClear = true; // Everything is cool, continue on.
PlatformDrive.Forward();
}
else
{
Serial.println(“ZOMG We’re gonna die!!!”); // For diagnostic and entertainment purposes.
pathClear = !pathClear;
if (basePosition < 91) // Determine where the base servo was when the obstacle was detected.
{
leftClear = !leftClear; // If it was pointing left then we can’t go that way.
}
else
{
if (basePosition > 90) // Same as above, onloy opposite
{
rightClear = !rightClear;
}
}
findRoute(); // Since there was something somewhere figure out a new way to go.
}
return(pathClear); // maybe might need to know this in the main loop in the future.
}
void loop()
{
for(basePosition = 60; basePosition < 145; basePosition += 3) // goes from 60 degrees to 145 degrees
{
baseServo.write(basePosition); // tell servo to go to basePositionition in variable ‘basePosition’
delay(5); // waits for the servo to reach the basePositionition
IRsensorRead(); // Get the averaged sensor reading
testPath(); // Make sure the path is clear
}
for(basePosition = 145; basePosition>=60; basePosition-= 3) // goes from 145 degrees to 60 degrees
{
baseServo.write(basePosition); // tell servo to go to basePositionition in variable ‘basePosition’
delay(5); // waits for the servo to reach the basePositionition
IRsensorRead(); // Get the averaged sensor reading
testPath(); // Make sure the path is clear
}
}
