twitter
Search
Wednesday
Feb012012

2WD Roving Robot

This is a simple robot using an Adafruit Motor Shield, Arduino Uno, and two motors. The video shows off the sensors and arrangements. The source code is below:

/*
Autonomous roving robot
Parts: 
   -Arduino Uno
   -Parallax PING))) ultrasonic sensor
   -Adafruit motor shield http://www.ladyada.net/make/mshield/
   -2WD Mobile Platform http://www.makershed.com/ProductDetails.asp?ProductCode=MKSEEED7
   -Lever switch (for back bumper)
   -Indicator LED
   -Energizer XPAL XP8000 battery (custom cable and self-built regulator for motors)
Original motion functions and design from Robot Living 8/30/2010 
Code borrowed from example sketches:
   -Ping
Original code created June 2011
by Daniel Gentleman
thoughtfix@gmail.com
http://thoughtfix.com

Additional credits: 
Victor Brilon
Mark Balliet
luckylarry.co.uk for the math on the Sharp sensor
Advice from arduino.cc forum
Advice from from #arduino on irc.freenode.net
Advice from adafruit forums
Further inspiration
      -Adafruit Industries
      -Make Magazine
Adafruit Motor shield library

Wiring (remember, analog 0 is also digital 14) 
- Left motor to Motor 1
- Right motor to Motor 2
- LEDs on digital 15 and 16
- PING))) on digital 17
- Sharp IR sensors on digital 18 and 19
- Lever switch (bumper attached) on digital 14
- USB power (from a USB battery) to the Arduino
- Regulated power (I used an Energizer XPAL XP8000 regulated to +5v) for servos
*/


#include <AFMotor.h>

AF_DCMotor motorL(1, MOTOR12_1KHZ);
AF_DCMotor motorR(2, MOTOR12_1KHZ);


int BSensor = 14; //Back sensor
int FLEDPin = 15; //Forward LED
int BLEDPin = 16; //Backup LED
int pingPin = 17; //Front sensor
int FSensorR = 18; //Front right sensor
int FSensorL = 19;  //Front left sensor
int count = 0;
int b=0;
int bs=0;
int sensorValue;
int forwardSensor;
int sensorValueB=1;
int leftSensor=0;
int rightSensor=0;
int obstacle=0;


void setup() {
  Serial.begin(9600); 
  randomSeed(analogRead(0));
  pinMode(BLEDPin, OUTPUT); 
  pinMode(FLEDPin, OUTPUT);
  pinMode(BSensor, INPUT);
  pinMode(pingPin, INPUT);
  pinMode(FSensorR, INPUT);
  pinMode(FSensorL, INPUT);
  motorL.setSpeed(200);
  motorR.setSpeed(200);
  motorL.run(RELEASE);
  motorR.run(RELEASE);
}

void loop()
{
  uint8_t i;
  Forward();
}

void Forward () // Charge forth!
{
  digitalWrite(FLEDPin, HIGH);                
  digitalWrite(BLEDPin, LOW);
  ForwardSensor ();
  if (obstacle == 0){ //unless something is in the way
  Serial.println("Going forward.");
  motorL.run(FORWARD);
  motorR.run(FORWARD);
  }

}

void ForwardSensor ()
{
  leftSensor = (ForwardSensorLeft());
  rightSensor =(ForwardSensorRight());
  forwardSensor = (getDistance());
  delay(30);
  //  Serial.print(leftSensor);
  //  Serial.println(" left");
  //  Serial.print(rightSensor);
  //  Serial.println(" right");
  //  Serial.print(forwardSensor);
  //  Serial.println(" front");
  if (forwardSensor <=30 || leftSensor <= 30 || rightSensor <= 30 )
  { // I gave 10cm of "wiggle room" so it doesn't turn endlessly. 
    obstacle=1;
    if (ForwardSensorRight() > (ForwardSensorLeft()-10)){
      RightBackward();
    }
    else if (ForwardSensorRight() < (ForwardSensorLeft()-10)){
      LeftBackward();
    }
    else{
      Backward();
    }
  }
  else {
    obstacle = 0;
  }
  sensorValue=0;
  rightSensor=0;
  leftSensor=0;
}

int ForwardSensorRight () 
{ // This function is here so you don't have to re-write code
  // if you use a different sensor.
  sensorValue = irDistance(FSensorR); 
  return sensorValue;
}

int ForwardSensorLeft ()
{ // This function is here so you don't have to re-write code
  // if you use a different sensor.
  sensorValue = irDistance(FSensorL); 
  return sensorValue;
}

void BackwardSensor ()
{
  sensorValueB = digitalRead(BSensor); 
  if (sensorValueB == 1)
  {
    Serial.println("Object detected while going backwards.");
    motorL.run(RELEASE);
    motorR.run(RELEASE);
    for (bs=0; bs <= 3; bs++) // Add blinky lights for personality on detecting object when backing up.
    {
      digitalWrite(FLEDPin, HIGH);                
      digitalWrite(BLEDPin, HIGH);
      delay(5);
      digitalWrite(FLEDPin, LOW);                
      digitalWrite(BLEDPin, LOW);
      delay (5);
    }
    b=31;
    bs=0;
    Forward();

  }
  sensorValueB = 0;
}

void Backward ()
{
  motorL.run(RELEASE);
  motorR.run(RELEASE);
  digitalWrite(FLEDPin, LOW);                
  digitalWrite(BLEDPin, HIGH);
  Serial.println("Going backwards.");

  do
  {
    b++;
    motorL.run(BACKWARD);
    motorR.run(BACKWARD);
    delay(20);
    BackwardSensor ();
  } 
  while (b < 20);

  b=0;
}


void LeftBackward ()
{
  motorL.run(RELEASE);
  motorR.run(RELEASE);
  digitalWrite(FLEDPin, LOW);                
  digitalWrite(BLEDPin, HIGH);
  Serial.println("Going left backwards.");

  do
  {
    b++;
    //    Serial.print("b = ");
    //    Serial.print(b);
    motorL.run(BACKWARD);
    motorR.run(RELEASE);
    BackwardSensor ();
    delay (20);
  } 
  while (b < 20);

  b=0;
}

void RightBackward ()
{
  motorL.run(RELEASE);
  motorR.run(RELEASE);
  digitalWrite(FLEDPin, LOW);                
  digitalWrite(BLEDPin, HIGH);
  Serial.println("Going right backwards.");

  do
  {
    b++;
    //    Serial.print("b = ");
    //    Serial.print(b);
    motorL.run(RELEASE);
    motorR.run(BACKWARD);
    BackwardSensor ();
    delay (20);
  } 
  while (b < 20);

  b=0;
}

int getDistance()

  // The PING))) is triggered by a HIGH pulse of 2 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
reread:  // takes another reading if cm=0
  long duration, cm;
  pinMode(pingPin, OUTPUT);
  digitalWrite(pingPin, LOW);
  delayMicroseconds(2);
  digitalWrite(pingPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(pingPin, LOW);
  // The same pin is used to read the signal from the PING))): a HIGH
  // pulse whose duration is the time (in microseconds) from the sending
  // of the ping to the reception of its echo off of an object.
  pinMode(pingPin, INPUT);
  duration = pulseIn(pingPin, HIGH);

  // convert the time into a distance
  cm = microsecondsToCentimeters(duration);
  //  Serial.print(cm);
  //  Serial.print("cm");
  //  Serial.println();
  delay(100);
  //  if (cm == 0) {
  //    goto reread;
  //  }
  return cm;


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/58);
}

int irDistance(int irPin) {
  float volts = analogRead(irPin)*0.0048828125;   // value from sensor * (5/1024) - if running 3.3.volts then change 5 to 3.3
  int distance = 32.5*pow(volts, -1.10);          // theretical distance 32.5/ (1/Volts)S 
  //  Serial.print(distance);
  //  Serial.print(" from ");
  //  Serial.println(irPin);  
  delay (10);
  return distance;        // print the raw analog value to serial port
}

PrintView Printer Friendly Version

EmailEmail Article to Friend

Reader Comments (1)

Hello, great little set up! Trying something similar myself as a first robot. Could you upload the wiring diagram?

Thanks,
Der
November 23, 2012 | Unregistered Commenterder

PostPost a New Comment

Enter your information below to add a new comment.

My response is on my own website »
Author Email (optional):
Author URL (optional):
Post:
 
All HTML will be escaped. Hyperlinks will be created for URLs automatically.
« Laser ToyBot for Cats | Main | Twitter-posting Kitty Dishes »