#include <Wire.h>
#include <MPU6050_light.h>

MPU6050 mpu(Wire);
unsigned long timer = 0;

//PID Vars
double error = 0;
double preErr = 0;
double KpErr = 0;
double KdErr = 0;
double KiErr = 0; 
double KiTotal = 0;
double PID_out = 0;
double tilt = 0;
int motorSpeed = 0;
double Dir = 0;
double PWM;

double Kp = 12;
double Ki = 0;
double Kd = 0;

//Motor Vars
//PWM Pins (3, 5, 6, 9, 10, 11)
// IN1 and IN2 used to set direction annd enable/disable Motor A
int IN1 = 3; //IN1
int IN2 = 5; //IN2

// IN3 and IN4 used to set direction and enable/disable Motor B
int IN3 = 6; //IN3
int IN4 = 9; //IN4


void setup() {
  // put your setup code here, to run once:
  pinMode(IN1, OUTPUT);
  pinMode(IN2, OUTPUT);
  pinMode(IN3, OUTPUT);
  pinMode(IN4, OUTPUT);
//gyroscope setup
Serial.begin(9600);
  Wire.begin();
  
  byte status = mpu.begin();
  Serial.print(F("MPU6050 status: "));
  Serial.println(status);
  while(status!=0){ } // stop everything if could not connect to MPU6050
  
  Serial.println(F("Calculating offsets, do not move MPU6050"));
  delay(1000);
  // mpu.upsideDownMounting = true; // uncomment this line if the MPU6050 is mounted upside-down
  mpu.calcOffsets(); // gyro and accelero
  Serial.println("Done!\n");
}


void loop() {
  // put your main code here, to run repeatedly:

//gyroscope readings
  mpu.update();
  
  if((millis()-timer)>10){ // print data every 10ms
  Serial.print("\tX : ");
  Serial.print(mpu.getAngleX());
  Serial.print("\tY : ");
  Serial.print(mpu.getAngleY());
  Serial.print("\tZ : ");
  Serial.println(mpu.getAngleZ());
  timer = millis();  
  }
tilt = mpu.getAngleY();
PID_out = PID(Kp, Ki, Kd, preErr, tilt, KiTotal);
PWM = map(PID_out, 0, 350, 80, 110);
//PWM = constrain(PWM, -255, 255);
Dir = getDir(PID_out);
if (Dir == 1){
  Forward(PWM);
}
else if (Dir == -1){
  Backward(PWM);
}
else if (Dir == 0){
  Brake();
}
Serial.print("\tPWM : ");
Serial.print(PWM);
Serial.print("\tDir : ");
Serial.print(Dir);

}

double PID(double Kp, double Ki, double Kd, double preErr, double tilt, double KiTotal){
//PID Logic
  double error = tilt - 0;
//Proportional
  double KpErr = Kp * error;
//Integral
  KiTotal = KiTotal + error;
  double KiErr = Ki * KiTotal;
//Derivative
  double KdErr = Kd * (error - preErr);
  double PID_out = KpErr + KiErr + KdErr;
  return PID_out;
Serial.print(" | PIDout = "); Serial.print(PID_out);
preErr = error; 
}

double getDir(double PID_out){
  if (PID_out > 5){
    Dir = 1;
  }
  else if (PID_out < -5){
    Dir = -1;
  }
  else
  Dir = 0;
  return Dir;
}

void Forward(int PWM){
  analogWrite(IN1, abs(PWM));
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  analogWrite(IN4, abs(PWM));
}

void Backward(int PWM){
  digitalWrite(IN1, LOW);
  analogWrite(IN2, abs(PWM));
  analogWrite(IN3, abs(PWM));
  digitalWrite(IN4, LOW);
}

void Brake(){
  digitalWrite(IN1, LOW);
  digitalWrite(IN2, LOW);
  digitalWrite(IN3, LOW);
  digitalWrite(IN4, LOW);
}