//Robi v1.0
// Control of 2WD-1 robot platform using Arduino UNO.
// with obstacle avoidance using HC-SR04 ultrasonic sensor.
//DISTANCE VARIABLES
const int trigPin = 3;
const int echoPin = 2;
int dist_check1, dist_check2, dist_check3;
long duration, distance;
int dist_result;
//MOTORS VARIABLES
const int mot1f = 5;
const int mot1b = 6;
const int mot2f = 10;
const int mot2b = 11;
int mot_speed = 225; //motors speed
//LOGICS VARIABLES
const int dist_stop = 25;
const int max_range = 800;
const int min_range = 0;
int errorLED = 13;
//INITIALIZATION
void setup() {
  Serial.begin(9600);
  pinMode(trigPin, OUTPUT);
  pinMode(echoPin, INPUT);
  pinMode(errorLED, OUTPUT);
}
//BASIC PROGRAM CYCLE
void loop() {
  autopilot();
}
//***********************FUNCTIONS*******************************
void autopilot() {
  while (true) {
    if (Serial.available() > 0) {
      char command = Serial.read();
      if (command == 'x') {
        motors_stop();
        return;
      }
    }
    int result = ping();  //Check distance
    if (result <= min_range || result >= max_range) {  // Check range
      digitalWrite(errorLED, HIGH);
      delay(500);
    } else if (result < dist_stop) {  // Check stop range
      digitalWrite(errorLED, LOW);
      motors_back();
      delay(1000);
      motors_stop();
      delay(200);
      motors_left();
      delay(300);
      motors_stop();
      delay(200);
    } else {  // If all is OK, go forward
      motors_forward();
      delay(100);
    }
  }
}
int ping() {  //CHECK DISTANCE FUNCTION (3x)
  long sum = 0;
  for (int i = 0; i < 3; i++) {
    digitalWrite(trigPin, LOW);
    delayMicroseconds(2);
    digitalWrite(trigPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(trigPin, LOW);
    duration = pulseIn(echoPin, HIGH);
    distance = duration / 58;
    sum += distance;
  }
  dist_result = sum / 3;
  return dist_result;
}
void motors_forward() {  //MOTORS FORWARD FUNCTION
  analogWrite(mot1f, mot_speed);
  analogWrite(mot2f, mot_speed);
  digitalWrite(mot1b, LOW);
  digitalWrite(mot2b, LOW);
}
void motors_back() {  //MOTORS BACK FUNCTION
  digitalWrite(mot1f, LOW);
  digitalWrite(mot2f, LOW);
  analogWrite(mot1b, mot_speed);
  analogWrite(mot2b, mot_speed);
}
void motors_stop() {  //MOTORS STOP FUNCTION
  digitalWrite(mot1f, LOW);
  digitalWrite(mot2f, LOW);
  digitalWrite(mot1b, LOW);
  digitalWrite(mot2b, LOW);
}
void motors_left() {  //MOTORS LEFT FUNCTION
  analogWrite(mot1f, mot_speed);
  digitalWrite(mot2f, LOW);
  digitalWrite(mot1b, LOW);
  analogWrite(mot2b, mot_speed);
}
void motors_right() {  //MOTORS RIGHT FUNCTION
  digitalWrite(mot1f, LOW);
  analogWrite(mot2f, mot_speed);
  analogWrite(mot1b, mot_speed);
  digitalWrite(mot2b, LOW);
}
void motors_forward_left() {  //FORWARD LEFT FUNCTION
  int k = mot_speed * 0.8;
  analogWrite(mot1f, mot_speed);
  analogWrite(mot2f, k);
  digitalWrite(mot1b, LOW);
  digitalWrite(mot2b, LOW);
}
void motors_forward_right() {  //FORWARD RIGHT FUNCTION
  int k = mot_speed * 0.8;
  analogWrite(mot1f, k);
  analogWrite(mot2f, mot_speed);
  digitalWrite(mot1b, LOW);
  digitalWrite(mot2b, LOW);
}
void motors_back_left() {  //BACK LEFT FUNCTION
  int k = mot_speed * 0.8;
  digitalWrite(mot1f, LOW);
  digitalWrite(mot2f, LOW);
  analogWrite(mot1b, k);
  analogWrite(mot2b, mot_speed);
}
void motors_back_right() {  //BACK RIGHT FUNCTION
  int k = mot_speed * 0.8;
  digitalWrite(mot1f, LOW);
  digitalWrite(mot2f, LOW);
  analogWrite(mot1b, mot_speed);
  analogWrite(mot2b, k);
}