#define SPEED_1       5 //left PWM
#define DIR_1         4 //HIGH to forward
#define SPEED_2       6 //right PWM
#define DIR_2         7 //HIGH to forward
#define gear_sound    70
///#define gear_1        160
#define gear_1        250
#define gear_2        250
#define trigPin_left  3 //HC-SR04 Trig
#define echoPin_left  2 //HC-SR04 Echo
#define trigPin_right 9 //HC-SR04 Trig
#define echoPin_right 8 //HC-SR04 Echo
#define min_distance  35 //in centimetres
#define breaking_rc   '_'
#define forward_rc    'w'
#define back_rc       's'
#define right_rc      'd'
#define left_rc       'a'
#define gear1_rc      '<'
#define gear2_rc      '>'
#define changmod_rc   'm'
#define logging       false
char tmp = breaking_rc;
bool manual = true; //is radio control enable
byte cur_speed = gear_2;
void forward(){
  digitalWrite(DIR_1, HIGH);
  digitalWrite(DIR_2, HIGH);
  analogWrite(SPEED_1, cur_speed);
  analogWrite(SPEED_2, cur_speed);
}
void breaking(){
  analogWrite(SPEED_1, 0);
  analogWrite(SPEED_2, 0);
}
void left(){
  digitalWrite(DIR_1, HIGH);
  digitalWrite(DIR_2, LOW);
  analogWrite(SPEED_1, cur_speed);
  analogWrite(SPEED_2, cur_speed);
}
void right(){
  digitalWrite(DIR_1, LOW);
  digitalWrite(DIR_2, HIGH);
  analogWrite(SPEED_1, cur_speed);
  analogWrite(SPEED_2, cur_speed);
}
void back(){
  digitalWrite(DIR_1, LOW);
  digitalWrite(DIR_2, LOW);
  analogWrite(SPEED_1, cur_speed);
  analogWrite(SPEED_2, cur_speed);
}
float distance_left(){
  digitalWrite(trigPin_left, LOW);
  delayMicroseconds(5); //2-5 ?s
  digitalWrite(trigPin_left, HIGH);
  delayMicroseconds(10); //10 ?s
  digitalWrite(trigPin_left, LOW);
  return pulseIn(echoPin_left, HIGH) / 58.2;
}
float distance_right(){
  digitalWrite(trigPin_right, LOW);
  delayMicroseconds(5); //2-5 ?s
  digitalWrite(trigPin_right, HIGH);
  delayMicroseconds(10); //10 ?s
  digitalWrite(trigPin_right, LOW);
  return pulseIn(echoPin_right, HIGH) / 58.2;
}
void setup() {
  for(int i = 4; i <= 7; i++)
    pinMode(i, OUTPUT);
  pinMode(12, INPUT);
  pinMode(trigPin_left, OUTPUT);
  pinMode(echoPin_left, INPUT);
  pinMode(trigPin_right, OUTPUT);
  pinMode(echoPin_right, INPUT);
  randomSeed(analogRead(0));
  if (logging) {
    Serial.begin(9600);
    while(!Serial);
  }
  Serial2.begin(9600);
  //=
  while(!Serial2);
  if (manual) {
    while (!Serial2.available());
    if (Serial2.read() == 'p') {
      Serial2.write('1');
      //=
      while (Serial2.read() != breaking_rc);
      analogWrite(SPEED_1, gear_sound);
      analogWrite(SPEED_2, gear_sound);
      delay(256);
      analogWrite(SPEED_1, 0);
      analogWrite(SPEED_2, 0);
      delay(128);
      analogWrite(SPEED_1, gear_sound);
      analogWrite(SPEED_2, gear_sound);
      delay(256);
      analogWrite(SPEED_1, 0);
      analogWrite(SPEED_2, 0);
      delay(128);
      analogWrite(SPEED_1, gear_sound);
      analogWrite(SPEED_2, gear_sound);
      delay(256);
      analogWrite(SPEED_1, 0);
      analogWrite(SPEED_2, 0);
      delay(128);
      analogWrite(SPEED_1, gear_sound);
      analogWrite(SPEED_2, gear_sound);
      delay(128);
      analogWrite(SPEED_1, 0);
      analogWrite(SPEED_2, 0);
      delay(64);
      analogWrite(SPEED_1, gear_sound);
      analogWrite(SPEED_2, gear_sound);
      delay(128);
      analogWrite(SPEED_1, 0);
      analogWrite(SPEED_2, 0);
      delay(64);
      analogWrite(SPEED_1, gear_sound);
      analogWrite(SPEED_2, gear_sound);
      delay(128);
      analogWrite(SPEED_1, 0);
      analogWrite(SPEED_2, 0);
    }
  } else  {
    analogWrite(SPEED_1, gear_sound);
    analogWrite(SPEED_2, gear_sound);
    delay(256);
    analogWrite(SPEED_1, 0);
    analogWrite(SPEED_2, 0);
    delay(256);
    analogWrite(SPEED_1, gear_sound);
    analogWrite(SPEED_2, gear_sound);
    delay(512);
    analogWrite(SPEED_1, 0);
    analogWrite(SPEED_2, 0);
    Serial2.write('0');
  }
}
void loop() {
  if (manual) {
    while (Serial2.available()){
      tmp = Serial2.read();
      if (logging) {
        Serial.print(tmp);
        Serial.print(" $ ");
        Serial.print(distance_left());
        Serial.print(" - ");
        Serial.println(distance_right());
      }
      switch (tmp) {
        case forward_rc:
          forward();
          break;
        case back_rc:
          back();
          break;
        case right_rc:
          right();
          break;
        case left_rc:
          left();
          break;
        case gear1_rc:
          cur_speed = gear_1;
          break;
        case gear2_rc:
          cur_speed = gear_2;
          break;
        case changmod_rc:
          cur_speed = gear_1;//чтобы сам ездил медленно
          manual = !manual;
          break;
        default:
          breaking();
          break;
      }
    }
    //breaking();
  } else {
    if (Serial2.read() == changmod_rc)
      manual = !manual;
    if ((distance_left() < min_distance) || (distance_right() < min_distance)) {
      breaking();
      if (distance_left() >= distance_right())
        left();
      else
        right();
      //delay(256);//продолжительность автоповорота
    } else {
      //ran = random();
      forward();
    }
  }
}