//Pin defenitions and other essential values
#define pwm_r 5  //PWM control for motor outputs 1 and 2 is on digital pin 5 RIGHT
#define pwm_l 11  //PWM control for motor outputs 3 and 4 is on digital pin 11 LEFT
#define dir_r 12  //dir control for motor outputs 1 and 2 is on digital pin 12 RIGHT Low is forward
#define dir_l 13  //dir control for motor outputs 3 and 4 is on digital pin 13 LEFT High is forward
#define PINGA 9 //Trigger and echo for the Front Sensor
#define PINGB 8 // Echo for the Front Sensor
#define BASE_SPEED 128 // base speed of the robot
#define SETPOINT 10 //setpoint

// Variables
long sonica = 0; //Sonic a intermediate distance
long sonicb = 0;// sonic b intermediate distance
volatile int counterL = 0; // Counter for the Left Wheel encoder
volatile int counterR = 0; // Counter for the Right Wheel encoder
double rspeed = 128;
double lspeed = 128;
double SetPoint = 10;
double ratio = 1;
double Kp = .25;
double diff = 0;
double spchange = 0;
double Max = 0;
double Min = 0;
double max_dist = 25;
double integral = 0;
double Ki = .25;

void setup()
{
  attachInterrupt(0, AddL, CHANGE); // Intettupt for the Left Wheel encoder
  attachInterrupt(1, AddR, CHANGE); // Interrupt for the Right Wheel encoder
  pinMode(5, OUTPUT);  //Set control PWM pins to be outputs Speed variance
  pinMode(11, OUTPUT);
  pinMode(dir_r, OUTPUT); // Set the direction pins to Output (high or low)
  pinMode(dir_l, OUTPUT);
  Serial.begin(9600); //Opens up a serial port for command rx and data tx
  Max = BASE_SPEED+((max_dist - SetPoint)*Kp)*BASE_SPEED;
  Min = -Max;
}
void AddL(){ // Addition for the left wheel
 counterL++;
}
void AddR(){ //Addition for the right wheel
 counterR++;
}
void loop()
{
 sonica = Sonic(PINGA); // Collect a sample from the front sensor
 sonicb = Sonic(PINGB); // Collect a sample from the right sensor
 
 if (sonica >= 35){  
    if(SETPOINT == sonicb){
        ratio = 1;
    }
    else if (SETPOINT != sonicb){
        diff = SETPOINT - sonicb;
        integral +=diff;
        ratio = (diff*Kp)+(integral*Ki); 
    }
    spchange = BASE_SPEED*ratio;
    
    rspeed += spchange;
    lspeed -= spchange;
    rspeed = map(rspeed, Min, Max, 0, 255);
    lspeed = map(lspeed, Min, Max, 0, 255);
    
    if (ratio == 1){
       rspeed = BASE_SPEED;
       lspeed = BASE_SPEED;      
    } 
  }
 else{
   rspeed = BASE_SPEED;
   lspeed = 0;
} 
if( sonicb <= 6){
  rspeed = BASE_SPEED;
  lspeed = 0;
}
if(rspeed >255)
  rspeed = 255;
if(lspeed > 255)
  lspeed = 255;
if(rspeed < 0)
  rspeed = 0;
if(lspeed < 0)
  lspeed = 0;

 SetWheel(pwm_r, rspeed, dir_r, LOW);
 SetWheel(pwm_l, lspeed, dir_l, HIGH);
  delay(100);

 //Serial.print(counterR);
 //Serial.print(",");
 //Serial.println(counterL);
// Serial.print(sonica);
 //Serial.print(",");
 //Serial.println(sonicb);
// Serial.print(",");
 Serial.print(rspeed);
 Serial.print(",");
 Serial.println(lspeed);
 counterR = 0; 
 counterL = 0;
// rspeed = BASE_SPEED;
// lspeed = BASE_SPEED;
 delay(100);
}
// Methods to control the motors
void SetWheel( int pwmpin, int vel, int dirpin, boolean dir){
  analogWrite(pwmpin, vel);
  digitalWrite(dirpin, dir);
}
//Method to read/write data to and from the sonic sensors, The below is setup per the PING))) Datasheet
long Sonic(int a){
long duration, cm;
  pinMode(a, OUTPUT);
  digitalWrite(a, LOW);
  delayMicroseconds(2);
  digitalWrite(a, HIGH);
  delayMicroseconds(5);
  digitalWrite(a, LOW);
  pinMode(a, INPUT);
  duration = pulseIn(a, HIGH);
  delay(10);
  cm = (duration/29) /2;
  return cm;
  }