#include <Encoder.h>
Encoder myEnc(2,4); // initiating tbe encoder pins of the system.
//Define Variables
const int
PWM_A = 3,
DIR_A = 12,
BRAKE_A = 9,
SNS_A = A0;
//Working variables//
double Output;
double errSum = 0;
int lastErr = 0;
long interval = 10;
int Input;
int error = 0;
double Setpoint = 1200;
unsigned long LastTime = 0;
int maxOutput = 255;
long oldPosition = -999;
unsigned long now = 0;
//Controller Parameter//
double kp = 3.50;
double ki = 0.0001;
double kd = 200.00;
void setup() {
pinMode(BRAKE_A, OUTPUT); // Brake pin on channel A
pinMode(DIR_A, OUTPUT); // Direction pin on channel A
Serial.begin (9600);
Serial.println("Start");
}
void compute()
{
Input = (myEnc.read());
/* How long since we last calculated the PID system*/
now = millis();
Serial.println("------");
double dt = (double)(now - LastTime); //Sampling time
/*Now to compute all working error variables*/
double error = (Setpoint - Input );
Serial.println(error);
errSum += (error * dt); //Intergral error
Serial.println(errSum);
double der_error = (error - lastErr)/ dt; // Derivative error
Output = (kp * error) + (ki * errSum) + (kd * der_error);
//This part constrain the output to its maximum and minimum value//
Output = constrain(Output, -maxOutput, maxOutput);
Serial.println(Output);
if (error < 0 ) {
digitalWrite(DIR_A, LOW);
analogWrite(3, abs(Output));
}
else {
digitalWrite(DIR_A, HIGH);
analogWrite(3, abs(Output));
}
//These code lines below will update for the next pid loop//
lastErr = error;
LastTime = now;
}
void loop()
{
unsigned long currentMillis = millis();
if (currentMillis - LastTime > interval) {
//Calling the sub function for the PID//
compute();
LastTime = currentMillis;
}
long newPosition = myEnc.read();
if (newPosition != oldPosition) {
oldPosition = newPosition;
Serial.println(newPosition);
}
}
I have displayed the completed code without the anti windup techniques. The code compiles and runs the motor but i receive a few problems when checking the serial monitor.
Problem 1) : The serial print of the "error" is not zero when the motor stops moving despite the new position of the encoder being the set point of the system.
problem 2): The integral error continually sums up past the point when the motor has stop moving and the serial monitor no longer prints the new position of the encoder.
Problem 3): The combination of these errors causes the output to give a value to the motor but it would not move past the set point.
