Arduino Obstacle avoidance with analog motors

I seem to have this working for the most part. I need to do some fine tuning, but should have it running pretty effectively shortly.

Da Code

const int pingPin = 7; // pin for ping input

const int dangerThresh = 10; //threshold for obstacles (in cm)
int leftDistance, rightDistance; //distances on either side
long duration; //time it takes to recieve PING))) signal


void setup() {
   // initialize serial communication:
  Serial.begin(9600);
 
   //Setup Channel A
  pinMode(12, OUTPUT); //Initiates Motor Channel A pin
  pinMode(9, OUTPUT); //Initiates Brake Channel A pin

  //Setup Channel B
  pinMode(13, OUTPUT); //Initiates Motor Channel B pin
  pinMode(8, OUTPUT);  //Initiates Brake Channel B pin
}

void loop()
{
  int distanceFwd = ping(); //set distance ahead to ping distance
  if (distanceFwd > dangerThresh) //if path is clear
  {
   //MOVE FORWARD
   //Motor A forward
  digitalWrite(12, HIGH); //Establishes forward direction of Channel A
  digitalWrite(9, LOW);   //Disengage the Brake for Channel A
  analogWrite(3, 255);   //Spins the motor on Channel A
 
    //Motor B Forward
  digitalWrite(13, HIGH);  //Establishes backward direction of Channel B
  digitalWrite(8, LOW);   //Disengage the Brake for Channel B
  analogWrite(11, 255);    //Spins the motor on Channel B
  }
  else //if path is blocked
  {
   digitalWrite(9, HIGH);  //Engage the Brake for Channel A
   digitalWrite(8, HIGH);  //Engage the Brake for Channel B
   delay(1000);
   //MOVE BACK TURNING FIRST WAY
   //Motor A Backwards
   digitalWrite(12, LOW); //Establishes backward direction of Channel A
   digitalWrite(9, LOW);   //Disengage the Brake for Channel A
   analogWrite(3, 175);   //Spins the motor on Channel A
   delay(1000);
   rightDistance = ping(); //scan to the right
   delay(1000);
   digitalWrite(9, HIGH);  //Engage the Brake for Channel A
   digitalWrite(8, HIGH);  //Engage the Brake for Channel B
   delay(1000);
   //return to center
   //Motor B backwards
   digitalWrite(13, LOW);  //Establishes backward direction of Channel B
   digitalWrite(8, LOW);   //Disengage the Brake for Channel B
   analogWrite(11, 175);    //Spins the motor on Channel B
   delay(1000);
 
   digitalWrite(9, HIGH);  //Engage the Brake for Channel A
   digitalWrite(8, HIGH);  //Engage the Brake for Channel B
   delay(1000);
 
   //MOVE BACK TURNING SECOND WAY
   //Motor B Backwards
   digitalWrite(13, LOW);  //Establishes backward direction of Channel B
   digitalWrite(8, LOW);   //Disengage the Brake for Channel B
   analogWrite(11, 175);    //Spins the motor on Channel B
   delay(1000);
 
   digitalWrite(9, HIGH);  //Engage the Brake for Channel A
   digitalWrite(8, HIGH);  //Engage the Brake for Channel B
   delay(1000);
 
   leftDistance = ping(); //scan to the left
   delay(1000);
   //RETURN TO CENTER
   //Motor A forward
   digitalWrite(12, LOW); //Establishes backward direction of Channel A
   digitalWrite(9, LOW);   //Disengage the Brake for Channel A
   analogWrite(3, 175);   //Spins the motor on Channel A
   delay(1000);
   compareDistance();
  }
}

  void compareDistance()
 {
   if (leftDistance>rightDistance) //if left is less obstructed
  {
   digitalWrite(9, HIGH);  //Engage the Brake for Channel A
   digitalWrite(8, HIGH);  //Engage the Brake for Channel B
   delay(1000);
  //Motor A backwards
    digitalWrite(12, LOW); //Establishes backward direction of Channel A
    digitalWrite(9, LOW);   //Disengage the Brake for Channel A
    analogWrite(3, 100);   //Spins the motor on Channel A
    delay(2000);
  }
   else if (rightDistance>leftDistance) //if right is less obstructed
  {
   digitalWrite(9, HIGH);  //Engage the Brake for Channel A
   digitalWrite(8, HIGH);  //Engage the Brake for Channel B
   delay(1000);
    //Motor B backwards
    digitalWrite(13, LOW);  //Establishes backward direction of Channel B
    digitalWrite(8, LOW);   //Disengage the Brake for Channel B
    analogWrite(11, 100);    //Spins the motor on Channel B
    delay(2000);
  }
   else //if they are equally obstructed
  {
   digitalWrite(9, HIGH);  //Engage the Brake for Channel A
   digitalWrite(8, HIGH);  //Engage the Brake for Channel B
   delay(1000);
   //Motor A forward
   digitalWrite(12, LOW); //Establishes backward direction of Channel A
   digitalWrite(9, LOW);   //Disengage the Brake for Channel A
   analogWrite(3, 100);   //Spins the motor on Channel A
   //Motor B Forward
   digitalWrite(13, HIGH);  //Establishes backward direction of Channel B
   digitalWrite(8, LOW);   //Disengage the Brake for Channel B
   analogWrite(11, 100);    //Spins the motor on Channel B
   delay(2000);
  }
 }
 // establish variables for duration of the ping,
 // and the distance result in inches and centimeters:
long ping()
{
  // 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:
  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);

  // 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 duration / 29 / 2;

Parallax Rangefinder output to Parallax 2x16 Serial LCD

Da Code:

My Code:

const int TxPin = 2;

const int pingPin = 7;

#include <SoftwareSerial.h>

SoftwareSerial mySerial = SoftwareSerial(255, TxPin);

void setup() {

  // initialize serial communication:

    

  pinMode(TxPin, OUTPUT);

  digitalWrite(TxPin, HIGH);

  

  mySerial.begin(9600);

  delay(100);

  mySerial.write(12);                 // Clear             

  mySerial.write(17);                 // Turn backlight on

  delay(5);                           // Required delay

  mySerial.print("ShawnRobot 1.0");  // First line

  mySerial.write(13);                 // Form feed

  delay(1000);

  mySerial.print("second line test");   // Second line

  mySerial.write(212);                // Quarter note

  mySerial.write(220);                // A tone

  delay(3000);                        // Wait 3 seconds

  mySerial.write(18);                 // Turn backlight off

  delay(100);

  mySerial.write(12);                 // Clear

}

void loop() 

{

  // establish variables for duration of the ping, 

  // and the distance result in inches and centimeters:

  long duration, inches, cm;

  

  // 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:

  

  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

  inches = microsecondsToInches(duration);

  cm = microsecondsToCentimeters(duration);

  

  mySerial.write(12);                 // Clear

  mySerial.write(17);                // Turn backlight on

  mySerial.print(inches);

  mySerial.print("in ");

  mySerial.write(13);

  mySerial.print(cm);

  mySerial.print("cm ");

  mySerial.println();

  

  delay(500);

}

  

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;

}

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;

}

First Foray into Arduino Robotics.

Going to start posting the Robotics Adventures of the Murphy Family here.

Enjoy.

Built this so Far:

Now have the code finished to make the rangefinder output to the LCD. 

\