// for https://forum.arduino.cc/t/how-to-control-a-servo-motor-with-potentiometer-using-pid/1109937
// https://wokwi.com/projects/361132949689682945

/*
Note that the output slide pot is not attched to the servo,
so you need to move the output slide pot manaally.

*/
#include <Servo.h>

Servo myServo;
const int inputFlexPin = A0;
const int outputFlexPin = A1;
const int servoPin = 9;

float Kp = 0.1, Ki = 0, Kd = 0; // Kp=1 means  1degreeServoChange/(1ADCcountError)

float servoState = 0;

int desiredPosition = 90; // the position we want the servo to be in
int currentPosition = 90; // the current position of the servo
int previousError = 0; // the error from the previous iteration of the control loop
int integralError = 0; // the sum of all the errors so far

void setup() {
  myServo.attach(servoPin); // attach servo to pin 9
  pinMode(inputFlexPin, INPUT); // setup analog input for potentiometer
  pinMode(outputFlexPin, INPUT); // setup analog input for potentiometer
  myServo.write(servoState);
  Serial.begin(115200);
}

int SetPoint = 0;
int OutputValue = 0;
float controlSignal = 0;

void loop() {
  unsigned long interval = 100;
  static unsigned long lastPID = 0;
  unsigned long now = millis();
  if (now - lastPID >= interval) {
    lastPID += interval;
    SetPoint = analogRead(inputFlexPin); // read the value from the potentiometer
    OutputValue = analogRead(outputFlexPin); // read the value from the potentiometer

    desiredPosition = map(SetPoint, 0, 1023, 0, 180); // map the potentiometer value to a servo position

    int error = SetPoint - OutputValue; // calculate the error between the desired position and the current position
    integralError += error; // add the error to the integral error term
    int derivativeError = error - previousError; // calculate the derivative of the error

    // calculate the control signal (in this case, the servo position) using a PID controller
    controlSignal = Kp * error + Ki * integralError + Kd * derivativeError;

    previousError = error; // store the current error for the next iteration of the control loop

    servoState = constrain(servoState + controlSignal, 0, 180);
    //Serial.print('p');
    // update the servo position
    myServo.write(servoState);
    currentPosition = controlSignal;
  }



  delay(20); // wait for 20 milliseconds before running the control loop again

  report();
}

void report(void) {
  unsigned long interval = 250;
  static unsigned long last = -interval;
  if (millis() - last < interval) return;
  last += interval;
  Serial.print("SP:");
  Serial.print( SetPoint);
  Serial.print(" PV:");
  Serial.print( OutputValue);
  Serial.print(" CV:");
  Serial.print( controlSignal,2);
  Serial.print( " Servo°:");
  Serial.print(servoState);
  Serial.println();
}