//Feel free to use this code.
//Please be respectful by acknowledging the author in the code if you use or modify it.
//Original Author: Bruce Allen
//Modified by: Jason Edwards
//Date: 1/12/10


const int DigiIR = 2; //This is where the IR pin is located (const)
// Following lines of code set the speed(pwm) and the direction(dir)
const int pwm_a=3;
const int pwm_b = 11;
const int dir_a = 12;
const int dir_b = 13;
const long SamplePoint = 489/10000;//this constant is variable that adjusts the range accordingly
//variables needed to store values
long pulse, inches, cm;
int object = 0; // variable to store digital value
int analogRange = 0;
long range, IRinches, IRcm;

void setup() {

  pinMode(pwm_a, OUTPUT);  //Set control pins to be outputs
  pinMode(pwm_b, OUTPUT);
  pinMode(dir_a, OUTPUT);
  pinMode(dir_b, OUTPUT);

  //This opens up a serial connection to shoot the results back to the PC console
  Serial.begin(9600);


}

void loop() {


  pinMode(DigiIR, INPUT);

    //Used to read in the pulse that is being sent by the MaxSonar device.
  //Pulse Width representation with a scale factor of 147 uS per Inch.

  range = analogRead(analogRange);
  object = digitalRead(DigiIR);
  pulse = pulseIn(pwPin, HIGH);
  analogWrite(pwm_a, 200);
  analogWrite(pwm_b, 200);

  if(object == HIGH){
    Serial.println("Path Clear");
    digitalWrite(dir_a, HIGH);  //Set motor direction, 1 low, 2 high
    digitalWrite(dir_b, HIGH);
  }
  else{
     Serial.println("Object Detected");
     digitalWrite(dir_a, LOW); //Set motor direction, 1 low, 2 high
     digitalWrite(dir_b, LOW);
  }
  delay(500);
}