const int trigPin = 4;          //proximity sensor trig pin
const int echoPin =5;           //proximity sensor echo pin

const int AIN1 = 13;           //control pin 1 on the motor driver for the right motor
const int AIN2 = 12;            //control pin 2 on the motor driver for the right motor
const int PWMA = 11;            //speed control pin on the motor driver for the right motor
const int PWMB = 9;           //speed control pin on the motor driver for the left motor
const int BIN2 = 10;           //control pin 2 on the motor driver for the left motor
const int BIN1 = 8;           //control pin 1 on the motor driver for the left motor
const int mpidout = 255;   //maximum PWM signal to be given to the motors

const float manualangle =38.0L;       //manual estimation of balance angle
const float pi=3.14159L;            //pi
const float diagonallength= 9.6L;        //length from wheel center to sensor
const float mountangle = 51*pi/180L;    //angle between sensor and diagonal (see video for more)
const float wheelradius=1.3125L;       //radius of wheel

double distance;              //distance found by proximity sensor
double lastdistance =0;               //stores previous distance, used to calculate velocity and acceleration
                   
float dt;          //time between each loop, used for integral and derivative (li, ld)
float lasttime;       //time used to find dt
double de;          //change in angleerror, used to find the derivative component of the lower PID loop
double lerror;     //previous error used to find de

const double lpk =17L;      //proportional (P) constant for tuning the lower PID loop (see video for more)
const double lik =.075L;    //integral (I) constant for tuning the lower PID loop (see video for more)
const double ldk =550L;    //derivative (D) constant for tuning the lower PID loop (see video for more)

double angleerror = 0;              //how far robot is from desired angle, calculated with proximity sensor values and angle offset found with higher PID loop, input for lower PID loop and part of the proportional component of the lower PID loop (see video for more)
double li;         //integral (accumulated error) of the angle error with respect to time, part of the integral component of the lower PID loop
double ld;         //derivative (velocity) of the angle error with respect to time (change in angle error / change in time), part of the integral component of the lower PID loop
  
double lpid;          //output of the lower PID loop, used to determine motor power
    
double leftlpid;     //left motor power
double rightlpid;    //right motor power
  
double lastlpid;        //previous lpid, used for more accurate position estimates 
double lastperror;      //last positionerror used to find pd
  
double pd;                       //change in positionerror, used to find the derivative component of the higher PID loop
double positionerror;      //position error found by integrating power given to motors, input for higher PID loop and directly makes up the proportional component of the higher PID loop                     
double hi;                //integral (accumulated error) of the position error with respect to time (change in , part of the integral component of the higher PID loop
double hd;              //derivative (velocity) of the position error with respect to time (change in position error / change in time), part of the integral component of the higher  PID loop
double hpid;                         //output of higher PID loop, add together to approach ideal balance angle/hold position (see video for more)
const double hpk =.000023L;           //proportional (P) constant for tuning the higher PID loop (see video for more)
const double hik =.00000000008L;     //integral (I) constant for tuning the cascading PID loop (see video for more)
const double hdk =.055L;              //integral (I) constant for tuning the cascading PID loop (see video for more)


const float filtermargin = 1;       //maximum difference between consecutive distance readings which won't be filtered out

 //variables below are for bluetooth control
 #include <SoftwareSerial.h>         
const int txPin=7;
const int rxPin =6;
bool isfront;
bool isback;
bool isleft;
bool isright;
int btinput;
float wt;
float bdt;
float lt;
float rt;
const float steeringsens = .05;
const float drivesens = .5;
SoftwareSerial mySerial (rxPin, txPin);
 
  void setup() {
  // put your setup code here, to run once:
  pinMode(trigPin, OUTPUT);       //this pin will send ultrasonic pulses out from the distance sensor
  pinMode(echoPin, INPUT);        //this pin will sense when the pulses reflect back to the distance sensor
  
  pinMode(AIN1, OUTPUT);
  pinMode(AIN2, OUTPUT);
  pinMode(PWMA, OUTPUT);
  pinMode(BIN1, OUTPUT);
  pinMode(BIN2, OUTPUT);
  pinMode(PWMB, OUTPUT);
  
 mySerial.begin(9600);
 Serial.begin(9600);
 distance=getDistance();  //finds a starting value as to not trigger the filter
}


void loop() {
  
 //reads ultrasonic sensor values until consecutive readings are within filtermargin, then sets distance used for calculations to the most recent reading
 lastdistance=getDistance();     
 while((lastdistance-distance)>filtermargin or (lastdistance-distance)<(-filtermargin)){  
    lastdistance=getDistance();     
  }
  distance = lastdistance;

 //equation to find how far robot is from balance point, inputs are distance from the filter and the higher PID angle adjustments (see video for more)
 angleerror = 180L/pi*(asin(distance*sin(mountangle)/sqrt(pow(diagonallength, 2)+pow(distance, 2)-2L*diagonallength*distance*cos(mountangle)))-pi/2L+acos(wheelradius/sqrt(pow(diagonallength, 2)+pow(distance, 2)-2L*diagonallength*distance*cos(mountangle))))-manualangle+hpid;

//adjusts error outputs to make robot lean/go forwards or backwards
if(isfront){
  angleerror+=drivesens;
}
if(isback){
angleerror-=drivesens;
}

//lower PID outputs values to motors to pursue desired angle (balance)
de = angleerror - lerror;
dt = millis()-lasttime;
lasttime = millis();
lerror = angleerror;
//derivative (velocity) of the angleerror with respect to time
ld = de/dt;
//integral (accumulated error) of the angleerror with respect to time
li += angleerror*dt;
 //adds P, I, and D together to complete the lower PID output
 lpid = ((angleerror*lpk +li*lik+ld*ldk));
 //Serial.println(String(angleerror*lpk) + " "  +String(li*lik)+ " " + String(ld*ldk) + " " + String(dt)); 

  //constrains lower PID output to PWM range    
  if(lpid>mpidout)lpid=mpidout;
  if(lpid<mpidout*-1)lpid=mpidout*-1;

  //modifies lower PID output for steering, higher difference at lower outputs for turning stability
   leftlpid =lpid;
   rightlpid =lpid;
 if(isright){
  leftlpid+=(255-abs(leftlpid))*steeringsens;
  rightlpid-=(255-abs(rightlpid))*steeringsens;
}
 if(isleft){
  leftlpid-=(255-abs(leftlpid))*steeringsens;
  rightlpid+=(255-abs(rightlpid))*steeringsens;
}
  if(leftlpid<0)leftlpid=round(map(leftlpid, -255, 0, -255, -38));
  if(leftlpid>0)leftlpid=round(map(leftlpid, 0, 255, 38, 255));
  if(rightlpid<0)rightlpid=round(map(rightlpid, -255, 0, -255, -38));
  if(rightlpid>0)rightlpid=round(map(rightlpid, 0, 255, 38, 255));
  Serial.println("r"+String(leftlpid)+" "+String(rightlpid));

  //drives motors with determined power
  rightMotor(rightlpid);
  leftMotor(leftlpid);
 
 //clears positionerror while robot should be moving 
 if(isfront or isback){
   positionerror=0;
    lastperror=0;
    hi=0;
 }

 //higher PID adjusts desired angle to keep the robot in place 
  else{
  //estimates robot position by multiplying the power given to the motors by the time spent under that power and adding it to a total (integrating motor power with respect to time)
 positionerror+=lastlpid*dt;         
 pd=positionerror-lastperror;
 lastperror=positionerror;
 //derivative (velocity) of the positionerror with respect to time
 hd = pd/dt;
 //integral (accumulated error) of the positionerror with respect to time
 hi += positionerror*dt;
 hpid += ((positionerror*hpk +hi*hik+hd*hdk))/1000;
 }
 lastlpid=lpid;
 
 
 
   
 //bluetooth input handler
   if (mySerial.available()) {
     btinput=(mySerial.read());
    if(btinput==49){
      isfront=true;
      wt=millis();
  }
 if(btinput==50){
      isback=true;
      bdt=millis();
  }
  if(btinput==51){
      isright=true;
      rt=millis();
        }
 if(btinput==52){
      isleft=true;
      lt=millis();
      }
}
  if(isfront and (millis()-wt)>1500){
     isfront=false;
  }
  if(isback and (millis()-bdt)>1500){
     isback=false;
  }
  if(isright and (millis()-rt)>1500){
     isright=false;
     }
  if(isleft and (millis()-lt)>1500){
     isleft=false; 
  }
}

float getDistance()
{
  float echoTime;                   //variable to store the time it takes for a ping to bounce off an object
  float calculatedDistance;         //variable to store the distance calculated from the echo time

  //send out an ultrasonic pulse that's 10ms long
  digitalWrite(trigPin, HIGH);
  delayMicroseconds(10);
  digitalWrite(trigPin, LOW);

  echoTime = pulseIn(echoPin, HIGH);      //use the pulsein command to see how long it takes for the
                                          //pulse to bounce back to the sensor

  calculatedDistance = echoTime / 148.0;  //calculate the distance of the object that reflected the pulse (half the bounce time multiplied by the speed of sound)

  return calculatedDistance;              //send back the distance that was calculated
}

void rightMotor(int motorSpeed)                       //function for driving the right motor
{
  if (motorSpeed > 0)                                 //if the motor should drive forward (positive speed)
  {
    digitalWrite(AIN1, HIGH);                         //set pin 1 to high
    digitalWrite(AIN2, LOW);                          //set pin 2 to low
  }
  else if (motorSpeed < 0)                            //if the motor should drive backward (negative speed)
  {
    digitalWrite(AIN1, LOW);                          //set pin 1 to low
    digitalWrite(AIN2, HIGH);                         //set pin 2 to high
  }
  else                                                //if the motor should stop
  {
    digitalWrite(AIN1, LOW);                          //set pin 1 to low
    digitalWrite(AIN2, LOW);                          //set pin 2 to low
  }
  analogWrite(PWMA, abs(motorSpeed));                 //now that the motor direction is set, drive it at the entered speed
}

/********************************************************************************/
void leftMotor(int motorSpeed)                       //function for driving the right motor
{
  if (motorSpeed > 0)                                 //if the motor should drive forward (positive speed)
  {
    digitalWrite(BIN1, HIGH);                         //set pin 1 to high
    digitalWrite(BIN2, LOW);                          //set pin 2 to low
  }
  else if (motorSpeed < 0)                            //if the motor should drive backward (negative speed)
  {
    digitalWrite(BIN1, LOW);                          //set pin 1 to low
    digitalWrite(BIN2, HIGH);                         //set pin 2 to high
  }
  else                                                //if the motor should stop
  {
    digitalWrite(BIN1, LOW);                          //set pin 1 to low
    digitalWrite(BIN2, LOW);                          //set pin 2 to low
  }
  analogWrite(PWMB, abs(motorSpeed));                 //now that the motor direction is set, drive it at the entered speed
}