/*
  Forum: https://forum.arduino.cc/t/pid-loop-not-writing-over-error/1318034
  Wokwi: https://wokwi.com/projects/413660436808326145
   
  Original sketch from post #1 
   on   Wokwi: https://wokwi.com/projects/413546141063186433

  ec2021

*/

#include <Servo.h>
#include "MPU6050.h"
#define GYROSCALE 0.00763358778626
#define ACCELSCALE 0.00006103515625

MPU6050 accelgyro;
//Add the includes for the sensor libraries

//Setup the gains for your control loop here
const float KP = 0.001;
const float KI = 0.0;
const float KD = 0.0;

//Setup the variables for your control
Servo fin_servo; //create servo object to control a servo
float set_point = 0;
int16_t ax, ay, az;
int16_t gx, gy, gz;
float f_gx, f_gy, f_gz;
float ang_vel = 0;
float error;
float motor_output;
float angle_value;
int16_t bias;
long sum;
float f_angle;
float integral = 0;
float derivative;
float error_previous = 0;

//Setup the time recording variables, these are unsigned long as they can only be positive integers and can be quite large values.
unsigned long DT, end_time, start_time;

void setup() {
  // put your setup code here, to run once:
  //Setup serial port and baud rate, ensure baudrate in serial monitor matches up!
  Serial.begin(115200);
  //attach your servo motor to pin 8, check if your hardware uses pin 8 for the servo motor signal.
  fin_servo.attach(8);

  fin_servo.write(75);
    // get or set setpoint value
  set_point = 75; // desired angle in degrees, change this to what is required.


  //calibrate the sensor
  for (int i = 0; i < 500; i++) {
    accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
    sum = sum + gx;
    delay(10);
  }
  bias = sum / 500;
}

void loop() {
  // put your main code here, to run repeatedly:
  //get starttime
  start_time = millis();
  
  //get sensor value, this needs to be returned in angvelocity (this should be calibrated as well)
  accelgyro.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
  ang_vel = (gx - bias) * GYROSCALE;

  //integrate the angular vel to get angle.
  angle_value = angle_value + ang_vel * (DT / 1000.0) * (180.0/3.1415);

  //calc error
  error = set_point - angle_value;
    //calc integral error
  integral = integral + error * DT / 1000.0;
  //calc derivative error
  derivative = (error - error_previous) / DT / 1000.0;
  //calc motor output
  motor_output = KP * error;// + KI * integral + KD * derivative;

  //check if motor output is within its limits of the servo motor.

  if (150 >= motor_output && motor_output >= 30) {
    fin_servo.write(motor_output);
  }

  
  //set error_previous = error for derivative error calc next time around
  error_previous = error;

  //get the endtime
  end_time = millis();

  DT = end_time - start_time; //calc loop time, this should be less that 50ms but greater than 10ms

  //print DT to serial to check
  //printout values for debugging
  Serial.print("AngVel = ");
  Serial.print(ang_vel);
  Serial.print("\tAngle = ");
  Serial.print(angle_value);
  Serial.print("\tError = ");
  Serial.print(error);
  Serial.print("\tMotor = ");
  Serial.print(motor_output);
  Serial.print("\tDT = ");
  Serial.println(DT);
}