// Code used for the obstacle avoiding, line following robot.
const int  MotorControl[] = {5,6,7,8,9,10}; // Define the motor driver input controls
const int  LineSens    [] = {A0,A1,A2    }; // Define Line sensor's L,C,R inputs
int        LineVal     [] = {0,0,0       }; // Variable to check the sensor's values
byte brightnessVal = 127;                   // Set the brightness val required to pass args
char Direction;                             // Define the direction of the car
// Switch between line following mode and obstacle avoiding mode
const int modesw = 2;
// Variables to calculate the Us sens. measured distance
const int trigPin = 11;
const int echoPin = 3;
long duration;
int distance;

// PID stuff
float Kp = 2; // proportional constant
float Ki = 3; // integral constant
float Kd = 3; // derivate constant
int P; int I; int D;
int error;
int lastError = 0;

void setup() {

pinMode(MotorControl, OUTPUT);
pinMode(     trigPin, OUTPUT);
pinMode(LineSens    , INPUT );
pinMode(     echoPin, INPUT );
pinMode(modesw, INPUT_PULLUP);
Serial.begin(9600);

}

void loop() {
if(digitalRead(modesw) == 0){
  LineSensor();
  Control(Direction);
}else{
  UltrasonicSens();
  Control(Direction);
}


}


void Control(char x) {

  // Switch between the possible directions
  switch(x){
    case 'F':
      //set motor1 forward
    digitalWrite(MotorControl[0], 1 );
    digitalWrite(MotorControl[1], 0 );
      //set motor2 forward
    digitalWrite(MotorControl[2], 1 );
    digitalWrite(MotorControl[3], 0 );
    break;
    case 'B':
     //set motor1 backward
    digitalWrite(MotorControl[0], 0 );
    digitalWrite(MotorControl[1], 1 );
      //set motor2 backward
    digitalWrite(MotorControl[2], 0 );
    digitalWrite(MotorControl[3], 1 );
    break;
    case 'L':
      //set motor1 off
    digitalWrite(MotorControl[0], 0 );
    digitalWrite(MotorControl[1], 0 );
      //set motor2 forward
    digitalWrite(MotorControl[2], 1 );
    digitalWrite(MotorControl[3], 0 );
    break;
    case 'R':
      //set motor1 forward
    digitalWrite(MotorControl[0], 1 );
    digitalWrite(MotorControl[1], 0 );
      //set motor2 off
    digitalWrite(MotorControl[2], 0 );
    digitalWrite(MotorControl[3], 0 );
    break;}

  }
  
void LineSensor(){

  LineVal[0] = analogRead(LineSens[0]);
  LineVal[1] = analogRead(LineSens[1]);
  LineVal[2] = analogRead(LineSens[2]);
  int LineAVG = (LineVal[0]+LineVal[1]+LineVal[2])/3;
  error = LineAVG - 200;
  P = error;
  I = I + error;
  D = error - lastError;
  lastError = error;
  int pwm = P*Kp + I*Ki + D*Kd; 
  int pwma = 100 + pwm;
  int pwmb = 100 - pwm;
  if(pwma > 200){
    pwma = 200;
  }else if(pwma < 0){
    pwma = 0;
  }else if(pwmb < 0){
    pwmb = 0;
  }
  analogWrite(MotorControl[4], pwma);
  analogWrite(MotorControl[5], pwmb);

  if       (LineVal[0] < brightnessVal || (LineVal[0] < brightnessVal && LineVal[1] < brightnessVal)){
    Direction = 'R';
  }else if (LineVal[2] < brightnessVal || (LineVal[2] < brightnessVal && LineVal[1] < brightnessVal)){
    Direction = 'L';
  }else if (LineVal[1] < brightnessVal){
    Direction = 'F';
  }
  return Direction;

}

void UltrasonicSens(){

  analogWrite(MotorControl[4], 120);
  analogWrite(MotorControl[5], 120);
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  duration = pulseIn(echoPin, HIGH);
  distance = duration * 0.034 / 2;

  if(distance > 20){
    Direction = 'F';
  }else{
    analogWrite(MotorControl[4], 200);
    analogWrite(MotorControl[5], 200);
    Direction = 'B';
    delay(100);
    Direction = 'R';
    }
  return Direction;

}