//************************************************************
/*
 * Code for robotic car using Arduino compatible controller
 * 2016 - 07 - 17 Original Release
 * 2018 - 09 - 22 Revision 2
 * Plevrakis Theophilos
 * tplevrak@tplevrak.gr
 * 
 * originaly posted on 
 * blog.astronomy.gr
 * http://astro.planitario.gr/ardu-robocar-01/
 * 
 * 
 * 
 */

 
// Global Variables declaration

long CurrentDistance;

long MinDistance = 20;  // Minimum distance for the car to perceive the presence of obstacle

int CarRunning;

int TurningDelay = 750; // How long will the robot turn in plain turn mode
int TurningDelaySharp = 750; // How long will the robot turn in Sharp turn mode
int TurningAgain = 0; // Set what kind of avoidance turn will it make
  
int DSPin01 = 7;    // Distance Sensor Pin 1
int DSPin02 = 6;    // Distance Sensor Pin 2 

// led pins
int ledpin_red_right = 3;
int ledpin_red_left = 4;
int ledpin_blue_right = 2;
int ledpin_blue_left = 5;

// motor pins
int leftmotorpin1 = 11;
int leftmotorpin2 = 10;
int rightmotorpin1 = 9;
int rightmotorpin2 = 8;

//***********************************************************************

void setup() {
  
  pinMode(13, OUTPUT);    // just used to turn the Pin 13 led off
  digitalWrite(13, LOW);

  // led pins
  pinMode(ledpin_red_right, OUTPUT);
  pinMode(ledpin_red_left, OUTPUT); 
  pinMode(ledpin_blue_right, OUTPUT);
  pinMode(ledpin_blue_left, OUTPUT);   

  //motor pins   
  pinMode(leftmotorpin1, OUTPUT);
  pinMode(leftmotorpin2, OUTPUT);
  pinMode(rightmotorpin1, OUTPUT);
  pinMode(rightmotorpin2, OUTPUT);
  
  // Distance Sensor
  pinMode(DSPin02, INPUT);
  pinMode(DSPin01, OUTPUT);  
  
  digitalWrite(13, LOW);
  digitalWrite(rightmotorpin1, LOW);
  digitalWrite(rightmotorpin2, LOW);
  digitalWrite(leftmotorpin1, LOW);
  digitalWrite(leftmotorpin2, LOW); 
  digitalWrite(ledpin_red_right, HIGH);
  digitalWrite(ledpin_red_left, HIGH);  
  digitalWrite(ledpin_blue_right, LOW);
  digitalWrite(ledpin_blue_left, LOW);  
 
  Serial.begin(9600);

  teodelay(2000);

  pause_blinking(2000,8);
  
}

//***********************************************************************

void loop() {

MainCarRoutine(-1);

}

//***********************************************************************

void MainCarRoutine (int UseAvoidOstacle) {

  // Main routine that runs the show

  if (UseAvoidOstacle==-1) {
      CarRunning = AvoidObstacle();
  } 
  else {
      CarRunning=1;
  }

  
  Serial.print("CarRunning = ");  
  Serial.print(CarRunning);
  Serial.println();

  // teodelay(2000);
  
  if (CarRunning == 1) {
    StartCar();
  } 
  
}

int AvoidObstacle () {


  // Checking the distance and initiating obstacle avoidance
  // if distance is less than MinDistance
  
    int OResult=1;
  
    CurrentDistance = DistanceMeasurement();
    
    if (CurrentDistance <= MinDistance) {
      OResult=0;
      StopCar();
      teodelay(500);
      BackCar();
      teodelay(500);
      AvoidObstacleTurn();
    }
    else if (CurrentDistance==0) {
      OResult=1;
    }
    
  return OResult;
  
}


void AvoidObstacleTurn () {

  // Function to choose what kind of turn is going to be used to avoid obstacle
  // It is necessary because by using only one kind of turning the car maybe
  // stuck


  if (TurningAgain == 0) {
    TurnCar("RIGHT", "SHARP", TurningDelaySharp);
    TurningAgain = 1;
    
      Serial.print(" Maneuver 0 : ");
      Serial.print(" Sharp Right Normal ");      
      Serial.println();     
  }
  else if (TurningAgain == 1) {
    TurnCar("LEFT", "SHARP", TurningDelaySharp);
    TurningAgain = 2; 
    
      Serial.print(" Maneuver 1 : ");
      Serial.print(" Sharp Left Normal ");      
      Serial.println();     
  }
  else if (TurningAgain == 2) {
    TurnCar("RIGHT", "SHARP", (TurningDelaySharp / 2));
    TurningAgain = 3; 

      Serial.print(" Maneuver 2 : ");
      Serial.print(" Sharp Right Fast  ");      
      Serial.println();    
  }  
  else if (TurningAgain == 3) {
    TurnCar("LEFT", "SHARP", (TurningDelaySharp / 2));
    TurningAgain = 4; 

      Serial.print(" Maneuver 3 : ");
      Serial.print(" Sharp Left Fast  ");      
      Serial.println();     
  }
  else {
    TurnCar("LEFT", "PLAIN", (TurningDelaySharp));
    TurningAgain = 0;

      Serial.print(" Maneuver Generic : ");
      Serial.print(" Plain Left Normal  ");      
      Serial.println();     
  }

}
//***********************************************************************


long DistanceMeasurement (){


  // Return a distance measuring  
  return SensorMeasuring(DSPin01,DSPin02);
  
}


long SensorMeasuring(int trigPin, int echoPin) {

  // Actually using the sensor to acquire a reading
  // It can be used to get data from more than one sensors
  // and ca easily be adapted to return inches of cm.

  long duration, cm, inches;
  
  // The sensor is triggered by a HIGH pulse of 10 or more microseconds.
  // Give a short LOW pulse beforehand to ensure a clean HIGH pulse:
  digitalWrite(trigPin, LOW);
  delayMicroseconds(5);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
 
  // Read the signal from the sensor: a HIGH pulse whose
  // duration is the time (in microseconds) from the sending
  // of the ping to the reception of its echo off of an object.
  pinMode(echoPin, INPUT);
  duration = pulseIn(echoPin, HIGH);
 
  // convert the time into a distance
  cm = (duration/2) / 29.1;
  inches = (duration/2) / 74; 
  
  Serial.print(inches);
  Serial.print(" in, ");
  Serial.print(cm);

  if (trigPin==12) {
    Serial.print(" cm Height");
  }
  else {
    Serial.print(" cm Distance");
  }
  Serial.println();
  
  // teodelay(2000);

  return cm;
  
}


void StartCar() {

  // Car moving forward
      motor_right("ON", "FRONT");
      motor_left("ON", "FRONT");

      Serial.print(" Forward the Car ");
      Serial.println(); 
      
}

void BackCar() {

  // Car moving backward      
      motor_right("ON", "BACK");
      motor_left("ON", "BACK");
      
      Serial.print(" Reverse the Car ");
      Serial.println();       
   
}

void StopCar() {

  // Car stops  
      motor_right("OFF", "BACK");   
      motor_left("OFF", "BACK"); 

      Serial.print(" Stop the Car ");
      Serial.println();      
  
} 

void TurnCar(char LEFTRIGHT[], char SHARP[], int TurnDuration) {


  // Car turns by user choice of
  // direction, LEFT or RIGHT
  // sharp turn, SHARP or PLAIN
  // turn duration
      
      if (LEFTRIGHT == "RIGHT") {

        if (SHARP == "SHARP") {
          motor_right("ON", "BACK");
          motor_left("ON", "FRONT");        
        }
        else {
          motor_right("OFF", "BACK");
          motor_left("ON", "FRONT");                 
        }               
      }

      else {
        
        if (SHARP == "SHARP") {
          motor_right("ON", "FRONT");
          motor_left("ON", "BACK");        
        }
        else {
          motor_right("ON", "FRONT");
          motor_left("OFF", "BACK");                 
        }             
      }

      if (TurnDuration >= 0) {
        teodelay(TurnDuration);
        StopCar;        
      }
      
      return;  
}

void led_use(char SIDE[], char COLOR[], char ONOFF[]) {


  // switching on/off the LEDs

      int ledpin = 0;

      if (COLOR == "RED") {
        if (SIDE == "LEFT") {
          ledpin = ledpin_red_left;
        }
        else {
          ledpin = ledpin_red_right;
        }        
      }
      else if (COLOR == "BLUE") {
        if (SIDE == "LEFT") {
          ledpin = ledpin_blue_left;
        }
        else {
          ledpin = ledpin_blue_right;
        } 
      }
      else {
        ledpin = 13;
      }

      
      if (ONOFF == "OFF") {
        digitalWrite(ledpin, LOW); 
      }
      else {
        digitalWrite(ledpin, HIGH); 
      }
}

void motor_left(char ONOFF[], char FRONTBACK[]) {


  // switching left motor by user's choice
  // on/off
  // forward backward
      
      if (ONOFF == "OFF")   {
        digitalWrite(leftmotorpin1, LOW); 
        digitalWrite(leftmotorpin2, LOW);
        led_use("LEFT", "BLUE", "OFF");  
        led_use("LEFT", "RED", "OFF");         
        return;
      }
      else {
        if (FRONTBACK == "BACK") {
          digitalWrite(leftmotorpin1, LOW);
          digitalWrite(leftmotorpin2, HIGH);
          led_use("LEFT", "BLUE", "OFF");  
          led_use("LEFT", "RED", "ON");          
        }
        else {
          digitalWrite(leftmotorpin1, HIGH);
          digitalWrite(leftmotorpin2, LOW); 
          led_use("LEFT", "BLUE", "ON");  
          led_use("LEFT", "RED", "OFF");                    
        }
      }
 
      Serial.print(" Left Motor : ");
      Serial.print(ONOFF);
      Serial.print(" - ");
      Serial.print(FRONTBACK);
      Serial.println();

  
      return;
}

void motor_right(char ONOFF[], char FRONTBACK[]) {

  // switching right motor by user's choice
  // on/off
  // forward backward     
      
      if (ONOFF == "OFF")   {
        digitalWrite(rightmotorpin1, LOW); 
        digitalWrite(rightmotorpin2, LOW);
        led_use("RIGHT", "BLUE", "OFF");  
        led_use("RIGHT", "RED", "OFF");        
        return;
      }
      else {
        if (FRONTBACK == "BACK") {
          digitalWrite(rightmotorpin1, LOW);
          digitalWrite(rightmotorpin2, HIGH);
          led_use("RIGHT", "BLUE", "OFF");  
          led_use("RIGHT", "RED", "ON");           
        }
        else {
          digitalWrite(rightmotorpin1, HIGH);
          digitalWrite(rightmotorpin2, LOW);
          led_use("RIGHT", "BLUE", "ON");  
          led_use("RIGHT", "RED", "OFF");                      
        }
      }

      Serial.print(" Right Motor : ");
      Serial.print(ONOFF);
      Serial.print(" - ");
      Serial.print(FRONTBACK);
      Serial.println();
      return;
}

void pause_blinking (int duration, int blinktimes) {

  // just a LED blinking pattern to demonstrate initiation of the main loop
  // can be used for anuthing, and with blue and red LEDs looks kind of a police car.
        
    int delaytime = 0;

    if (blinktimes== 0) {      
    }
    else {
      delaytime = (duration / (blinktimes * 2));
    }


    if (blinktimes == 0) {
      teodelay(duration);
    }
    else {
      
      led_use("LEFT", "BLUE", "OFF");
      led_use("LEFT", "RED", "OFF");
      led_use("RIGHT", "BLUE", "OFF");
      led_use("RIGHT", "RED", "OFF");
            
      for (int i = 1; i <= blinktimes; i++) {
  
        led_use("LEFT", "BLUE", "ON");
        led_use("LEFT", "RED", "OFF");
        led_use("RIGHT", "BLUE", "OFF");
        led_use("RIGHT", "RED", "ON");        
    
        teodelay(delaytime);
    
        led_use("LEFT", "BLUE", "OFF");
        led_use("LEFT", "RED", "ON");
        led_use("RIGHT", "BLUE", "ON");
        led_use("RIGHT", "RED", "OFF"); 
    
        teodelay(delaytime);    
    
        led_use("LEFT", "BLUE", "ON");
        led_use("LEFT", "RED", "ON");
        led_use("RIGHT", "BLUE", "ON");
        led_use("RIGHT", "RED", "ON"); 
    
        teodelay(delaytime);       
      }
      
      led_use("LEFT", "BLUE", "OFF");
      led_use("LEFT", "RED", "OFF");
      led_use("RIGHT", "BLUE", "OFF");
      led_use("RIGHT", "RED", "OFF"); 
    }
}


void teodelay(int duration) {

  // my version of a delay function based on millis
  // because delay function acts kind of weird sometimes

    int runninginterval = 0;
    int loopfactor  = -1;
    unsigned int StartTime = millis();
    
    while (loopfactor == -1) {

      unsigned int CurrentTime = millis();

      runninginterval = (CurrentTime - StartTime);
      if (runninginterval < duration) {
          loopfactor = -1;
      }
      else {
          loopfactor = 0;
      }
      
    }   
}
