Using PID Control for motor control

int gearRatio = 298;
int ppr = 3;

//motor 1 
const int enablePin1 = 8;
const int pin1 = 10;
const int pin2 = 12;

const byte interruptPin = 3;
const int pinEncoder = 9; //interrupt pin 

//PID constants 

double Kp1 = 18.7;
double Kd1 = 2.11;
double Ki1 = 14.57;

double demandPosition = 0;
volatile float currentPosition = 0;
double errorPosition = 2;
double errorPosition_diff = 0;
double errorPosition_sum = 0;
double errorPosition_prev = 0;
double Output = 0;
double integral;

volatile long int Position = 0; //count ticks from encoder 

float degPosition = 0.0; 
float radPosition = 0.0;

int flag = 0;

//  Initialize timer
long t = 0;
long t0 = micros(); 
const int freq = 1000;
//  Calculate sampling time in us
const int dt = round((1.0/(float)freq)*1000000.0); 

void setup () {
  digitalWrite(enablePin1, HIGH);
  pinMode(pin1, OUTPUT);
  pinMode(pin2, OUTPUT);

  pinMode(interruptPin, INPUT_PULLUP);
  pinMode(pinEncoder, INPUT_PULLUP);
  attachInterrupt(digitalPinToInterrupt(interruptPin), encoder, RISING);

void loop () {
  if (flag==0) {
  if (t>dt) {
        if(degPosition>=360.0) {
        Position = 0; 
        if (degPosition<=-360.0) {
        Position = 0; 
        degPosition = (Position*360.0/(ppr*gearRatio));
        radPosition = degPosition*0.0174532925;

        demandPosition = 90.0;
        currentPosition = degPosition;

        errorPosition = demandPosition-currentPosition;

        Serial.print(currentPosition, 3);
        errorPosition_diff = (errorPosition-errorPosition_prev)/dt;
        errorPosition_sum += errorPosition;
        integral = errorPosition_sum*dt*Ki1;
        if (integral > 255) {integral = 255;}
        if (integral < -255) {integral = -255;}

        Output = errorPosition*Kp1 + errorPosition_diff*Kd1 + integral;

        errorPosition_prev = errorPosition;

        if (Output >= 0) {
        if(Output>255) {Output = 255;}
        else if(Output<30) {Output = Output+30;}
        else {
        if(Output <-255) {Output = -255;}
        else if(Output>-30) {Output = Output-30;}

        Serial.print(" This is the Output: ");

        if (Output >= 0) {
          analogWrite(pin1, round(Output));
          analogWrite(pin2, 0);
        } else {
          analogWrite(pin2, round(-Output));
          analogWrite(pin1, 0); 
        t0 = micros();
     else {
      t = micros()-t0;
  digitalWrite(enablePin1, LOW);
  flag = 1;

void encoder() {
  }else {

I am trying to implement PID control for position of a DC motor with gearbox and encoder. PD works pretty fine. When I tried to add the "I" term however it seems not working. Maybe I am doing something wrong? Thanks for the help

You don't make it easy for the helpers! Where is a '|' Term?

in general, the integral term is not necessary for positioning.

presumably the goal is to drive the motor to a position. the proportional term looks at the difference in position to determine the motor voltage. the drawback is it's likely to cause overshoot.

the differential term measures the speed and subtracts it from the proportional value to slow the motor down as it approaches the position.

if speed were being maintained, and there is no error and hence no motor voltage, the speed would drop to zero. this is where the integral term is used to maintain a speed when there is not error and a purely analog circuit is used

this is why an integral term is not need for positioning

I mean the Integral term. So when my output equations uses just P or PD gain it works, however when I try to add the integral part to the output it doesn't work.

Hi @kclprectech .
why don't you use the PID library for arduino?

RV mineirin

I understand. However, its for a coursework at Uni and we need to control the position with PID so I assumed it should work also adding the integral part? With P or PD I reach the desired position with minimal overshoot, when I add the integral seems to oscillate a lot and doesn't reach to the position at all

How did you pick your coefficients?

I am not sure how to use the library. Your read from what pin and the output goes to which other pin? Because I have two encoders pins and also 2 pins for motor direction

mathematically and then trial and error

It seems then that it's functioning, but your tuning of the I term still needs some work. IIRC, Wikipedia has a decent article on PID tuning.

you may be interested in Adding Encoder to Older Walthers Turntable to Support Indexing and code

the coefficients i settled on were Kp of 2.0 and Kd of 0.11.

once i got it to roughly stop at a target with overshoot, i played with Kd to stop with minimal overshoot. i made the mistake of initially adding the derivative term, but once I substracted it, it didn't take long to see a dramatic improvement

Check through how you're working with the I term ... it seems to be accounted for more than once and looks like there could be windup issues.

In your code, you have:

Output = errorPosition * Kp1 + errorPosition_diff * Kd1 + integral;
errorPosition_diff = (errorPosition - errorPosition_prev) / dt;
errorPosition_sum += errorPosition;
integral = errorPosition_sum * dt * Ki1;

I previously had some iTerm windup in my library but now have it resolved ... maybe take a look at the compute function here (near the top of the page) .

Check the comments in this link for a "trick" to improve reset windup (I used something like this for my solution): Improving the Beginner’s PID: Reset Windup

I am able to make the code stop with minimal overshoot using the P or PD control. But the problem occurs when adding the Integral term. Do you see any issues in the code?

what do you mean to be accounted for more than once?

You may well find that adding integral may require you to reduce the proportional term to maintain stability .

With proportional only there will be an offset to to the set point when it settles and the integral term takes care of that . On proportional only you are aiming for stability consistent with “fast” response, not particularly reaching the set point .
Get p & I working first , then add the D to control overshoot if needed ( approaching the set point too fast )
Bear in mind sampling times will have an effect as will any lags in the system caused by the mass of your motor /gearbox . Lags are a real problem and you may have to tune so the P is fairly low and there is no overshoot.

At the set point the proportional term goes to zero ( there is no error ) and the integral term is holding up the output , derivative is also then zero .

PID is based on the absolute error. not sure why you're calculating and errorPosition_sum

integral = errorPosition_sum*dt*Ki1;

if you needed the integral term, i believe it should be

integral += errorPosition * dt * Ki1;

i forgot about dt. shouldn't you use it consistently? shouldn't the derivative value be

(errorPosition-errorPosition_prev) * dt * Kd1

and the proportional value be

errorPosition * dt * Kp1 

the errorPosition_sum will calculate the absolute error as it adds the error for every loop. Derivative you divide by dt and Proportional doesn't need it

Why create errorPosition windup? Now the PID will increasingly over-react as the actual error becomes smaller ... but the over reaction will create larger errors. End result is oscillation I guess.

Looked more closely and yeah, I guess you're using the integral once per PID loop iteration, but I still suspect there's integral windup.

Have you tried plotting your results and PID terms?

Could do a windup test by making Kd=0 (PI controller), then compare to a P controller (Ki=0, Kd=0).

isn't the absolute error the absolute value of the difference in the current and target position? the value the proportional term is determined from?

if you sum the error on each iteration, the value will exceed the initial.

consider the following, starting 10 units from a target at zero, what the error is at each step and what the sum of the error is

  10 pos  10 err  10 sum
   9 pos   9 err  19 sum
   8 pos   8 err  27 sum
   7 pos   7 err  34 sum
   6 pos   6 err  40 sum
   5 pos   5 err  45 sum
   4 pos   4 err  49 sum
   3 pos   3 err  52 sum
   2 pos   2 err  54 sum
   1 pos   1 err  55 sum

yes that is exactly what I do. So I shouldn't sum the error at each iteration? And yes sorry the P term is determined by the absolite error as you mentioned