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

#include "PID_v1.h"

#define PIN_INPUT A0
#define PIN_SP A1
#define PIN_OUTPUT 3

//Define Variables we'll be connecting to
double Setpoint, Input, Output, Err;
int vSetpoint, vInput;


//Specify the links and initial tuning parameters
float Ti = 5;
double Kp=2, Ki=1/Ti, Kd=0;
 int pOnE = 1;    //diff part on error
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, pOnE, DIRECT);

//***********************
// subrutin Tampilkan
//***********************
void Tampilkan(){
  Serial.print(Setpoint); Serial.print(",\t"); 
  Serial.print(Input); Serial.print(",\t");
  Serial.print(Err); Serial.print("\t");
  Serial.println(Output); 
}

void setup()
{
  //initialize the variables we're linked to
  Serial.begin(9600);
  // Input=500;
  // Setpoint=551.2;
  Input = analogRead(PIN_INPUT);
  Setpoint = analogRead(PIN_SP);

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

void loop()
{
  vInput = analogRead(PIN_INPUT);  
  vSetpoint = analogRead(PIN_SP);
  Input=map(vInput,0,1023,0,100);
  Setpoint=map(vSetpoint,0,1023,0,100);

  // Input=50.0;
  // Setpoint=55.0;

  Input = (float) vInput * 100/1024 ;
  Setpoint = (float) vSetpoint *100/1024;

  Err=Setpoint-Input;
  myPID.Compute();
  analogWrite(PIN_OUTPUT, Output);
  Tampilkan();
}