#include <analogWrite.

h> //library for analog write with esp32

#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

float ankle_angle[70] = { -0.4, -2.5, -4.7, -6.5, -7.6, -7.8, -7.2, -6, -4.6, -3.1,
-1.8, -0.6, 0.4, 1.2, 2.1, 2.8, 3.4, 3.9, 4.2, 4.6, 4.9, 5.3, 5.9, 6.4, 6.7, 6.9,
6.9, 6.9, 6.8, 6.8, 6.8, 6.5, 5.9, 4.8, 3.2, 1.2, -1.4, -4.4, -8, -11.8, -15.4, -
18.3, -20.1, -20.5, -19.8, -18.2, -16.2, -14, -11.8, -9.9, -8.1, -6.5, -5.2, -4.1,
-3.2, -2.5, -2, -1.6, -1.3, -1.1, -0.8, -0.4, 0.1, 0.7, 1.3, 1.7, 1.7, 1.2, 0, };
float time_step[71] = {0, 0.014, 0.029, 0.043, 0.057, 0.072 ,0.086, 0.1 ,0.114,
0.129, 0.143, 0.157, 0.172, 0.186, 0.2, 0.215, 0.229, 0.243, 0.257, 0.272, 0.286,
0.3, 0.315, 0.329, 0.343, 0.358, 0.372, 0.386, 0.4, 0.415, 0.429, 0.443, 0.458,
0.472, 0.486, 0.501, 0.515, 0.529, 0.543, 0.558, 0.572, 0.586, 0.601, 0.615, 0.629,
0.644, 0.658, 0.672, 0.686, 0.701, 0.715, 0.729, 0.744, 0.758, 0.772, 0.787, 0.801,
0.815, 0.829, 0.844, 0.858, 0.872, 0.887, 0.901, 0.915, 0.93, 0.944, 0.958,
0.972,0.986};
int delta_t = 0;
const int dirA=4; // only this pin is for directio
const int dirB=27; // this pin is used as enable on motor driver
int i = 0;
int n = 0;
int timeloop;
int prevtime;
//Specify the links and initial tuning parameters

////////////////////////////////////////////////////

const byte numChars = 32;

char receivedChars[numChars]; // an array to store the received data

boolean newData = false;

double dataNumber = 0; // new for this version

///////////////////////////////////////////////////

double Kp=1.5, Ki=0 , Kd=0; // tune PID value here

PID myPID(&Input, &Output, &Setpoint, Kp, Ki, Kd, DIRECT); //PID creaded by the
name "myPI

void setup(){

Serial.begin(250000); // 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
timeloop= millis();
Setpoint = 0; // 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.
//Serial.println("LABEL,Time,Counter,millis,encoder value,desired value");
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

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

myPID.Compute(); // compute PID

timeloop = millis();
//Setpoint = 2200;
//Serial.print(encoder.getCount());
//Serial.print(",");Serial.println(Setpoint);

//Serial.print("value of kp =");Serial.print(Kp);Serial.print("actual value

=");Serial.print(encoder.getCount());Serial.print("speed out put
=");Serial.println(Output);

if (millis() > prevtime + delta_t + 0 ){//prevtime + delta_t

//Serial.print("encoder actual value is =");
//Serial.println(encoder.getCount());

prevtime = millis();
Setpoint = ankle_angle[n]/0.011;
//Setpoint = Setpoint + 2200;
delta_t = (time_step[n+2] - time_step[n])*1000;
//Serial.print("Set point value is =");
//Serial.println(Setpoint);
//Serial.println(millis());

//Serial.print("DATA,TIME,");
//Serial.print(i++);
Serial.print(",");Serial.print(millis());Serial.print(",");
Serial.print(encoder.getCount());
Serial.print(",");Serial.println(Setpoint);
n+2;
if(n >= 69){
n=0;
}
}
//Serial.println(Setpoint);
 //Serial.println(encoder.getCount());
if(Output<0){ //this is CW direction
digitalWrite(dirA,HIGH); // 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,LOW);
digitalWrite(dirB,HIGH);
analogWrite(2, Output);
}
//Kp=1;
//delay(100);
recvWithEndMarker();
showNewNumber();

void recvWithEndMarker() {
static byte ndx = 0;
char endMarker = '\n';
char rc;

if (Serial.available() > 0) {
rc = Serial.read();

if (rc != endMarker) {
receivedChars[ndx] = rc;
ndx++;
if (ndx >= numChars) {
ndx = numChars - 1;
}
}
else {
receivedChars[ndx] = '\0'; // terminate the string
ndx = 0;
newData = true;
}
}
}

void showNewNumber() {
if (newData == true) {
dataNumber = 0; // new for this version
dataNumber = atof(receivedChars); // new for this version
Kp = dataNumber;
//Kp = static_cast<double>(dataNumber);
//Serial.print("This just in ... ");
//Serial.println(receivedChars);
//Serial.print("Data as Number ... "); // new for this version
//Serial.println(dataNumber); // new for this version
newData = false;
myPID.SetTunings(Kp, Ki, Kd);
}
}