//https://wokwi.com/projects/305936654686749250
//https://wokwi.com/projects/328312829780165204

#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <Wire.h>
#include <Servo.h>
Adafruit_MPU6050 mpu;

Servo servo1;
Servo servo2;

float w_x = 0; //angular velocity x-direction
float w_y = 0; //angular velocity y-direction
float r_x = 90; //angle rotated in x-direction as detected by IMU
float r_x_prev = 0;
float r_y = 90; //angle rotated in y-direction as detected by IMU
float r_y_prev = 0;
float s_x = 90; //Servo motor angle x-dirn
float s_y = 90; //Servo motor angle y-dirn

float a1 = 90; //a1 is the initial/ideal state angle
float e1=0; //x-dirn
float e2 = 0; //x-dirn
float start_i = 0; //angle at which integration begins x-dirn
float i=0; //integral term x-dirn
float e3=0; //y-dirn
float e4 = 0; //y-dirn
float start_i_2 = 0; //angle at which integration begins y-dirn
float i_2=0; //integral term y-dirn

float delay_time = 2; //in milliseconds

float subtract_abs(float no1, float no2){
  float b = no1-no2;
  if (b>0){
    return b;
  }
  else{
    return -b;
  }
}
void error (char q){
  if (q=='x'){
    e2 = r_x - s_x;
    if (start_i>0){
    i += 0.05*(e2 + e1);
    }
  }
  if (q=='y'){
    e4 = r_y - s_y;
    if (start_i>0){
    i_2 += 0.05*(e4 + e3);
    }
  }
}
void PID (char p){
  float a = 13.5;
  float c = 5.57;
  float d;
  if (p=='x'){
    if (start_i>0){
      d = 0.0024;
    }
    if (start_i==0 || e2==0){
      d = 0;
    }
    r_x_prev = r_x;
    r_x += (a*e2) + (c*(e2-e1)) + (d*i);
    e1 = e2;
  }
  if (p=='y'){
    if (start_i_2>0){
      d = 0.024;
    }
    if (start_i_2==0 || e4==0){
      d = 0;
    }
    r_y_prev = r_y;
    r_y += (a*e4) + (c*(e4-e3)) + (d*i_2);
    e3 = e4;
  }
}


void setup(void) {
  Serial.begin(115200);
  while (!Serial)
    delay(10); // will pause Zero, Leonardo, etc until serial console opens

  Serial.println("Adafruit MPU6050 test!");

  // Try to initialize!
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }
  Serial.println("MPU6050 Found!");

  servo1.attach(8);
  servo2.attach(7);
  
  Serial.println("");
  delay(100);
  servo1.write(90);
  servo2.write(90);
}

void loop() {
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  
  if (w_x>0){
    if(r_x<=r_x_prev+0.0001 || r_x>=r_x_prev-0.0001){
      start_i = 1;
    }
    if(r_y<=r_x_prev+0.0001 || r_y>=r_y_prev-0.0001){
      start_i_2 = 1;
    }
  }
  Serial.print("Angular velocity X: ");
  w_x = g.gyro.x * (180/3.14); //w_x degrees/sec
  Serial.print(w_x);
  Serial.print(" Y: ");
  w_y = g.gyro.y * (180/3.14); //w_y degrees/sec
  Serial.print(w_y);
  Serial.println(" degrees/sec");

  r_x += w_x * delay_time * 0.001;
  r_y += w_y * delay_time * 0.001;

  error('x');
  Serial.print("Error in x- axis angle is: ");
  Serial.println(e2);
  PID ('x');
  Serial.print("Output angle in X dirn is: ");
  Serial.println(r_x);

  error('y');
  Serial.print("Error in y- axis angle is: ");
  Serial.println(e4);
  PID ('y');
  Serial.print("Output angle in Y dirn is: ");
  Serial.println(r_y);

  s_x = r_x;
  s_y = r_y;

  servo1.write(s_x);
  servo2.write(s_y);
  delay(delay_time);
}