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

#include <PWM.h>
#include <PID_v1.h>

int32_t frequency = 35000;  //frequency (in Hz)

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

//Specify the links and initial tuning parameters
double Kp=3, Ki=1, Kd=0;  // 2 5 1
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT);  

void setup()
{
  Serial.begin(9600);
  InitTimersSafe(); 
  bool success = SetPinFrequencySafe(3,frequency);
    
  Setpoint = 500;
  //turn the PID on
  myPID.SetMode(AUTOMATIC);
}

void loop()
{
  Input = analogRead(A0);
 // Serial.println(Input); 
  myPID.Compute();
 // analogWrite(PIN_OUTPUT, Output);
  pwmWrite(3, Output);
 // Serial.println(Output);  
}