int pwm_r = 5;  //PWM control for motor outputs 1 and 2 is on digital pin 5 RIGHT
int pwm_l = 11;  //PWM control for motor outputs 3 and 4 is on digital pin 11 LEFT
int dir_r = 12;  //dir control for motor outputs 1 and 2 is on digital pin 12 RIGHT Low is forward
int dir_l = 13;  //dir control for motor outputs 3 and 4 is on digital pin 13 LEFT High is forward
int triga = 10; //Trigger for the Front Sensor
int trigb = 8; //Trigger for the Right Sensor
int echoa = 9; // Echo for the Front Sensor
int echob = 7; // Echo for the Right Sensor
long sonica = 0; //Sonic a intermediate distance
long sonicb = 0;// sonic b intermediate distance
volatile int counterL = 0; // Counter for the Left Wheel encoder
volatile int counterR = 0; // Counter for the Right Wheel encoder
int Stop = 0;
int full = 255;
int half = 128;
int quart = 64;

void setup()
{
  attachInterrupt(0, AddL, CHANGE); // Intettupt for the Left Wheel encoder
  attachInterrupt(1, AddR, CHANGE); // Interrupt for the Right Wheel encoder
  pinMode(pwm_r, OUTPUT);  //Set control PWM pins to be outputs (Speed variance)
  pinMode(pwm_l, OUTPUT);
  pinMode(dir_r, OUTPUT); // Set the direction pins to Output (high or low)
  pinMode(dir_l, OUTPUT);
  Serial.begin(9600); //Opens up a serial port for command rx and data tx
  pinMode(triga, OUTPUT); // Set the trigger pins to Output (High, Low, High)
  pinMode(trigb, OUTPUT);
  pinMode(echoa, INPUT); // Set the echo pins to Input (pulseIn() command usage
  pinMode(echob, INPUT);
}
void AddL(){ // Addition for the left wheel
 counterL++;
}
void AddR(){ //Addition for the right wheel
 counterR++;
}
void loop()
{
  sonica = Sonic(triga,echoa); // Collect a sample from the front sensor
  sonicb = Sonic(trigb,echob); // Collect a sample from the right sensor

  if(sonica <30 || sonicb < 14){ // priority loop, the robot CANNOT crash forwards
       SetWheel(pwm_r, half, dir_r, HIGH); // If the robot gets too close to colliding with
       SetWheel(pwm_l, Stop, dir_l, LOW); // a wall OR it gets too close to a wall on the
  }
  else if(sonicb >= 14 && sonicb <= 20) { // If the Robot is within
       SetWheel(pwm_r, half, dir_r, HIGH); // This 6cm distance, it will move forward until
       SetWheel(pwm_l, half, dir_l, LOW);  // it falls out of the range.
  }
  else if(sonicb > 20 && sonica >30){
       SetWheel(pwm_l, half, dir_l,LOW); // if the sensor gets too far from the wall
       SetWheel(pwm_r, Stop, dir_r, HIGH); // on the right side and it is not about to collide
       // with a wall on the front, turn Left to compensate
  }
       delay(40);
       Serial.print(counterR);
       Serial.print(",");
       Serial.println(counterL);
       counterR = 0;
       counterL = 0;
 }
// Methods to control the motors
void SetWheel( int pwmpin, int vel, int dirpin, boolean dir){
  analogWrite(pwmpin, vel);
  digitalWrite(dirpin, dir);
}
//Method to read/write data to and from the sonic sensors, The below is setup per the HC-SR04 Datasheet
long Sonic(int a, int b){
long duration, cm;
  digitalWrite(a, LOW);
  delayMicroseconds(2);
  digitalWrite(a, HIGH);
  delayMicroseconds(5);
  digitalWrite(a, LOW);
  duration = pulseIn(b, HIGH);
  delay(10);
  cm = (duration/29) /2;
  return cm;
  }