/*****************************
  Pins for Ultrasonic Sensor
******************************/
const byte trigPin1 = 11;          // Trigger Pin of sensor is connected at digital Pin 5 of Arduino
const byte echoPin1 = 12;          // Echo Pin of sensor is connected at digital Pin 6 of Arduino
// 2nd Ultrasonic sensor
const byte trigPin2 = 9;            // Trigger Pin of sensor is connected at digital Pin 5 of Arduino
const byte echoPin2 = 10;           // Echo Pin of sensor is connected at digital Pin 6 of Arduino

long duration;                      // This will measure time taken by wave to reflect back

double distance1;                      // This will store distance calculated
double distance2;                      // This will store distance calculated

/********************
   Motor Driver Pins
 *******************/
const byte motor1PWMPin = 5; // Motor 1 pwm pin to adjust speed
const byte motor2PWMPin = 6; // Motor 2 pwm pin to adjust speed

const byte in1 = 2;           // Motor control pin1
const byte in2 = 3;           // Motor control pin2
const byte in3 = 4;           // Motor control pin3
const byte in4 = 7;           // Motor control pin4


/****************
   PID Variables
*****************/
#include <PID_v1.h>

//Define Variables we'll be connecting to
double Output1;             // Variable to store output pwm variable value to control motor
double Output2;             // Variable to store output pwm variable value to control motor

// Set the adjust points for each distance sensor
double Setpoint1 = 25;      // Set Point value to keep distance constant
double Setpoint2 = 45;      // Set Point value to keep distance constant

//Specify the links and initial tuning parameters
double Kp = 10, Ki = 5, Kd = 3;
PID myPID1(&distance1, &Output1, &Setpoint1, Kp, Ki, Kd, DIRECT);
PID myPID2(&distance2, &Output2, &Setpoint2, Kp, Ki, Kd, DIRECT);

int minPwm = -80;   // Set the minimum pwm value pID needs to return in case of overshoot it will be used to drive in reverse
int maxPwm = 80;    // Set the maximum pwm value pID needs to return in to go forward

bool reverseMotor1 = LOW; // Flag to check if motor is reversed
bool reverseMotor2 = LOW; // Flag to check if motor is reversed

int thresDistance = 5;    // Threshold distance to reverse motor if motor exceeds set point + this distance

void setup()
{
  Serial.begin(115200);
  //Set the pin modes of ultrasonic sensor pins
  pinMode(trigPin1, OUTPUT);   // Sets the trigPin as an Output
  pinMode(echoPin1, INPUT);    // Sets the echoPin as an Input
  pinMode(trigPin2, OUTPUT);   // Sets the trigPin as an Output
  pinMode(echoPin2, INPUT);    // Sets the echoPin as an Input

  // Set motor control pins as output
  for (byte j = 2; j < 8; j++) {
    pinMode(j, OUTPUT);         // Set the pin mode as output
  }
  //turn the PID on
  myPID1.SetMode(AUTOMATIC);        // Set PID mode to automatic
  myPID1.SetSampleTime(10);
  myPID1.SetOutputLimits(minPwm, maxPwm);  // Set the min and max limit calculated by PID
  myPID2.SetMode(AUTOMATIC);        // Set PID mode to automatic
  myPID2.SetSampleTime(10);
  myPID2.SetOutputLimits(minPwm, maxPwm);  // Set the min and max limit calculated by PID

  // Start both motors
  digitalWrite(in1, HIGH);            // Set the in1 pin of driver high (To turn motor in opposite direction change LOW with HIGH and HIGH with LOW)
  digitalWrite(in2, LOW);             // Set the in2 pin of driver LOW (To turn motor in opposite direction change LOW with HIGH and HIGH with LOW)
  digitalWrite(in3, HIGH);            // Set the in3 pin of driver high (To turn motor in opposite direction change LOW with HIGH and HIGH with LOW)
  digitalWrite(in4, LOW);             // Set the in4 pin of driver LOW (To turn motor in opposite direction change LOW with HIGH and HIGH with LOW)
}

void loop ()
{
  // Calculate pwm value by using sensor 1 reading and adjust motor speed
  distance1 = dist(trigPin1, echoPin1);
  
  
  myPID1.Compute(); // Compute the PID value and use it to adjust motor speed
  analogWrite(motor1PWMPin, abs(Output1));
  if (Output1 < 0){                     // check if pwm signal generated is less than 0 start motor in reverse
    digitalWrite(in1, LOW);            // Set the in1 pin of driver high (To turn motor in opposite direction change LOW with HIGH and HIGH with LOW)
    digitalWrite(in2, HIGH);             // Set the in2 pin of driver LOW (To turn motor in opposite direction change LOW with HIGH and HIGH with LOW)
  }
  else{   // Otherwise move in forward
    digitalWrite(in1, HIGH);            // Set the in1 pin of driver high (To turn motor in opposite direction change LOW with HIGH and HIGH with LOW)
    digitalWrite(in2, LOW);             // Set the in2 pin of driver LOW (To turn motor in opposite direction change LOW with HIGH and HIGH with LOW)
  }

  // Calculate pwm value by using sensor 1 reading and adjust motor speed
  distance2 = dist(trigPin2, echoPin2);
  
  myPID2.Compute(); // Compute the PID value and use it to adjust motor speed
  analogWrite(motor2PWMPin, abs(Output2));
  if (Output2 < 0){                     // check if pwm signal generated is less than 0 start motor in reverse
    digitalWrite(in3, LOW);            // Set the in1 pin of driver high (To turn motor in opposite direction change LOW with HIGH and HIGH with LOW)
    digitalWrite(in4, HIGH);             // Set the in2 pin of driver LOW (To turn motor in opposite direction change LOW with HIGH and HIGH with LOW)
  }
  else{   // Otherwise move in forward
    digitalWrite(in3, HIGH);            // Set the in1 pin of driver high (To turn motor in opposite direction change LOW with HIGH and HIGH with LOW)
    digitalWrite(in4, LOW);             // Set the in2 pin of driver LOW (To turn motor in opposite direction change LOW with HIGH and HIGH with LOW)
  }
  Serial.print(distance1); Serial.print(" | ");Serial.print(Output1); Serial.print(" | ");
  Serial.print(distance2); Serial.print(" | "); Serial.println(Output2);
}
double dist(byte trigPin, byte echoPin) {
  // This function will take trig pin and echo pin number as input and return the distance for that sensor pins
  // Clears the trigPin
  long distance = 0;        // Variable to store ditance and return it
  digitalWrite(trigPin, LOW);
  delayMicroseconds(2);
  // Sets the trigPin on HIGH state for 10 micro seconds
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);
  // Reads the echoPin, returns the sound wave travel time in microseconds
  duration = pulseIn(echoPin, HIGH);
  // Calculating the distance
  distance = duration * 0.034 / 2;
  return distance;
}