/* This code is for the balancing robot project. You can find more about this on https://www.electronoobs.com.
 * The code will read data from an IMU module, calculate the PID value in order to balance the robot at 0º and
 * then it will create pules using a timer and apply those to some stepper motors and move the robot.
 * Kind thanks to Joop Brokking for the help: https://www.youtube.com/user/MacPuffdog
 *
 * Tutorial: https://electronoobs.com/eng_arduino_tut159.php
 * Schematic: https://electronoobs.com/eng_arduino_tut150_sch1.php */
 

//Include Libraries
#include <Wire.h>                                  //Wire.h is used to get i2c data from the MPU6050
#include <Encoder.h>

/////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////PID VALUES////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
float Kp = 28;                    //P Gain; Mine was 30
float Ki = 0.3;                  //I Gain; Mine was 0.61
float Kd = 4;                     //D Gain; Mine was 9
float Moving_Speed = 20;          //Moving speed with Bluetooth Control; Mine was 20
float Max_Speed = 160;            //Max mooving speed; Mine was 160
int Acc_Offset = 0.05;            //Accelerometer offset value (find this before you run the code)
/////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
Encoder myEnc(11, 12);

/////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////VARIABLES////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
byte Activated, Received_byte;
int left_motor;
int Left_Motor_Speed;
int CC_Speed_Left_Motor;
int Left_Motor_Speed_Prev;
int right_motor;
int Right_Motor_Speed;
int CC_Speed_Right_Motor;
int Right_Motor_Speed_Prev;
int Received_Since;
int Gyro_X_Raw, Gyro_Y_Raw, Acc_Raw;
long Gyro_Y_Offset, Gyro_X_Offset;
unsigned long Loop_Time;
float Gyro_Angle, Acc_Angle, Auto_Setpoint;
float Temp_Error, PID_I, Setpoint, gyro_input, PID_Value, Last_D_Error;
float PID_Value_left, PID_Value_right;
int MPU6050_ADDR = 0x68;                          //MPU6050 I2C address (0x68 or sometimes 0x69)



/////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////Input/Output///////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
int DIR_L = 2;      //Pin for left driver direction pin
int STEP_L = 3;     //Pin for left driver steps pin
int DIR_R = 4;      //Pin for right driver direction pin
int STEP_R = 5;     //Pin for right driver steps pin
int Enable = 6;     //Pin for drivers enable (both use same pin)
int LED1 = 7;       //Left LED is connected on D7
int LED2 = 8;       //Right LED is connected on D8
int Buzzer = 9;     //Buzzer is connected on D9
/////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////




/////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////SETUP LOOP////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
void setup() {
  Serial.begin(9600);       //Start the serial port at 9600 kbps
  Wire.begin();             //Start I2C communication
  TWBR = 12;                //Also set I2C clock speed to 400kHz

  //Define the pins mode (Output or Input)
  pinMode(DIR_L, OUTPUT);
  pinMode(STEP_L, OUTPUT);
  pinMode(DIR_R, OUTPUT);
  pinMode(STEP_R, OUTPUT);
  pinMode(LED1, OUTPUT);
  pinMode(LED2, OUTPUT);
  pinMode(Buzzer, OUTPUT);

  /*Read this: Now we define a "timer", timer 2 in this case. This timer will be used to create
  the step pulses for the motors. This timer will click every 20us and make the required calculations.
  The code will be inside the subroutine of TIMER2_COMPA_vect at the end of the code*/
  TCCR2A = 0;               //Start with TCCR2A set to zero
  TCCR2B = 0;               //Start with TCCR2B set to zero
  TIMSK2 |= (1 << OCIE2A);  //Interupt enable bit OCIE2A set to 1
  TCCR2B |= (1 << CS21);    //Set CS21 bit: We set prescaler to 8
  OCR2A = 39;               //Compare register is 39, so...   20us/(1s/(16MHz/8))-1
  TCCR2A |= (1 << WGM21);   //Mode: Clear timer on compare

  //Start MPU6050 communication
  Wire.beginTransmission(MPU6050_ADDR);       //From the datastheet, the address is 0x68, but you can change that above.
  Wire.write(0x6B);                           //Write on 0x6B register
  Wire.write(0x00);                           //Set register to 00000000 and activate gyro
  Wire.endTransmission();                     //End the i2c transmission
  //Change gyro scale to +/-250deg/sec
  Wire.beginTransmission(MPU6050_ADDR);       //My MPU6050 address is 0x68, change it at the begginning of the code
  Wire.write(0x1B);                           //Write on 0x1B register
  Wire.write(0x00);                           //Set scale to 250dps, full scale
  Wire.endTransmission();                     //End the i2c transmission
  //Change accelerometer scale to +/-4g.
  Wire.beginTransmission(MPU6050_ADDR);       //My MPU6050 address is 0x68, change it at the begginning of the code
  Wire.write(0x1C);                           //Write on 0x1C register
  Wire.write(0x08);                           //Set scale to +/-4g
  Wire.endTransmission();                     //End the i2c transmission
  //Enable some filters
  Wire.beginTransmission(MPU6050_ADDR);       //My MPU6050 address is 0x68, change it at the begginning of the code
  Wire.write(0x1A);                           //Write on 0x1A register
  Wire.write(0x03);                           //Set Digital Low Pass Filter to ~43Hz
  Wire.endTransmission();                     //End the i2c transmission

  /*When we start, the gyro might have an offset value. We make 520 readdings and get that calibration value
  We use taht later in the code to substract the raw offset. */
  for (int i = 0; i < 520; i++) {                               //Create 520 loops
    if (i % 20 == 0){
      digitalWrite(LED1, !digitalRead(LED1));                   //Blink the LED every 20 loops
      digitalWrite(Buzzer, !digitalRead(Buzzer));               //Buzz every 20 loops
    }
    Wire.beginTransmission(MPU6050_ADDR);                       //Start i2c communication with MPU6050
    Wire.write(0x43);                                           //We read from register 0x43
    Wire.endTransmission();                                     //End the i2c transmission
    Wire.requestFrom(MPU6050_ADDR, 4);                          //Request 2 bytes from the MPU6050
    Gyro_Y_Offset += Wire.read() << 8 | Wire.read();            //Merge high and low byte and get an integer
    Gyro_X_Offset += Wire.read() << 8 | Wire.read();            //Merge high and low byte and get an integer
    delayMicroseconds(3500);                                    //Small delay
  }
  Gyro_X_Offset /= 520;                                         //Divide the total value by 520 to get the avarage gyro offset
  Gyro_Y_Offset /= 520;                                         //Divide the total value by 520 to get the avarage gyro offset

  delay(200);                                                   //Small Delay
  pinMode(Enable, OUTPUT);                                      //Set Enable pin as OUTPUT
  digitalWrite(Enable, LOW);                                    //Finally, we enable the stepper drivers (drivers are enabled with LOW)

  //Set the Loop_Time variable at the next end loop time
  Loop_Time = micros() + 4000;                                 //Loop time is 4000us

}

long oldPosition  = -999;




/////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////VOID LOOP////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
void loop() {
//  long Kd = myEnc.read()/8;
//  if (Kd != oldPosition) {
//    oldPosition = Kd;
//    Serial.println(Kd);
//  }


  if (Serial.available()) {                                     //If there is serial data available from HC-05 module
    Received_byte = Serial.read();                              //Store the received data
    Received_Since = 0;                                         //Reset the counter
  }
  if (Received_Since <= 25) {
    Received_Since ++;                                          //The received data wwill last 25 loops, around 100 milliseconds
  }
  else Received_byte = 0x00;                                    //After 100ms we reset the received data





  /////////////////////////////////////////////////////////////////////////////////////////////////
  /////////////////////////////////////////CALCUALTE ANGLE/////////////////////////////////////////
  /////////////////////////////////////////////////////////////////////////////////////////////////
  Wire.beginTransmission(MPU6050_ADDR);                           //Start communication with MPU6050
  Wire.write(0x3F);                                               //Start reading register 3F
  Wire.endTransmission();                                         //End i2c transmission
  Wire.requestFrom(MPU6050_ADDR, 2);                              //Request 2 bytes from the MPU6050
  Acc_Raw = Wire.read() << 8 | Wire.read();                       //Merge high and low byte and get an integer
  Acc_Raw += Acc_Offset;                                          //Add the accelerometer offset value
  if (Acc_Raw > 8200)Acc_Raw = 8200;                              //Prevent division by zero by limiting the acc data to +/-8200;
  if (Acc_Raw < -8200)Acc_Raw = -8200;                            //Prevent division by zero by limiting the acc data to +/-8200;

  Acc_Angle = asin((float)Acc_Raw / 8200.0) * 57.296;             //Calculate the current angle according to the accelerometer data

  if (Activated == 0 && Acc_Angle > -0.5 && Acc_Angle < 0.5) {    //If the accelerometer angle is almost 0
    Gyro_Angle = Acc_Angle;                                       //Load the accelerometer angle in the Gyro_Angle variable
    Activated = 1;                                                //Set "Activated" variable and start PID control
  }

  Wire.beginTransmission(MPU6050_ADDR);                           //Start communication with MPU6050
  Wire.write(0x43);                                               //Start reading register 43
  Wire.endTransmission();                                         //End i2c transmission
  Wire.requestFrom(MPU6050_ADDR, 4);                              //Request 4 bytes from the gyro
  Gyro_Y_Raw = Wire.read() << 8 | Wire.read();                    ////Merge high and low byte and get an integer
  Gyro_X_Raw = Wire.read() << 8 | Wire.read();                    ////Merge high and low byte and get an integer

  Gyro_X_Raw -= Gyro_X_Offset;                                    //Add the gyro offset value
  Gyro_Angle += Gyro_X_Raw * 0.000031;                            //Calculate traveled angle during this loop
  Gyro_Y_Raw -= Gyro_Y_Offset;                                    //Add gyro offset value
  Gyro_Angle = Gyro_Angle * 0.9996 + Acc_Angle * 0.0004;          //Correct the drift of the gyro angle with the accelerometer angle





  /////////////////////////////////////////////////////////////////////////////////////////////////
  ///////////////////////////////////////////PID CONTROL///////////////////////////////////////////
  /////////////////////////////////////////////////////////////////////////////////////////////////
  /*PID control si almos alwasy the same. We have a setpoint, which is the desired angle in this case (horizontal).
    The PID will adjsut the speed and direction of the motors, so we can always go towards the desired angle. To get
    the PID value, we use a PID algorithm formula.*/

  //First, we calculate the error between the real angle and the value taht we want, in this case would be 0º
  Temp_Error = Gyro_Angle - Auto_Setpoint - Setpoint;

  if (PID_Value > 10 || PID_Value < -10) {
    Temp_Error += PID_Value * 0.015 ;
  }

  //I value
  PID_I += Ki * Temp_Error;                                                 //Calculate the "I" value
  if (PID_I > 400)PID_I = 400;                                              //We limit the "I" to the maximum output
  else if (PID_I < -400)PID_I = -400;


  //Calculate the PID output value
  PID_Value = Kp * Temp_Error + PID_I + Kd * (Temp_Error - Last_D_Error);
  if (PID_Value > 400)PID_Value = 400;                                      //Limit the P+I to the maximum output
  else if (PID_Value < -400)PID_Value = -400;

  Last_D_Error = Temp_Error;                                                //Store the error for the next loop

  if (PID_Value < 6 && PID_Value > - 6)PID_Value = 0;                       //Dead-band where the robot is more or less balanced

  if (Gyro_Angle > 30 || Gyro_Angle < -30 || Activated == 0) {              //If the robot falls or the "Activated" is 0
    PID_Value = 0;                                                          //Set the PID output to 0 so the motors are stopped
    PID_I = 0;                                                              //Reset the I-controller memory
    Activated = 0;                                                          //Set the Activated variable to 0
    Auto_Setpoint = 0;                                                      //Reset the Auto_Setpoint variable
  }

  /////////////////////////////////////////////////////////////////////////////////////////////////
  /////////////////////////////////////////HC-05 CONTROL///////////////////////////////////////////
  /////////////////////////////////////////////////////////////////////////////////////////////////
  PID_Value_left = PID_Value;                               //Get PID output for the left motor
  PID_Value_right = PID_Value;                              //Get PID output for the right motor

  if (Received_byte & B00000001) {                          //We receive a a 00000001 so we turn Right
    PID_Value_left += Moving_Speed;                         //Increase the left motor speed
    PID_Value_right -= Moving_Speed;                        //Decrease the right motor speed
  }
  if (Received_byte & B00000010) {                          //We receive a a 00000010 so we turn Left
    PID_Value_left -= Moving_Speed;                         //Decrease the left motor speed
    PID_Value_right += Moving_Speed;                        //Increase the right motor speed
  }

  if (Received_byte & B00000100) {                          //We receive a a 00000100 so we go forward
    if (Setpoint > -2.5)Setpoint -= 0.05;                   //Change the setpoint angle so the robot leans forwards
    if (PID_Value > Max_Speed * -1)Setpoint -= 0.005;       //Change the setpoint angle so the robot leans forwards
  }
  if (Received_byte & B00001000) {                          //We receive a a 00001000 so we go backwards
    if (Setpoint < 2.5)Setpoint += 0.05;                    //Change the setpoint angle so the robot leans backwards
    if (PID_Value < Max_Speed)Setpoint += 0.005;            //Change the setpoint angle so the robot leans backwards
  }

  if (!(Received_byte & B00001100)) {                       //We receive a a 00001100 so no movement
    if (Setpoint > 0.5)Setpoint -= 0.05;                    //If the PID setpoint is higher than 0.5, reduce setpoint by 0.05 every loop
    else if (Setpoint < -0.5)Setpoint += 0.05;              //If the PID setpoint is lower than -0.5, increase setpoint by 0.05 every loop
    else Setpoint = 0;                                      //If the PID setpoint is lower than 0.5 or highert than -0.5, set the setpoint to 0
  }

  if (Setpoint == 0) {                                      //If the setpoint is zero degrees
    if (PID_Value < 0)Auto_Setpoint += 0.001;               //Increase the Auto_Setpoint if the robot is still moving forewards
    if (PID_Value > 0)Auto_Setpoint -= 0.001;               //Decrease the Auto_Setpoint if the robot is still moving backwards
  }


  /////////////////////////////////////////////////////////////////////////////////////////////////
  ////////////////////////////////////////MOTORS CONTROL///////////////////////////////////////////
  /////////////////////////////////////////////////////////////////////////////////////////////////
 
  if (PID_Value_left > 0){
    PID_Value_left = 405 - (1 / (PID_Value_left + 9)) * 5500;
  }
  else if (PID_Value_left < 0){
    PID_Value_left = -405 - (1 / (PID_Value_left - 9)) * 5500;
  }
  if (PID_Value_right > 0){
    PID_Value_right = 405 - (1 / (PID_Value_right + 9)) * 5500;
  }
  else if (PID_Value_right < 0){
    PID_Value_right = -405 - (1 / (PID_Value_right - 9)) * 5500;
  }

  //Calculate the pulse time for the left and right motor
  if (PID_Value_left > 0){
    left_motor = 400 - PID_Value_left;
  }
  else if (PID_Value_left < 0){
    left_motor = -400 - PID_Value_left;
  }
  else left_motor = 0;

  if (PID_Value_right > 0){
    right_motor = 400 - PID_Value_right;
  }
  else if (PID_Value_right < 0){
    right_motor = -400 - PID_Value_right;
  }
  else right_motor = 0;
 
  Left_Motor_Speed = left_motor;
  Right_Motor_Speed = right_motor;

 
  /*The angle calculations are tuned for a loop time of 4 milliseconds.
  //We make sure every loop is exactly 4 milliseconds so we set the Loop_Time to +4000 microseconds every loop.*/
  while (Loop_Time > micros());
  Loop_Time += 4000;
}//End of void loop









/////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////TIMER2_COMPA_vect////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
ISR(TIMER2_COMPA_vect) {
  //Left motor pulses
  CC_Speed_Left_Motor ++;                                       //Increase CC_Speed_Left_Motor by 1 every time this routine is executed
  if (CC_Speed_Left_Motor > Left_Motor_Speed_Prev) {            //If the number of loops is larger then the Left_Motor_Speed_Prev variable
    CC_Speed_Left_Motor = 0;                                    //Reset the CC_Speed_Left_Motor variable
    Left_Motor_Speed_Prev = Left_Motor_Speed;                   //Load the next Left_Motor_Speed variable
    if (Left_Motor_Speed_Prev < 0) {                            //If the Left_Motor_Speed_Prev is negative
      PORTD &= 0b11111011;                                      //Set D2 low. Reverse  direction
      Left_Motor_Speed_Prev *= -1;                              //Invert the Left_Motor_Speed_Prev variable
    }
    else PORTD |= 0b00000100;                                   //Set output D2 high. Forward direction.
  }
  else if (CC_Speed_Left_Motor == 1)PORTD |= 0b00001000;        //Set output D3 high to create a pulse for the stepper
  else if (CC_Speed_Left_Motor == 2)PORTD &= 0b11110111;        //Set output D3 low because the pulse only has to last for 20us



  //Right motor pulses
  CC_Speed_Right_Motor ++;                                      //Increase CC_Speed_Right_Motor by 1 every time the routine is executed
  if (CC_Speed_Right_Motor > Right_Motor_Speed_Prev) {          //If the number of loops is larger then the Right_Motor_Speed_Prev variable
    CC_Speed_Right_Motor = 0;                                   //Reset the CC_Speed_Right_Motor variable
    Right_Motor_Speed_Prev = Right_Motor_Speed;                 //Load the next Right_Motor_Speed variable
    if (Right_Motor_Speed_Prev < 0) {                           //If the Right_Motor_Speed_Prev is negative
      PORTD |= 0b00010000;                                      //Set output D4 low. Reverse the direction
      Right_Motor_Speed_Prev *= -1;                             //Invert the Right_Motor_Speed_Prev variable
    }
    else PORTD &= 0b11101111;                                   //Set D4 high. Forward direction.
  }
  else if (CC_Speed_Right_Motor == 1)PORTD |= 0b00100000;       //Set output D5 high to create a pulse for the stepper controller
  else if (CC_Speed_Right_Motor == 2)PORTD &= 0b11011111;       //Set output D5 low because the pulse only has to last for 20us
}//End of timer rroutine