#include <analogWrite.

h> //library for analog write with esp32

#include <PID_AutoTune_v0.h>//-----------------------------------------------------
double kpmodel=1.5, taup=100, theta[50];//-------------------------------------
double outputStart=5;//------------------------------------
byte ATuneModeRemember=2;////-------------------------------------
double aTuneStep=50, aTuneNoise=1,
aTuneStartValue=100;//-------------------------------------
unsigned int aTuneLookBack=20;//-------------------------------------
boolean tuning = true;//------------------------------------
unsigned long modelTime, serialTime;//------------------------------

#include <ESP32Encoder.h> // encoder library

#include <PID_v1.h> // PID library

//#define PIN_OUTPUT 3
ESP32Encoder encoder; //encoder created by the name "encoder"

//Define Variables we'll be connecting to

double Setpoint, Input, Output;// variable created mus be double

const int dirA=4; // only this pin is for directio

const int dirB=27; // this pin is used as enable on motor driver

//Specify the links and initial tuning parameters

double Kp=2, Ki=0.5, Kd=2; // tune PID value here
PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT); //PID creaded by the
name "myPID"
PID_ATune aTune(&Input, &Output);//-------------------------------------
boolean useSimulation = false;//-------------------------------------
void setup(){

Serial.begin(115200); // serial begin

// Enable the weak pull up resistors
ESP32Encoder::useInternalWeakPullResistors=UP;

// use pin 19 and 18 for the first encoder

encoder.attachHalfQuad(19, 18); // these are the pins of encoder connection

// set starting count value after attaching

encoder.setCount(0); // on startup the encoder value will be 0

Serial.println("Encoder Start = " + String((int32_t)encoder.getCount()));

pinMode(4, OUTPUT);
pinMode(2, OUTPUT);
pinMode(27, OUTPUT);
//initialize the variables we're linked to
Setpoint = 2000; // target value to be achieved by the motor
Input= encoder.getCount(); // this is the input of PID which is encoder in
this case
//turn the PID on
myPID.SetMode(AUTOMATIC); //
myPID.SetSampleTime(10); // pid update time in millis
myPID.SetOutputLimits(-255, 255); // pid output range.

analogWriteResolution(LED_BUILTIN, 12); // this is setting of analog write

library, leave as is for pin 2
 // if other analog pin is to be used then change LED_BUILTIN to desired pin
number

if(useSimulation)
{
for(byte i=0;i<50;i++)
{
theta[i]=outputStart;
}
modelTime = 0;
}
if(tuning)
{
tuning=false;
changeAutoTune();
tuning=true;
}
serialTime = 0;
}

void loop(){
Input = encoder.getCount(); // update the input value with new encoder value
myPID.Compute(); // compute PID

//Serial.println(Setpoint-Input); //this show how much target is remaining or

error
//Serial.println("Output value from PID = " + String(Output));

if(Output<0){ //this is CW direction

digitalWrite(dirA,LOW); // low is CW direction
digitalWrite(dirB,HIGH); // dir B is enable pin
analogWrite(2, abs(Output)); // motor pwm is pwm
}
else if(Output>=0){ // this ccw direction
digitalWrite(dirA,HIGH);
digitalWrite(dirB,HIGH);
analogWrite(2, Output);
}

//---------------------------------------------------------------------
unsigned long now = millis();

if(!useSimulation)
{ //pull the input in from the real world

Input = encoder.getCount();
}

if(tuning)
{
byte val = (aTune.Runtime());
if (val!=0)
{
tuning = false;
}
if(!tuning)
{ //we're done, set the tuning parameters
Kp = aTune.GetKp();
 Ki = aTune.GetKi();
Kd = aTune.GetKd();
myPID.SetTunings(Kp,Ki,Kd);
AutoTuneHelper(false);
}
}
else myPID.Compute();

if(useSimulation)
{
theta[30]=Output;
if(now>=modelTime)
{
modelTime +=100;
DoModel();
}
}
else
{
analogWrite(2,Output);
}

//send-receive with processing if it's time

if(millis()>serialTime)
{
SerialReceive();
SerialSend();
serialTime+=500;
}//-----------------------------------------------------------------------
}

//++++++++++++++++++++++++++++++++++
+====================================================
void changeAutoTune()
{
if(!tuning)
{
//Set the output to the desired starting frequency.
Output=aTuneStartValue;
aTune.SetNoiseBand(aTuneNoise);
aTune.SetOutputStep(aTuneStep);
aTune.SetLookbackSec((int)aTuneLookBack);
AutoTuneHelper(true);
tuning = true;
}
else
{ //cancel autotune
aTune.Cancel();
tuning = false;
AutoTuneHelper(false);
}
}

void AutoTuneHelper(boolean start)

{
if(start)
ATuneModeRemember = myPID.GetMode();
 else
myPID.SetMode(ATuneModeRemember);
}

void SerialSend()
{
Serial.print("setpoint: ");Serial.print(Setpoint); Serial.print(" ");
Serial.print("input: ");Serial.print(Input); Serial.print(" ");
Serial.print("output: ");Serial.print(Output); Serial.print(" ");
if(tuning){
Serial.println("tuning mode");
} else {
Serial.print("kp: ");Serial.print(myPID.GetKp());Serial.print(" ");
Serial.print("ki: ");Serial.print(myPID.GetKi());Serial.print(" ");
Serial.print("kd: ");Serial.print(myPID.GetKd());Serial.println();
}
}

void SerialReceive()
{
if(Serial.available())
{
char b = Serial.read();
Serial.flush();
if((b=='1' && !tuning) || (b!='1' && tuning))changeAutoTune();
}
}

void DoModel()
{
//cycle the dead time
for(byte i=0;i<49;i++)
{
theta[i] = theta[i+1];
}
//compute the input
Input = (kpmodel / taup) *(theta[0]-outputStart) + Input*(1-1/taup) +
((float)random(-10,10))/100;