Arduino Obstacle Avoidance Code

//Arduino/Energia Obstacle Avoidance Robot Code, Callum King-Underwood, 17/10/2013, do whatever the hell you want with it

//These are the pins used for the HCSR04
int echo = 2;
int trigger = 3;

//These are the pins for the motor driver, I used an L9110S
int AIA = 10;  //Motor A, input A
int AIB = 9;   //Motor A, input B
int BIA = 8;   //Motor B, input A
int BIB = 7;   //Motor B, input B

//Variables for the distance measurements
int distance;  //This you leave alone, the program will fill it as required.
int dangerLevel = 50;//this is in cm, the distance at which to avoid an obstacle.
void setup()
{
  // set the motor driver pins to output
  pinMode(AIA, OUTPUT);
  pinMode(AIB, OUTPUT);
  pinMode(BIA, OUTPUT);
  pinMode(BIB, OUTPUT);
  //set the motor driver pins high to start with.
  //both pins being high results in the motor not spinning, as does both low, I don't think it matters which you use.
  digitalWrite(AIA, HIGH);
  digitalWrite(AIB, HIGH);
  digitalWrite(BIA, HIGH);
  digitalWrite(BIB, HIGH);

  //set the ultrasound pins up too
  pinMode(echo, INPUT);
  pinMode(trigger, OUTPUT);
  digitalWrite(trigger, LOW);

  Serial.begin(9600);
}

void loop()
{
  //Send a short pulse on HCSR04 trigger
  digitalWrite(trigger, HIGH);
  delay(1);
  digitalWrite(trigger, LOW);

  //record the pulse from the echo pin
  distance = pulseIn(echo, HIGH);

  //dividing it by 58 roughly converts to cm, tweak this value if you wish.
  distance = distance / 58;
  //debug feature: print out the distance over serial.
  Serial.println(distance);
  if (distance > dangerLevel){
    //there is no obstacle too close to the robot. We will go forwards.
    //you may need to rewire the motor or swap a low for a high and vice versa to get the correct direction for your robot
    digitalWrite(AIA, LOW);
    digitalWrite(AIB, HIGH);
    digitalWrite(BIA, LOW);
    digitalWrite(BIB, HIGH);
  }
  else{
    //there is an obstacle, so one motor will go forwards and the other backwards.
    digitalWrite(AIA, LOW);
    digitalWrite(AIB, HIGH);
    digitalWrite(BIA, HIGH);
    digitalWrite(BIB, LOW);
    //Confirm that we are avoiding an obstacle over serial.
    Serial.println("Danger Hit");
  }
}