Scoutbot V0.1

Well everything is finally wired up and ready to rock and roll. 3 Ultrasonic sensors facing forward, left (45deg) and right (45deg) as well as a forward facing Sharp IR sensor and a rear IR sensor.

The first attempt at some collision code (shown below). It has some issues that need to be addressed but its a starting point. The code looks at the forward ultrasonic and if the value is less than 25cm it compares the left and right sensors for the best way to go. If the front ultrasonic sees a distance of 10cm the robot reverses.

The value of 25cm may not be far enough based on the first fun, or I need to look a little into the breaking code for the motors. The robot can also get stuck in corners quite easily so that needs addressing. I also think the timing of values taken from the ultrasonics may need looking into, as the program goes through each one every loop, when there isn't a need.

I've also started posting at http://letsmakerobots.com with a blog and design ideas

//--------------------------------------------------------- //
//                                                                                                         //
//Scoutbot V0.1                                                                                   //
//                                                                                                        //
//Programmed by Chris83 2012                                                         //
//                                                                                                       //
// Featuring code snippets from Arduino PING Tutorial                      //
// IR Code with help from www.luckylarry.co.uk                                //
//
//---------------------------------------------------------//

//---------------------------------------------
// Includes : Include all header files

#include "DualVNH5019MotorShield.h"

//---------------------------------------------
// Constants : Declare all constants

//Ultrasonics Pin Numbers
const int ultra_cen    = 53;
const int ultra_right  = 51;
const int ultra_left   = 49;

//IR Pin Numbers
const int ir_front     = 15;
const int ir_back       = 14;

//Anaolgue Resolution
const float analogue_5v =  0.0048828125; // (5V / 1024)
const float analogue_3v =  0.0032226563; // (3.3V / 1024)

//---------------------------------------------
// Variables : Declare all global variables

// Setup The motor shield with declaration md
DualVNH5019MotorShield md;

//Ultrasonic distance variables
int center_dist, right_dist, left_dist;

//IR distance variables
int IR_front_dist, IR_back_dist;

//---------------------------------------------
// Functions : Declare all Functions

float get_distance_IR(int IR_Pin)
{
  float volts = analogRead(IR_Pin) * analogue_5v;
  
  float distance = 27*pow(volts, -1.10);
  
  return distance -1;
}

long get_distance_ultra(int ultra_Pin)
{

  // Local Variables for get_distance
  long duration, distance; 
  
  // Sensor Pre-set . Set pin LOW first for a clean signal. Then High to activate Ultrasonic senor
  pinMode(ultra_Pin, OUTPUT);
  digitalWrite(ultra_Pin, LOW);
  delayMicroseconds(2);
  digitalWrite(ultra_Pin, HIGH);
  delayMicroseconds(5);
  digitalWrite(ultra_Pin, LOW);

  // Sensor Center Detect
  pinMode(ultra_Pin, INPUT);
  duration = pulseIn(ultra_Pin, HIGH);
  
  //Convert Duration to CM
  distance = microsecondsToCentimeters(duration);

 //Return distance in CM
 return distance; 
}

long microsecondsToCentimeters(long microseconds)
{
   // The speed of sound is 340 m/s or 29 microseconds per centimeter.
  // The ping travels out and back, so to find the distance of the
  // object we take half of the distance travelled.
  return microseconds / 29 / 2;
} 
//---------------------------------------------
// Setup: 

void setup()
{
  Serial.begin(9600);
  
  md.init(); // Init the Motor Shield md
  
}

//---------------------------------------------
// Main:

void loop()
{
  
 center_dist = get_distance_ultra(ultra_cen);
 right_dist  = get_distance_ultra(ultra_right);
 left_dist   = get_distance_ultra(ultra_left);
 
 IR_front_dist = get_distance_IR(ir_front);
 IR_back_dist = get_distance_IR(ir_back);
 

      
 Serial.print("Center: ");
 Serial.println(center_dist);
 
 Serial.print("Left: ");
 Serial.println(left_dist);
 
 Serial.print("Right: ");
 Serial.println(right_dist);
 
 Serial.print("IR Front: ");
 Serial.println(IR_front_dist);
 
 Serial.print("IR Back: ");
 Serial.println(IR_back_dist);
 
 Serial.println("");
 
 if (center_dist >= 25) // If Center Ultrasonic sees a distance greater than 25cm go forward
   {
      md.setSpeeds (90,90); 
   }
    
 if (center_dist < 25 || IR_front_dist < 25) // If Center Ultrasonic sees a distance less than 25cm slow and compare
   {
      md.setBrakes (0,0);
      
      if (left_dist > right_dist) // Compare left and right ultrasonics. If Left is greater than right, GO LEFT
        {
           md.setSpeeds (-200, 200); // Start motors going LEFT
           delay (1000); // Run motors for 1 sec left
           md.setBrakes (0,0); // Stop motors
        }
        
        if (left_dist < right_dist) // Compare left and right ultrasonics. If Left is less than right, GO RIGHT
          {
           md.setSpeeds (200, -200); // Start motors going RIGHT
           delay (1000); // Run motors for 1 sec left
           md.setBrakes (0,0); // Stop motors
          }      
   }
     
 if (center_dist < 10) // If Center Ultrasonic sees a distance less than 10cm STOP!
   {
     md.setBrakes (0,0); // STOP!
     
     if (IR_back_dist > 25) // If Room to reverse ( greater than 25cm )
     {
       md.setSpeeds (-90, -90); // Go backwards
      delay (1000); //Run Motors for 1 second 
      md.setBrakes (0,0);
     }
     
     if (left_dist > right_dist) // Compare left and right ultrasonics. If Left is greater than right, GO LEFT
        {
           md.setSpeeds (-200, 200); // Start motors going LEFT
           delay (1000); // Run motors for 1 sec left
           md.setBrakes(0,0); // Stop motors
        }
        
        if (left_dist < right_dist) // Compare left and right ultrasonics. If Left is less than right, GO RIGHT
          {
           md.setSpeeds (200, -200); // Start motors going RIGHT
           delay (1000); // Run motors for 1 sec left
           md.setBrakes(0,0); // Stop motors
          }
   }
 delay(1000);  
}