Let's Make Robots!

YellowBot Source Code

I have not put togethger documentation for my Bots code. The code is evolving too quickly for detailed up front documentation. I would like tos tate that a primary goal of doing this Bot was to help me learn Object Oreinted Programming, as well as get me used to programming again. it's been 20 years since I've written something more than a simple shell. python or Perl script.

Thus I will warn you this code is chocked full of redundancies and inefficiencies. Bear with me, an old dog is learning new tricks, it's just taking time :) 

Version 1 was based on the simple object avoidance approach: Go until an object is detected. When detected stop, look left, look right. determine best direction to go. Turn in that direction and wander on. The only real downfall to this architecture is that the Bot can get hung up on things that are just off to the side of the sensor. You have to sweep the sensor back and forth, across the width of the bBt, to insure that the whole path is clear.

That's where V2 of my code comes in. I figured out how to test the path while sweeping the sensor side to side. When an obstacle is detected the Bot determines if it's to the left or right and turns the other way. This code is still crude. futurer improvements include seeing if an obstacle can be avoided with out stopping along with proportional turning radius. Plus making improvements on functions and classes as I learn. 

My code is pretty well commented, so I will let it speak for itself. I

 

Comment viewing options

Select your preferred way to display the comments and click "Save settings" to activate your changes.
kolnari's picture

Thanks for the clue.  Updated and added my email. I'll follow up with a properly formatted post of the code.

kolnari's picture
#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  
  } 
}

kolnari's picture

Sigh.

birdmun's picture

I didn't mean to imply you needed to repost your code. I was complimenting you. Most people, I'm sure me included, just paste the code w/ all the extra spaces.

Secondly, when pasting HTML, you need to click the "button" that says HTML at the top of the text entry box. You should see:
B I U "strike" left justify, right justify, center justify, bullet list, numbered list, link, unlink (for adding/deleting web links), anchor, image, format brush(?), block quote, HTML, bar(?), and symbols.

If you click the HTML you will get a new window where you can paste HTML. I would suggest that if you plan to add anything to the end/beginning of your post that you add <p></p> in said HTML window. The p tags will allow you to type anything extra in the standard post font.

/*  tadpole1.ino
    Author: Kyle
    Date: 3-16-2013
    Purpose: First program to move my Tadpole
*/

// constants based on Mini Driver
const int BATTERYVOLTAGE = A7;
const int LEDPIN = 13;
const int LEFTMOTORSPEED = 9;
const int LEFTMOTORDIRECTION = 7;
const int RIGHTMOTORSPEED = 10;
const int RIGHTMOTORDIRECTION = 8;

//constants based on additional IR sensor
const int IRTRIGGER = 0;
const int LEFTIR = A0;
const int CENTERIR = A1;
const int RIGHTIR = A2;

//constants for motor speed and direction
const int FWD = 1;
const int REV = 0;
const int FAST = 255;
const int MED = 113;
const int SLOW = 40;

const int BLANK = 200;
const int TOOCLOSE = 15;

int leftDistance;
int centerDistance;
int rightDistance;

void setup() {
  Serial.begin(9600);
  pinMode(LEFTMOTORSPEED, OUTPUT);
  pinMode(LEFTMOTORDIRECTION, OUTPUT);
  pinMode(RIGHTMOTORSPEED, OUTPUT);
  pinMode(RIGHTMOTORDIRECTION, OUTPUT);
  pinMode(IRTRIGGER, OUTPUT);
  pinMode(LEDPIN, OUTPUT);
}
void loop() {
  //if (checkVoltage()) {
    if (1) {
    /*
    Forward = robotMove(spd, FWD, spd, FWD, dly);
    Reverse = robotMove(spd, REV, spd, REV, dly);
    Rotate Left = robotMove(spd, REV, spd, FWD, dly);
    Rotate Right = robotMove(spd, FWD, spd, REV, dly);
    Arc Left = robotMove(spd, FWD, faster spd, FWD, dly);
    Stop is not currently required.
    final 0 may be replaced for a delay
    Stop = robotMove(0, FWD, 0, FWD, 0);
    */
    checkDistance();
    
//    if ((leftDistance < TOOCLOSE) && (rightDistance < TOOCLOSE) || 
//        (centerDistance < TOOCLOSE)) {
//      robotMove(MED, REV, MED, REV, 1000);
//      robotMove(MED, REV, MED, FWD, 250);
//    } else if (leftDistance < TOOCLOSE) {
//      robotMove(MED, FWD, SLOW, FWD, 500);
//    } else if (rightDistance < TOOCLOSE) {
//      robotMove(SLOW, FWD, MED, FWD, 500);
//    } else {
//      robotMove(FAST, FWD, FAST, FWD, 500);
//    }
    if (centerDistance < TOOCLOSE) {
      robotMove(MED, REV, MED, REV, 1000);
      robotMove(MED, REV, MED, FWD, 250);
    } else {
      robotMove(FAST, FWD, FAST, FWD, 500);
    }
  }
}

void robotMove(int spdLM, boolean dirLM, int spdRM, boolean dirRM, 
              int dly) {
  analogWrite(LEFTMOTORSPEED, spdLM);
  digitalWrite(LEFTMOTORDIRECTION, dirLM);
  analogWrite(RIGHTMOTORSPEED, spdRM);
  digitalWrite(RIGHTMOTORDIRECTION, dirRM);
  delay(dly);
}

boolean checkVoltage() {
  boolean flag = 0;
  //512 is said to be 5v. I want to stop before I get there.
  if (analogRead(BATTERYVOLTAGE) > 513) {
    flag = 1;
  } else {
    while(1) {
      blinkLed3(200);
      blinkLed3(500);
      blinkLed3(200);
      delay(500);
      checkVoltage();
    }
  }
  return flag;
}

void blinkLed3(int dly) {
  boolean ledState = LOW;
  for(int i=0; i <= 2; ++i) {
    ledState = !ledState;
    digitalWrite(LEDPIN, ledState);
    delay(dly);
    ledState = !ledState;
    digitalWrite(LEDPIN, ledState);
    delay(BLANK);
  }
}

void checkDistance() {
  digitalWrite(IRTRIGGER, OUTPUT);
  leftDistance = analogRead(LEFTIR);
  centerDistance = analogRead(CENTERIR);
  rightDistance = analogRead(RIGHTIR);
//  Serial.print(leftDistance);
//  Serial.print("\t");
//  Serial.print(centerDistance);
//  Serial.print("\t");
//  Serial.println(rightDistance);
}

kolnari's picture

Thanks for helping me, I really appreciate it.I was really hoping to get my code posted, in a legible format, so I could share with the community.  When I was trying to figure out how to do sensor sweeping I could not find any code examples. I'm hoping to help fill that gap. Now that I have your instructions I'm going to try and post it again, to see if I can get it right. Now it's become personal :)

 

kolnari's picture

<pre>

#include&nbsp;&lt;<span style="color: #CC6600;">Servo</span>.h&gt;    <span style="color: #7E7E7E;">//includes the servo library</span>

<span style="color: #CC6600;">const</span> <span style="color: #CC6600;">int</span> DiagDelay = 0;                                                <span style="color: #7E7E7E;">// To be insterted where needed.  </span>
<span style="color: #CC6600;">const</span> <span style="color: #CC6600;">int</span> rightWheelEN = 6;                                                <span style="color: #7E7E7E;">// H-Bride  (pin 1, 1A-2A Enable)</span>
<span style="color: #CC6600;">const</span> <span style="color: #CC6600;">int</span> rightWheel1A = 7;                                                <span style="color: #7E7E7E;">// H-bridge (pin 2, 1A)</span>
<span style="color: #CC6600;">const</span> <span style="color: #CC6600;">int</span> rightWheel2A = 8;                                                <span style="color: #7E7E7E;">// H-bridge (pin 7, 2A)</span>
<span style="color: #CC6600;">const</span> <span style="color: #CC6600;">int</span> leftWheelEN  = 11;                                               <span style="color: #7E7E7E;">// H-Bride  (pin 9, 3A-4A Enable)</span>
<span style="color: #CC6600;">const</span> <span style="color: #CC6600;">int</span> leftWheel3A  = 12;                                               <span style="color: #7E7E7E;">// H-bridge (pin 10, 3A)</span>
<span style="color: #CC6600;">const</span> <span style="color: #CC6600;">int</span> leftWheel4A  = 13;                                               <span style="color: #7E7E7E;">// H-bridge (pin 15, 4A)</span>
<span style="color: #CC6600;">const</span> <span style="color: #CC6600;">int</span> rightWheelSpeed = 255;                                           <span style="color: #7E7E7E;">// Values used to PWM the Enable pin to control motor spped.</span>
<span style="color: #CC6600;">const</span> <span style="color: #CC6600;">int</span> leftWheelSpeed = 230;                                            <span style="color: #7E7E7E;">// Values used to PWM the Enable pin to control motor spped.</span>
<span style="color: #CC6600;">const</span> <span style="color: #CC6600;">int</span> baseServoPin     = 2;                                            <span style="color: #7E7E7E;">// Control pin for the servo controlling horizontal basePositionition.</span>
<span style="color: #CC6600;">const</span> <span style="color: #CC6600;">int</span> IRsensorServoPin = 4;                                            <span style="color: #7E7E7E;">// Control pin for the servo controlling the verticle IR sensor basePositionition.</span>
<span style="color: #CC6600;">const</span> <span style="color: #CC6600;">int</span> IRsensorInputPin = A0;                                           <span style="color: #7E7E7E;">// Analog input pin for the IR sensor reading value for distance.</span>

<span style="color: #CC6600;">int</span> obstacle       = 400;                                                  <span style="color: #7E7E7E;">// sensorValue value when a verticla object is encountered.  .</span>
<span style="color: #CC6600;">int</span> dropOff        = 100;                                                  <span style="color: #7E7E7E;">// sensorValue value when there is nothing encountered, i.e. no ground.   </span>
<span style="color: #CC6600;">int</span> basePosition   = 0;                                                    <span style="color: #7E7E7E;">// Initialize the base servo position variable.</span>

<span style="color: #CC6600;">Servo</span> baseServo;                                                           <span style="color: #7E7E7E;">// Define servo that horizontally (X Axis) rotates base plate. </span>
<span style="color: #CC6600;">Servo</span> IRsensorServo;                                                       <span style="color: #7E7E7E;">// Define servo that vertically (Y Axis) rotates the IR Sensor.  </span>

<span style="color: #CC6600;">int</span> sensorValue;                                                           <span style="color: #7E7E7E;">// This is the averaged sensor reading returned from AvgSensor function.</span>
<span style="color: #CC6600;">int</span> centerDistance = 0;                                                    <span style="color: #7E7E7E;">// Placeholder for sensor reading so we can compare to decide which way to go later.</span>
<span style="color: #CC6600;">int</span> leftDistance   = 0;                                                    <span style="color: #7E7E7E;">// Placeholder for sensor reading so we can compare to decide which way to go later. </span>
<span style="color: #CC6600;">int</span> rightDistance  = 0;                                                    <span style="color: #7E7E7E;">// Placeholder for sensor reading so we can compare to decide which way to go later. </span>

<span style="color: #CC6600;">boolean</span> pathClear   = <span style="color: #CC6600;">true</span>;                                                 <span style="color: #7E7E7E;">// State holder for sensorresult. True if no obstacle, false if path blocked.</span>
<span style="color: #CC6600;">boolean</span> centerClear = <span style="color: #CC6600;">true</span>;                                                 <span style="color: #7E7E7E;">// State holder for sensorresult. True if no obstacle, false if path blocked.</span>
<span style="color: #CC6600;">boolean</span> leftClear   = <span style="color: #CC6600;">true</span>;                                                 <span style="color: #7E7E7E;">// State holder for sensorresult. True if no obstacle, false if path blocked. </span>
<span style="color: #CC6600;">boolean</span> rightClear  = <span style="color: #CC6600;">true</span>;                                                 <span style="color: #7E7E7E;">// State holder for sensorresult. True if no obstacle, false if path blocked.</span>

<span style="color: #CC6600;">void</span> <span style="color: #CC6600;"><b>setup</b></span> ()
{
&nbsp;&nbsp;<span style="color: #CC6600;"><b>Serial</b></span>.<span style="color: #CC6600;">begin</span>(9600);
&nbsp;&nbsp;<span style="color: #CC6600;">pinMode</span>(rightWheelEN, <span style="color: #006699;">OUTPUT</span>);                                            <span style="color: #7E7E7E;">// set all the other pins you're using as outputs:</span>
&nbsp;&nbsp;<span style="color: #CC6600;">pinMode</span>(rightWheel1A, <span style="color: #006699;">OUTPUT</span>);
&nbsp;&nbsp;<span style="color: #CC6600;">pinMode</span>(rightWheel2A, <span style="color: #006699;">OUTPUT</span>);
&nbsp;&nbsp;<span style="color: #CC6600;">pinMode</span>(leftWheelEN, <span style="color: #006699;">OUTPUT</span>); 
&nbsp;&nbsp;<span style="color: #CC6600;">pinMode</span>(leftWheel3A, <span style="color: #006699;">OUTPUT</span>);
&nbsp;&nbsp;<span style="color: #CC6600;">pinMode</span>(leftWheel4A, <span style="color: #006699;">OUTPUT</span>);
&nbsp;&nbsp;<span style="color: #CC6600;">pinMode</span>(IRsensorServoPin, <span style="color: #006699;">OUTPUT</span>);                                      
&nbsp;&nbsp;<span style="color: #CC6600;">pinMode</span>(baseServoPin,     <span style="color: #006699;">OUTPUT</span>);
&nbsp;&nbsp;<span style="color: #CC6600;">pinMode</span>(IRsensorInputPin, <span style="color: #006699;">INPUT</span>);                                        <span style="color: #7E7E7E;">// Set all the pins being used as inputs:</span>
&nbsp;&nbsp;baseServo.<span style="color: #CC6600;">attach</span>(baseServoPin);                                          <span style="color: #7E7E7E;">// Establish the servo connection for the servo control library.</span>
&nbsp;&nbsp;IRsensorServo.<span style="color: #CC6600;">attach</span>(IRsensorServoPin);
&nbsp;&nbsp;baseServo.<span style="color: #CC6600;">write</span>(90);                                                     <span style="color: #7E7E7E;">// Set the IR sensor servos to an initial basePositionition.</span>
&nbsp;&nbsp;IRsensorServo.<span style="color: #CC6600;">write</span>(70);
&nbsp;&nbsp;<span style="color: #CC6600;">delay</span>(700);                                                              <span style="color: #7E7E7E;">// Delay time to allow servo to move.</span>
}

<span style="color: #CC6600;">class</span> PlatformMotors {                                                     <span style="color: #7E7E7E;">// Class for the motors. This is to drive the L293NE Motor controller IC.</span>
&nbsp;&nbsp;
&nbsp;&nbsp;<span style="color: #CC6600;">public</span>:

<span style="color: #CC6600;">void</span> Forward() {
&nbsp;&nbsp;&nbsp;<span style="color: #CC6600;"><b>Serial</b></span>.<span style="color: #CC6600;">println</span>(<span style="color: #006699;">"Forward"</span>); 
&nbsp;&nbsp;&nbsp;<span style="color: #CC6600;">analogWrite</span>(rightWheelEN, rightWheelSpeed);
&nbsp;&nbsp;&nbsp;<span style="color: #CC6600;">digitalWrite</span>(rightWheel1A, <span style="color: #006699;">LOW</span>);  
&nbsp;&nbsp;&nbsp;<span style="color: #CC6600;">digitalWrite</span>(rightWheel2A, <span style="color: #006699;">HIGH</span>); 
&nbsp;&nbsp;&nbsp;<span style="color: #CC6600;">analogWrite</span>(leftWheelEN, leftWheelSpeed); 
&nbsp;&nbsp;&nbsp;<span style="color: #CC6600;">digitalWrite</span>(leftWheel3A, <span style="color: #006699;">HIGH</span>); 
&nbsp;&nbsp;&nbsp;<span style="color: #CC6600;">digitalWrite</span>(leftWheel4A, <span style="color: #006699;">LOW</span>); 
&nbsp;&nbsp;&nbsp;<span style="color: #CC6600;">return</span>;
}

<span style="color: #CC6600;">void</span> Reverse() {
&nbsp;&nbsp;<span style="color: #CC6600;"><b>Serial</b></span>.<span style="color: #CC6600;">println</span>(<span style="color: #006699;">"Reverse"</span>);
&nbsp;&nbsp;<span style="color: #CC6600;">analogWrite</span>(rightWheelEN, rightWheelSpeed);
&nbsp;&nbsp;<span style="color: #CC6600;">digitalWrite</span>(rightWheel1A, <span style="color: #006699;">HIGH</span>);
&nbsp;&nbsp;<span style="color: #CC6600;">digitalWrite</span>(rightWheel2A, <span style="color: #006699;">LOW</span>); 
&nbsp;&nbsp;<span style="color: #CC6600;">analogWrite</span>(leftWheelEN, leftWheelSpeed); 
&nbsp;&nbsp;<span style="color: #CC6600;">digitalWrite</span>(leftWheel3A, <span style="color: #006699;">LOW</span>); 
&nbsp;&nbsp;<span style="color: #CC6600;">digitalWrite</span>(leftWheel4A, <span style="color: #006699;">HIGH</span>); 
&nbsp;&nbsp;<span style="color: #CC6600;">delay</span>(500);
&nbsp;&nbsp;Halt();
&nbsp;&nbsp;<span style="color: #CC6600;">return</span>;
}

<span style="color: #CC6600;">void</span> Halt() {
&nbsp;&nbsp;<span style="color: #CC6600;"><b>Serial</b></span>.<span style="color: #CC6600;">println</span>(<span style="color: #006699;">"Halt"</span>);
&nbsp;&nbsp;<span style="color: #CC6600;">analogWrite</span>(rightWheelEN, 0);
&nbsp;&nbsp;<span style="color: #CC6600;">digitalWrite</span>(rightWheel1A, <span style="color: #006699;">LOW</span>); 
&nbsp;&nbsp;<span style="color: #CC6600;">digitalWrite</span>(rightWheel2A, <span style="color: #006699;">LOW</span>); 
&nbsp;&nbsp;<span style="color: #CC6600;">analogWrite</span>(leftWheelEN, 0); 
&nbsp;&nbsp;<span style="color: #CC6600;">digitalWrite</span>(leftWheel3A, <span style="color: #006699;">LOW</span>); 
&nbsp;&nbsp;<span style="color: #CC6600;">digitalWrite</span>(leftWheel4A, <span style="color: #006699;">LOW</span>); 
&nbsp;&nbsp;<span style="color: #CC6600;">delay</span>(500);                     
&nbsp;&nbsp;<span style="color: #CC6600;">return</span>;
}

<span style="color: #CC6600;">void</span> CounterClockWise () {
&nbsp;&nbsp;<span style="color: #CC6600;"><b>Serial</b></span>.<span style="color: #CC6600;">println</span>(<span style="color: #006699;">"Turn CounterClcokcWise"</span>);
&nbsp;&nbsp;<span style="color: #CC6600;">analogWrite</span>(rightWheelEN, rightWheelSpeed);
&nbsp;&nbsp;<span style="color: #CC6600;">digitalWrite</span>(rightWheel1A, <span style="color: #006699;">HIGH</span>); 
&nbsp;&nbsp;<span style="color: #CC6600;">digitalWrite</span>(rightWheel2A, <span style="color: #006699;">LOW</span>); 
&nbsp;&nbsp;<span style="color: #CC6600;">analogWrite</span>(leftWheelEN, leftWheelSpeed); 
&nbsp;&nbsp;<span style="color: #CC6600;">digitalWrite</span>(leftWheel3A, <span style="color: #006699;">HIGH</span>); 
&nbsp;&nbsp;<span style="color: #CC6600;">digitalWrite</span>(leftWheel4A, <span style="color: #006699;">LOW</span>); 
&nbsp;&nbsp;<span style="color: #CC6600;">delay</span>(800);                    
&nbsp;&nbsp;Halt();
&nbsp;&nbsp;<span style="color: #CC6600;">return</span>;
}

<span style="color: #CC6600;">void</span> ClockWise ()
&nbsp;&nbsp;{
&nbsp;&nbsp;<span style="color: #CC6600;"><b>Serial</b></span>.<span style="color: #CC6600;">println</span>(<span style="color: #006699;">"Turn ClockWise"</span>);
&nbsp;&nbsp;<span style="color: #CC6600;">analogWrite</span>(rightWheelEN, leftWheelSpeed);
&nbsp;&nbsp;<span style="color: #CC6600;">digitalWrite</span>(rightWheel1A, <span style="color: #006699;">LOW</span>);  
&nbsp;&nbsp;<span style="color: #CC6600;">digitalWrite</span>(rightWheel2A, <span style="color: #006699;">HIGH</span>); 
&nbsp;&nbsp;<span style="color: #CC6600;">analogWrite</span>(leftWheelEN, leftWheelSpeed); 
&nbsp;&nbsp;<span style="color: #CC6600;">digitalWrite</span>(leftWheel3A, <span style="color: #006699;">LOW</span>); 
&nbsp;&nbsp;<span style="color: #CC6600;">digitalWrite</span>(leftWheel4A, <span style="color: #006699;">HIGH</span>); 
&nbsp;&nbsp;<span style="color: #CC6600;">delay</span>(800);                      
&nbsp;&nbsp;Halt();
&nbsp;&nbsp;<span style="color: #CC6600;">return</span>;
&nbsp;&nbsp;}
};

<span style="color: #CC6600;">int</span> IRsensorRead() {                                                       <span style="color: #7E7E7E;">// Get the IRsensor Reading function.</span>
&nbsp;&nbsp;<span style="color: #CC6600;">int</span> sensorSample = 0;                                                    <span style="color: #7E7E7E;">// Initialize the variable used for getting the sensor input. Not a rolling average.</span>
&nbsp;&nbsp;<span style="color: #CC6600;">int</span> numSamples = 20;
&nbsp;&nbsp;<span style="color: #CC6600;">int</span> sensorCummulative = 0;                                               <span style="color: #7E7E7E;">// Initialize the variable used for averaging multiple sensor readings during the sample session. </span>
&nbsp;&nbsp;<span style="color: #CC6600;">for</span> (<span style="color: #CC6600;">int</span> sampleCount = 0; sampleCount &lt; numSamples; sampleCount++)       <span style="color: #7E7E7E;">// Loop through multiple sensor readings.</span>
&nbsp;&nbsp;{
&nbsp;&nbsp;&nbsp;&nbsp;sensorSample&nbsp;=&nbsp;<span style="color: #CC6600;">analogRead</span>(IRsensorInputPin);                           <span style="color: #7E7E7E;">// Get the reading from the analog input pin.   </span>
&nbsp;&nbsp;&nbsp;&nbsp;sensorCummulative&nbsp;=&nbsp;sensorCummulative&nbsp;+&nbsp;sensorSample;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;<span style="color: #7E7E7E;">// Add the readings together for subsequent averaging.</span>
&nbsp;&nbsp;&nbsp;&nbsp;sensorValue&nbsp;=&nbsp;sensorCummulative&nbsp;/&nbsp;numSamples;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;<span style="color: #7E7E7E;">// Do the averaging division</span>
&nbsp;&nbsp;&nbsp;}
&nbsp;&nbsp;<span style="color: #CC6600;">return</span> (sensorValue);                                                    <span style="color: #7E7E7E;">// Return the averaged sensor reading (distance). </span>
}&nbsp;


<span style="color: #CC6600;">int</span> findRoute() {
&nbsp;&nbsp;
&nbsp;&nbsp;PlatformMotors&nbsp;PlatformDrive;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;<span style="color: #7E7E7E;">// Create the object for the PaltformMotoros class.</span>
&nbsp;&nbsp;
&nbsp;&nbsp;PlatformDrive.Halt();&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;<span style="color: #7E7E7E;">// For safety sake stop.</span>
&nbsp;&nbsp;PlatformDrive.Reverse();
&nbsp;<span style="color: #CC6600;">if</span> (!leftClear) {                                                         <span style="color: #7E7E7E;">// If obstacle is on the left turn right.</span>
&nbsp;&nbsp;&nbsp;PlatformDrive.ClockWise();
&nbsp;}
&nbsp;<span style="color: #CC6600;">else</span> {
&nbsp;&nbsp;&nbsp;PlatformDrive.CounterClockWise();&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;<span style="color: #7E7E7E;">// If obstacle is on th eright turn left.</span>
&nbsp;}&nbsp;&nbsp;
&nbsp;
&nbsp;&nbsp;<span style="color: #CC6600;"><b>Serial</b></span>.<span style="color: #CC6600;">println</span>(pathClear);
&nbsp;&nbsp;<span style="color: #CC6600;"><b>Serial</b></span>.<span style="color: #CC6600;">println</span>(leftClear);
&nbsp;&nbsp;<span style="color: #CC6600;"><b>Serial</b></span>.<span style="color: #CC6600;">println</span>(rightClear);
&nbsp;&nbsp;pathClear&nbsp;=&nbsp;&nbsp;<span style="color: #CC6600;">true</span>;                                                        <span style="color: #7E7E7E;">// Reset all of the path variables to true.</span>
&nbsp;&nbsp;leftClear&nbsp;=&nbsp;&nbsp;<span style="color: #CC6600;">true</span>;
&nbsp;&nbsp;rightClear&nbsp;=&nbsp;<span style="color: #CC6600;">true</span>;
&nbsp;&nbsp;
&nbsp;&nbsp;<span style="color: #7E7E7E;">//delay(5000);</span>
&nbsp;&nbsp;<span style="color: #CC6600;">return</span> 0;
}


<span style="color: #CC6600;">int</span> testPath()
{
&nbsp;&nbsp;PlatformMotors&nbsp;PlatformDrive;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;<span style="color: #7E7E7E;">// Create the object for the class PlatformMotors.  </span>
&nbsp;
&nbsp;&nbsp;<span style="color: #CC6600;">if</span> (sensorValue &lt; obstacle &amp;&amp; sensorValue &gt; dropOff)                   <span style="color: #7E7E7E;">// A high sensor value means vertical obstacle, a low sensor value means a dropoff.  </span>
&nbsp;&nbsp;{
&nbsp;&nbsp;&nbsp;&nbsp;<span style="color: #CC6600;"><b>Serial</b></span>.<span style="color: #CC6600;">println</span>(<span style="color: #006699;">"Forward"</span>);                                           <span style="color: #7E7E7E;">// For diagnostic purposes. </span>
&nbsp;&nbsp;&nbsp;&nbsp;pathClear&nbsp;=&nbsp;<span style="color: #CC6600;">true</span>;                                                    <span style="color: #7E7E7E;">// Everything is cool, continue on.</span>
&nbsp;&nbsp;&nbsp;&nbsp;PlatformDrive.Forward();
&nbsp;&nbsp;}
&nbsp;&nbsp;<span style="color: #CC6600;">else</span>
&nbsp;&nbsp;{
&nbsp;&nbsp;&nbsp;&nbsp;<span style="color: #CC6600;"><b>Serial</b></span>.<span style="color: #CC6600;">println</span>(<span style="color: #006699;">"ZOMG We're gonna die!!!!!!!!!!!!!!!"</span>);                <span style="color: #7E7E7E;">// For diagnostic and entertainment purposes.</span>
&nbsp;&nbsp;&nbsp;&nbsp;pathClear&nbsp;=&nbsp;!pathClear;
&nbsp;&nbsp;&nbsp;&nbsp;<span style="color: #CC6600;">if</span> (basePosition &lt; 91)                                                <span style="color: #7E7E7E;">// Determine where the base servo was when the obstacle was detected. </span>
&nbsp;&nbsp;&nbsp;&nbsp;{
&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;leftClear&nbsp;=&nbsp;!leftClear;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;<span style="color: #7E7E7E;">// If it was pointing left then we can't go that way.</span>
&nbsp;&nbsp;&nbsp;&nbsp;}
&nbsp;&nbsp;&nbsp;<span style="color: #CC6600;">else</span>
&nbsp;&nbsp;&nbsp;&nbsp;{&nbsp;
&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;<span style="color: #CC6600;">if</span> (basePosition &gt; 90)                                              <span style="color: #7E7E7E;">// Same as above, onloy opposite </span>
&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;{
&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;rightClear&nbsp;=&nbsp;!rightClear;
&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;}&nbsp;&nbsp;
&nbsp;&nbsp;&nbsp;&nbsp;}
&nbsp;&nbsp;&nbsp;&nbsp;findRoute();&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;<span style="color: #7E7E7E;">// Since there was something somewhere figure out a new way to go.</span>
&nbsp;&nbsp;}
&nbsp;&nbsp;<span style="color: #CC6600;">return</span>(pathClear);                                                      <span style="color: #7E7E7E;">// maybe might need to know this in the main loop in the future.</span>
}&nbsp;&nbsp;
&nbsp;
<span style="color: #CC6600;">void</span> <span style="color: #CC6600;"><b>loop</b></span>()
{
&nbsp;&nbsp;<span style="color: #CC6600;">for</span>(basePosition = 60; basePosition &lt; 145; basePosition += 3)          <span style="color: #7E7E7E;">// goes from 60 degrees to 145 degrees </span>
&nbsp;&nbsp;{&nbsp;&nbsp;&nbsp;&nbsp;
&nbsp;&nbsp;&nbsp;&nbsp;baseServo.<span style="color: #CC6600;">write</span>(basePosition);                                       <span style="color: #7E7E7E;">// tell servo to go to basePositionition in variable 'basePosition' </span>
&nbsp;&nbsp;&nbsp;&nbsp;<span style="color: #CC6600;">delay</span>(5);                                                            <span style="color: #7E7E7E;">// waits for the servo to reach the basePositionition </span>
&nbsp;&nbsp;&nbsp;&nbsp;IRsensorRead();&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;<span style="color: #7E7E7E;">// Get the averaged sensor reading  </span>
&nbsp;&nbsp;&nbsp;&nbsp;testPath();&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;<span style="color: #7E7E7E;">// Make sure the path is clear  </span>
&nbsp;&nbsp;}&nbsp;
&nbsp;&nbsp;<span style="color: #CC6600;">for</span>(basePosition = 145; basePosition&gt;=60; basePosition-= 3)             <span style="color: #7E7E7E;">// goes from 145 degrees to 60 degrees </span>
&nbsp;&nbsp;{&nbsp;
&nbsp;&nbsp;&nbsp;&nbsp;baseServo.<span style="color: #CC6600;">write</span>(basePosition);                                        <span style="color: #7E7E7E;">// tell servo to go to basePositionition in variable 'basePosition' </span>
&nbsp;&nbsp;&nbsp;&nbsp;<span style="color: #CC6600;">delay</span>(5);                                                             <span style="color: #7E7E7E;">// waits for the servo to reach the basePositionition </span>
&nbsp;&nbsp;&nbsp;&nbsp;IRsensorRead();&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;<span style="color: #7E7E7E;">// Get the averaged sensor reading   </span>
&nbsp;&nbsp;&nbsp;&nbsp;testPath();&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;&nbsp;<span style="color: #7E7E7E;">// Make sure the path is clear  </span>
&nbsp;&nbsp;}&nbsp;
}

</pre>

kolnari's picture


#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 
  }
}

birdmun's picture

You did a wonderful job killing the white space that would likely have otherwise appeared in this post. For future reference you might want to look at http://letsmakerobots.com/node/33446. It will help you post your code much more easily, and, you will even maintain the "pretty" highlights that the IDE uses. :)

I wanted to send this as an email, but, you have no contact info available here.