//CONTROL MOTOR POSITION

//PINS
const int channelA = 2;
const int channelB = 4;
int pul = 0;
const int enA = 10;
int enA_value = 0; //(0-255)
const int in1 = 8;
const int in2 = 9;
int direct = 0; // direct = 0: CCW, = 1: CW
float targetP = 20;
int i =0;
volatile float motorPosition = 0;
float previousPosition = -1;
//PID parameters
float kp = 0.5;
float ki = 3;
float kd = 1;
float controlsignal = 0; //u
float previousTime = 0;
float previousError = 0;
float errorIntegral = 0;
float currentTime = 0;
float deltaTime = 0;
float errorValue = 0;
float edot = 0;
void setup()
{
Serial.begin(9600);
pinMode(channelA,INPUT);
pinMode(channelB,INPUT);
attachInterrupt(digitalPinToInterrupt(channelA), checkEncoder, RISING);
}
void loop()
{
while (i < 6)
{
//forward
calculatePID();
driveMotor();
printValues();
//return
calculatePID ();
back ();
}
}
void calculatePID()
{
currentTime = micros();
deltaTime = (currentTime - previousTime)/1000000.0
previousTime = currentTime;
errorValue = motorPosition - targetP;
edot = (errorValue - previousError) / deltaTime; //Khau D
errorIntegral = errorIntegral + (errorValue * deltaTime); //Khau I
controlSignal = (kp * errorValue) + (kd * edot) + (ki * errorIntegral);
previousError = errorValue;
if (motorPosition == 30) {
i++;
 delay (1000);
motorPosition = 0;
}
}
void printValues()
{
Serial.println(i);
}
void back()
{
if (controlSignal < 0) //negative value: CCW
{
direct = 1;
}
else if (controlSignal > 0) //positive: CW
{
direct = -1;
}
else //0: STOP
{
direct = 0;
}
enA_value = (int)fabs(controlSignal);
if (enA_value > 255) //fabs() = floating point absolute value
{
enA_value = 255;
}

if (enA_value < 30 && errorValue != 0)

{
enA_value = 30;
}

if (direct == -1) //-1 == CCW

{
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
}
else if (direct == 1) // == 1, CW
{
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
}
else // == 0, stop/break
{
digitalWrite(in1, LOW);
digitalWrite(in1, LOW);
enA_value = 0;
}
analogWrite(enA, enA_value);
}
void driveMotor()
{
{
if (controlSignal < 0) //negative value: CCW
{
direct = -1;
}
else if (controlSignal > 0) //positive: CW
 {
direct = 1;
}
else //0: STOP
{
direct = 0;
}
enA_value = (int)fabs(controlSignal);
if (enA_value > 255) //fabs() = floating point absolute value
{
enA_value = 255;
}

if (enA_value < 30 && errorValue != 0)

{
enA_value = 30;
}

if (direct == -1) //-1 == CCW

{
digitalWrite(in1, LOW);
digitalWrite(in2, HIGH);
}
else if (direct == 1) // == 1, CW
{
digitalWrite(in1, HIGH);
digitalWrite(in2, LOW);
}
else // == 0, stop/break
{
digitalWrite(in1, LOW);
digitalWrite(in1, LOW);
enA_value = 0;
}
analogWrite(enA, enA_value);
Serial.print(errorValue);
Serial.print(" ");
}
void checkEncoder()
{
pul = digitalRead(channelB);

if (pul == 1) //CW direction

{
motorPosition++;
}
else //else, it is zero... -> CCW direction
{
motorPosition--;
}
}