Let's Make Robots!

Phase 4.1 - route choices in obstacle avoidance

Nothing new to see externally on the robot, but you can check out all the build pictures over on my photobucket library.

Everything done this time around is tweaking the sketch to make the robot choose a "smart" path, clear of obstacles. Basically, I just split up the ping sweep into two parts, a "scanning mode" to judge forward clearance & set a forward speed to either slow or fast, and a "look around" mode, which tells the robot which way to steer. The only statement in the void loop is "look around". Once it does this, it will determine which way to drive (left or right, backward or forward) and how fast. I have the robot stopping after every command, having executed that command between 1 to 2 seconds in duration (with a delay) depending on the direction and speed, and it seems to pick a smarter route than having it constantly drive. It also saves on battery life, as I feel it has demonstrated that it picks a smarter route than constantly driving about and having to correct on the move.

Here's a link to video if it performing the sketch below:

And here's the current sketch:

#include <AFMotor.h>

#include <Servo.h>

Servo myservo;

 

AF_DCMotor frontMotor(1); //Declaration of Front Motor for M1 of Motor Shield

AF_DCMotor rearMotor(3);  //Declaration of Rear Motor for M3 of Motor Shield

AF_DCMotor steerMotor(2);  //Declaration of Steer Motor for M2 of Motor Shield

 

const int echoPin = A4;  //sets echo to Analog pin 4

const int trigPin = A5; //sets trigger to Analog pin 5

 

int runSpeed = 255, turnSpeed = 255; //Declared speed for the Motor

 

int rightpos = 45; // variable to store the right servo look position 

int midpos = 95; // variable to store the middle servo look position 

int leftpos = 145; // variable to store the left servo look position 

 

int rightDist = 0; // variable to store the right ping distance

int midDist = 0; // variable to store the middle ping distance

int leftDist = 0; // variable to store the left ping distance

 

int distance; //variable to store the ping distance and write to either rightDist, midDist, leftDist for comparison

void setup() {

  // initialize serial communication:

  Serial.begin(9600);

  myservo.attach(9); // attaches the servo on pin 9 to the servo object 

  frontMotor.setSpeed(runSpeed);

  rearMotor.setSpeed(runSpeed);

  steerMotor.setSpeed(turnSpeed);

  frontMotor.run(RELEASE);

  rearMotor.run(RELEASE);

  steerMotor.run(RELEASE); 

}

void loop()

{

  lookAround();

}

 

long microsecondsToInches(long microseconds)

{

  // According to Parallax's datasheet for the PING))), there are

  // 73.746 microseconds per inch (i.e. sound travels at 1130 feet per

  // second).  This gives the distance travelled by the ping, outbound

  // and return, so we divide by 2 to get the distance of the obstacle.

  // See: http://www.parallax.com/dl/docs/prod/acc/28015-PING-v1.3.pdf

  return microseconds / 74 / 2;

}

 

void GoForward()

{

  Serial.println("Planning Forward Speed...");

  steerMotor.run(RELEASE); // stops steering motor

  if (distance <= 36 && distance >= 13)

  {

  AllForwardSlow();

  }

  else if (distance <= 12)

  {

  backupTurn();

  }

  else if (distance >= 37)

  {

  AllForwardFull();

  }

}

 

void AllForwardSlow() 

{

 Serial.println("Going Forward Slowly..."); 

  (runSpeed = 150);

  steerMotor.run(RELEASE); // stops steering motor

  frontMotor.run(FORWARD);

  frontMotor.setSpeed(runSpeed);

  rearMotor.run(FORWARD);

  rearMotor.setSpeed(runSpeed); 

  delay (1000);

}

 

void AllForwardFull() 

{

 Serial.println("Going Forward Full..."); 

  (runSpeed = 255);

  steerMotor.run(RELEASE); // stops steering motor

  frontMotor.run(FORWARD);

  frontMotor.setSpeed(runSpeed);

  rearMotor.run(FORWARD);

  rearMotor.setSpeed(runSpeed); 

  delay (1000);

}

 

void TurnRight()

{

  Serial.println("Turning Right...");

  steerMotor.run(FORWARD);

  steerMotor.setSpeed(turnSpeed);

  frontMotor.run(FORWARD);

  frontMotor.setSpeed(runSpeed);

  rearMotor.run(FORWARD);

  rearMotor.setSpeed(runSpeed);

  delay (1000);

  GoForward();

}

 

void TurnLeft()

{

  Serial.println("Turning Left...");

  steerMotor.run(BACKWARD);

  steerMotor.setSpeed(turnSpeed);

  frontMotor.run(FORWARD);

  frontMotor.setSpeed(runSpeed);

  rearMotor.run(FORWARD);

  rearMotor.setSpeed(runSpeed);

  delay (1000);

  GoForward();

}

 

void backupTurn() 

{

 Serial.println("backing up and turning...");

  (runSpeed = 255);

  steerMotor.run(FORWARD);

  steerMotor.setSpeed(turnSpeed);

  frontMotor.run(BACKWARD);

  frontMotor.setSpeed(runSpeed);

  rearMotor.run(BACKWARD);

  rearMotor.setSpeed(runSpeed);

  delay (1500);

  lookAround();

}

 

void scan()

{

 // establish variables for duration of the ping, 

  // and the distance result in inches and centimeters:

  long duration, inches, cm;

  // The sensor is triggered by a HIGH pulse of 10 or more microseconds.

  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:

  pinMode(trigPin, OUTPUT);

  digitalWrite(trigPin, LOW);

  delayMicroseconds(2);

  digitalWrite(trigPin, HIGH);

  delayMicroseconds(10);

  digitalWrite(trigPin, LOW);

  // Read the signal from the sensor: 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(echoPin, INPUT);

  duration = pulseIn(echoPin, HIGH);

  // convert the time into a distance

  inches = microsecondsToInches(duration);

  distance = inches;

}

 

void lookAround()

    Serial.println("Stopping to Look...");

    frontMotor.run(RELEASE); // stops front motor

    rearMotor.run(RELEASE); // stops rear motor

    steerMotor.run(RELEASE); // stops steering motor

    delay (1000);

    Serial.println("Looking Around...");

    delay (1000);

    myservo.write(rightpos);          // tell servo to go to right position 

    scan();

    rightDist = distance;         // stores the right distance measurement

    Serial.println("Right distance is...");

    Serial.println(rightDist);

    delay(1000);        

    myservo.write(midpos);           // tell servo to go to middle position  

    scan();

    midDist = distance;         // stores the right distance measurement

    Serial.println("Middle distance is...");

    Serial.println(midDist);

    delay(1000);            

    myservo.write(leftpos);          // tell servo to go to left position  

    scan();

    leftDist = distance;         // stores the right distance measurement

    Serial.println("Left distance is...");

    Serial.println(leftDist);

    delay(1000);

    myservo.write(midpos);           // re-center servo to go to middle position

      if ((rightDist > midDist) && (rightDist > leftDist))

    {

      TurnRight();  //TURN RIGHT

    }

    else if ((leftDist > rightDist) && (leftDist > midDist))

    {

      TurnLeft();  //Turn LEFT

    }

    else if ((midDist > rightDist) && (midDist > leftDist))

    {

      GoForward();  //GO FORWARD

    }

    else backupTurn();

}

I feel the autonomous navigation here is acceptible enough to get me out of a signal loss/other emergency while remotely operating, so I won't be pusuing a more optimized path routine for this robot. It is after all, meant primarily for manual control.
End Phase 4.1