// P = Kp.e(t)
//     Kp   = nilai konstanta proportional / parameter
//     e    = error selisih antara setpoint dan nilai aktual
//     t    = waktu
//     P    = setpoint - error/sensor

// I = Ki . 0(integral)t . e(t). dt
// I = Ki . (total error - error sekarang)
//     Ki   = Nilai Konstanta / parameter
//     e(t) = Nilai error
//     dt   = waktu error

// D = Kd . (de(t)/dt)
// D = Kd . (error sekarang - error sebelumnya)

#include <Stepper.h>
Stepper myStepper(400, 2, 3, 4, 5);

float kesalahan;
float integralKesalahan;
float derivatifKesalahan;
float kesalahanLalu = 0;
float Kp, Ki, Kd;
float input;

 
void setup() 
{
  Serial.begin(115200);

  myStepper.setSpeed(100);

  input = 25;
  Kp = 0;
  Ki = 0;
  Kd = 0;
}
 
void loop() 
{
  //float sensor = random(25,45);
  float sensor = analogRead(A0);
  sensor = map(sensor,0,1023,20,45);

  float respon = hitungPID(input, sensor);

  
  if(respon <=0)
  {
    respon = 0;
  }

  if(respon >= 20)
  {
    respon = 20;
  }
  
  if(sensor <= 25)
  {
    Kp = 0;
    Kd = 0;
  }
  if(respon >= 20)
  {
    Kp = Kp;
    Kd = Kd;
  }
  if(respon <= 0 || respon >= 0 && respon <= 19)
  {
    Kp = Kp - 0.01;
    Kd = Kd + 0.01;
  }

  myStepper.step(respon);

  Serial.print(sensor);
  Serial.print("||");
  Serial.print(Kp);
  Serial.print(",");
  Serial.print(Ki);
  Serial.print(",");
  Serial.print(Kd);
  Serial.print("||");
  Serial.print(input);
  Serial.print(",");
  Serial.print(sensor);
  Serial.print(",");
  Serial.println(respon);
  delay(10);
}
 
float hitungPID(float input, float sensor)
{
  kesalahan = input - sensor;
  integralKesalahan += kesalahan;
  derivatifKesalahan = kesalahan - kesalahanLalu;
  kesalahanLalu = kesalahan;
   
  return (Kp * kesalahan) + (Ki * integralKesalahan) + (Kd * derivatifKesalahan);
}