#define ENA 3 //must be PWM (~)
#define IN1 4
#define IN2 5
#define IN3 6
#define IN4 7
#define ENB 9 //must be PWM (~)
#define tr1 10
#define ec1 11
#define tr2 12
#define ec2 13

byte com = 0; //reply from voice recognition
long duration1, duration2;
int distance1, distance2;
int f = 0;
int b = 0;

void setup() {
  Serial.begin(9600);
  pinMode(ENA, OUTPUT);
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
  pinMode(ENB, OUTPUT);
  pinMode(tr1, OUTPUT);
  pinMode(ec1, INPUT);
  pinMode(tr2, OUTPUT);
  pinMode(ec2, INPUT);
  analogWrite(ENA, 100); //0-255 80
  analogWrite(ENB, 100); //0-255 80
  stopp();
  delay(2000);
  Serial.write(0xAA);
  Serial.write(0x37);//Switch to Compact Mode
  delay(1000);
  Serial.write(0xAA);
  Serial.write(0x23);//Import group 1 and be ready for voice instruction
}

void loop() {
  /*
    US1();
    US2();
    front();
    delay(1000);
    back();
    delay(1000);
    left();
    delay(1000);
    right();
    delay(1000);
    stopp();
    delay(1000);
  */
  US1();
  US2();
  if (f == 1 && distance1 < 10) {
    stopp();
    delay(1000);
    back();
    delay(1000);
    stopp();
    f = 0;
  }
  if (b == 1 && distance2 < 10) {
    stopp();
    delay(1000);
    front();
    delay(1000);
    stopp();
    b = 0;
  }

  while (Serial.available())
  {
    com = Serial.read();

    switch (com)
    {
      case 0x11:
        //front
        f = 1;
        if (f == 1 && distance1 < 10) {
          stopp();
          delay(1000);
          back();
          delay(1000);
          stopp();
          f = 0;
        }
        else if (f == 1 && distance1 > 10) {
          front();
          //delay(2000);
        }
        break;
      case 0x12:
        //back
        b = 1;
        if (b == 1 && distance2 < 10) {
          stopp();
          delay(1000);
          front();
          delay(1000);
          stopp();
          b = 0;
        }
        else if (b == 1 && distance2 > 10) {
          back();
          //delay(2000);
        }
        break;
      case 0x13:
        // right
        right();
        delay(1000);
        stopp();
        break;
      case 0x14:
        // left
        left();
        delay(1000);
        stopp();
        break;
      case 0x15:
        // stop
        stopp();
        break;
    }// end of SWITCH
  }

}// end of LOOP

void front() {
  digitalWrite(IN1, 1);
  digitalWrite(IN2, 0);
  digitalWrite(IN3, 1);
  digitalWrite(IN4, 0);
}

void back() {
  digitalWrite(IN1, 0);
  digitalWrite(IN2, 1);
  digitalWrite(IN3, 0);
  digitalWrite(IN4, 1);
}

void left() {
  digitalWrite(IN1, 1);
  digitalWrite(IN2, 0);
  digitalWrite(IN3, 0);
  digitalWrite(IN4, 1);
}

void right() {
  digitalWrite(IN1, 0);
  digitalWrite(IN2, 1);
  digitalWrite(IN3, 1);
  digitalWrite(IN4, 0);
}

void stopp() {
  digitalWrite(IN1, 0);
  digitalWrite(IN2, 0);
  digitalWrite(IN3, 0);
  digitalWrite(IN4, 0);
}

void US1() {
  digitalWrite(tr1, LOW);
  delayMicroseconds(5);
  digitalWrite(tr1, HIGH);
  delayMicroseconds(10);
  digitalWrite(tr1, LOW);
  duration1 = pulseIn(ec1, HIGH);
  distance1 = duration1 * 0.034 / 2;
  //Serial.print("Distance1 = ");
  //Serial.print(distance1);
  //Serial.println(" cm\t\t");
}

void US2() {
  digitalWrite(tr2, LOW);
  delayMicroseconds(5);
  digitalWrite(tr2, HIGH);
  delayMicroseconds(10);
  digitalWrite(tr2, LOW);
  duration2 = pulseIn(ec2, HIGH);
  distance2 = duration2 * 0.034 / 2;
  // Serial.print("Distance2 = ");
  //  Serial.print(distance2);
  //  Serial.println(" cm");
}