/********************************************************
 * PID Basic Example
 * Reading analog input 0 to control analog PWM output 3
 ********************************************************/

#include <PID_v1.h>

#define PIN_INPUT 0
#define PIN_OUTPUT 3

//Define Variables we'll be connecting to
double Setpoint, Input, Output;

//Specify the links and initial tuning parameters
double Kp=2, Ki=5, Kd=1;
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);
unsigned long lastPrint = millis();
void setup()
{
  Serial.begin(115200);
  //initialize the variables we're linked to
  Input = analogRead(PIN_INPUT);
  Setpoint = analogRead(A1);

  //turn the PID on
  myPID.SetMode(AUTOMATIC);
}

void loop()
{
  Input = analogRead(PIN_INPUT);
  Input = map(Input, 0, 1023, 0, 25);
  myPID.Compute();
  analogWrite(PIN_OUTPUT, Output);
  int valSet = map(analogRead(A1), 0, 1023, 0, 20);
  if(valSet != Setpoint){
    Setpoint = valSet;
    Serial.print("New Setpoint:");
    Serial.println(Setpoint);
  }
  if((millis() - lastPrint) > 1000){
    Serial.print("Input:");
    Serial.print(Input);
    Serial.print(", PID Output:");
    Serial.println(Output);
    lastPrint = millis();
  }
}