#include <LiquidCrystal.h>
#include <Servo.h>

LiquidCrystal lcd(13, 12, 11, 10, 9, 8);
Servo servo;
int servoPin = 6;
int pot = A0;

int temp;
int effort;
const float cP = 1.8;
const float cI = 1.2;
const float cD = 2;
int setPoint = 70;
int interval = 100;

void prints(int err, int tmp, int itgr, int eff, int delt)
{
  lcd.clear();
  lcd.setCursor(3,0);
  lcd.print("PID CONTROLLER");
  lcd.setCursor(0,1);
  lcd.print("err =" + (String)err);
  lcd.setCursor(12,1);
  lcd.print("temp=" + (String)tmp + "'");
  lcd.setCursor(0,2);
  lcd.print("itgr=" + (String)itgr);
  lcd.setCursor(12,2);
  lcd.print("eff =" + (String)eff);
  lcd.setCursor(0,3);
  lcd.print("delt=" + (String)delt);
  lcd.setCursor(12,3);
  lcd.print("deg =" + (String)eff + "'");
}

void PID(int input, int &output, float cPIn, float cIIn, float cDIn, int setPointIn, int intervalIn)
{
  static float Error, preError;
  static float integralError;
  static float deltaError;
  static float Proportional, Integral, Derivative;
  static unsigned long preTime;

  if(millis() - preTime >= intervalIn)
  {
    Error = setPointIn - input;
    integralError += ((Error + preError) * (float(intervalIn) / float(1000))) / 2;
    deltaError = abs(Error - preError);

    Proportional = cP * Error;
    Integral = cI * integralError;
    Derivative = cD * deltaError;

    output = (Proportional + Integral + Derivative);

    prints(Error, input, integralError, output, deltaError);
    preError = Error;
    preTime = millis();
  }
}

void setup()
{
  lcd.begin(20,4);
  servo.attach(servoPin);
  pinMode(pot, INPUT);
  servo.write(0);
}

void loop()
{
  // temp = map(analogRead(pot), 0, 1023, 0, 180);
  setPoint = map(analogRead(pot), 0, 1023, 0, 100); temp = 20 + (effort * 0.5);
  PID(temp, effort, cP, cI, cD, setPoint, interval);
  servo.write(effort);
}