PID Tuning Error

Hello! I am trying to tune my PD, but I seem to get some type of issue: no matter my Kd constant the output always looks the same. I am not an expert, but I believe there has to be an error my output is always the same right? I have plotted on excelt the current position with Kp = 16.99 and Kd= 1, 2.11 and 22.11 (random values) and the three curve just overlap.

 
#include <MatrixMath.h>
#define pi 3.1415926535897932384626433832795

int gearRatio = 298;
int ppr = 3;
//motor 1 
const int enablePin1 = 4;
const int pin1 = 6;
const int pin2 = 7;


//encoder 
const byte interruptPin = 2;
const int pinEncoder = 5; //interrupt pin 

//PID constants 

double Kp1 = 16.99;
double Kd1 = 2.11;
double Ki1 = 8;

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 () {
  
  Serial.begin(115200);
  pinMode(enablePin1,OUTPUT);
  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) {
  while(t0<4000000){
  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;//update current encoder reading
        errorPosition = demandPosition-currentPosition;

        
        //Serial.print(demandPosition, 3);
        //Serial.print("\t");
        Serial.println(currentPosition, 3);
        //Serial.print("\t");
        //Serial.print(errorPosition, 3);
        //Serial.print("\t");
     
          errorPosition_diff = (errorPosition-errorPosition_prev)/t;
          //errorPosition_sum += errorPosition*Ki1*t;
          //if (errorPosition_sum>100) {errorPosition_sum = 100;}
          //if (errorPosition_sum <-100) {errorPosition_sum = -100;}
       
       Output = errorPosition*Kp1+ errorPosition_diff*Kd1; 
        
  
       errorPosition_prev = errorPosition;

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

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

        if (Output >= 0) {
          analogWrite(pin1, round(Output));
          analogWrite(pin2, 0);
          //Serial.println("clockwise");
        } else {
          analogWrite(pin2, round(-Output));
          analogWrite(pin1, 0); 
          //Serial.println("Counterclockwise");
        }
        t0 = micros();
        //t = micros()-t0;
   }
     else {
      //t0 = micros();
      t = micros()-t0;
      Serial.println("t is smaller than dt");
     }
  }
  digitalWrite(enablePin1, LOW);
  flag = 1;
  }
}

void encoder() {
  if(digitalRead(pinEncoder)==LOW){
    Position++;
  }else {
    Position--;
  }
}

Thank you!

Your code is very difficult to follow, and if you are having trouble getting PID to run at all, manipulations like these are NOT advised.

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

Start simply and get the basic process running sensibly before imposing restrictions. You should consider using the Arduino PID library, as the math is done correctly.

Finally, for a process that is measured around in a circle (for example, navigational direction in degrees or rotation angle on any scale), compass wrap is usually avoided by making the PID setpoint to be zero, and measuring the position error (in degrees) as

error = (current_angle-desired_angle + 540)%360 - 180;`//integer error ranges from -180 to 179

The PID actually works and the error minimizes as expected. However, not much difference its noted while tuning the constants. I am not sure what you refer to with the last equation (
error = (current_angle-desired_angle + 540)%360 - 180;`//integer error ranges from -180 to 179
)

As its a school project I need to avoid using the PID library

I’m not sure you have enough digits of π in there.

Oh, never mind, you don’t bother to use π in the one place you might have, so not the problem.

a7

1 Like

You never use “freq” - so your code appears to run continuously rather than every millisecond. Without some time delay, you’ll never have a detectable change so the derivative will always be zero.

That is the PID error term, for integer degrees. Saves a lot of head scratching once you understand it, and it eliminates the urge to introduce nonsensical discontinuities like this, which will usually cause a controller to go crazy:

        if(degPosition>=360.0) {
        Position = 0; 
        }
        if (degPosition<=-360.0) {
        Position = 0; 
        }

Then it is not "actually working" and the math is not done correctly. Remove the Kd term and get Kp working as expected, first.

Make sure that the loop timing is sensible. The sampling period should be constant and one tenth the system response time or faster.

I think some study of PID control would be worth while , then write yourself a procedure with your controller in it , sort of:

Output_signal = Kp*Error

And just get that working , (then add integral and finally derivative ).Be good if you could adjust your Kp value with a potentiometer or keyboard input , so you can adjust values quickly . You should be able to get stability or instability ( oscillating ) with just proportional control by twiddling the gain value Kp, although there will be a error from the set point .
If /when you add integral you may need to then alter Kp again - all good fun

Control stuff

Proportional control

You shouldn’t need to set limits on the output , and think about precision , and work in consistent variable types . Pi= 3.142 should be more than good enough !

Refreshing attitude given the number of students that show up here bent on getting someone to do their work for them (aka plagiarism). But that doesn't mean you shouldn't do your research. I highly recommend reading the very thorough documentation written by the PID Library's author: http://brettbeauregard.com/blog/2011/04/improving-the-beginners-pid-introduction/

How should I make sure that the timing is correct? Do I need to make sure that code runs every millisecond?

As I am using interrupts I should avoid using time delays. At the same time how can I have the code running every millisecond?

At this point, it is not clear that the loop timing is working correctly. Study the Blink Without Delay example to get a better idea how to do constant loop timing.

The PID loop needs to run at a speed about 10x faster than the system response time constant, which you should already know. What is that time constant?

For a PD controller, shouldn't Ki =0?

double Kp1 = 16.99;
double Kd1 = 2.11;
double Ki1 = 8;

Also, (I haven't looked) but maybe you should check if you're basing this on a suitable transfer function for a PD controller in your code.

what is the value of dt? (pretty large denominator)

how does it affect this value?

here are some values. the calculated value of Output is 1500+ but limited to +/- 255. is that necessary? could Kp be reduced

          targ    pos     err   Kp1      dErr   Kd1       out      acc      spd
 loop:    90.0    7.6,  90.00 16.99,   0.0900  2.11,  1529.29   7.6464   7.6464
 loop:    90.0   22.3,  82.35 16.99,  -0.0076  2.11,  1399.17   6.9959  14.6423
 loop:    90.0   42.7,  67.71 16.99,  -0.0146  2.11,  1150.38   5.7519  20.3942
 loop:    90.0   67.1,  47.32 16.99,  -0.0204  2.11,   803.87   4.0194  24.4136
 loop:    90.0   93.5,  22.90 16.99,  -0.0244  2.11,   389.08   1.9454  26.3590
 loop:    90.0  119.5,  -3.46 16.99,  -0.0264  2.11,   -58.77  -0.2938  26.0652
 loop:    90.0  143.1, -29.52 16.99,  -0.0261  2.11,  -501.61  -2.5081  23.5571
 loop:    90.0  162.1, -53.08 16.99,  -0.0236  2.11,  -901.84  -4.5092  19.0479

Did anyone notice that dt is an integer?

I re-coded the timer part and was able to fix the issue. Thank you

could you post the corrected code?

 
#include <MatrixMath.h>
#define pi 3.1415926535897932384626433832795

int gearRatio = 298;
int ppr = 3;
//motor 1 
const int enablePin1 = 8;
const int pin1 = 10;
const int pin2 = 12;


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

//PID constants 

double Kp1 = 10;
double Kd1 = 0.7;
double Ki1 = 8;

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 currentTime = 0;
long previousTime = millis(); 
double Time;
 
const int dt = 10; //ms


void setup () {
  
  Serial.begin(115200);
  pinMode(enablePin1,OUTPUT);
  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) {
  currentTime = millis();
  Time = float(currentTime - previousTime); 
  if (Time>dt) {
        Time/=1000;
        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;//update current encoder reading
        errorPosition = demandPosition-currentPosition;

        
        //Serial.print(demandPosition, 3);
        //Serial.print("\t");
        Serial.println(currentPosition, 3);
        //Serial.print("\t");
        //Serial.print(errorPosition, 3);
        //Serial.print("\t");
     
          errorPosition_diff = (errorPosition-errorPosition_prev)/Time;
          errorPosition_sum += errorPosition*Ki1*Time;
          if (errorPosition_sum>255) {errorPosition_sum = 255;}
          if (errorPosition_sum <-255) {errorPosition_sum = -255;}
       
       Output = errorPosition*Kp1 + errorPosition_diff*Kd1 + errorPosition_sum;
        
  
       errorPosition_prev = errorPosition;
       previousTime = currentTime; 

        if (Output >= 0) {
        if(Output>255) {Output = 255;}
        else if(Output<70) {Output = Output+70;}
        }
        else {
        if(Output <-255) {Output = -255;}
        else if(Output>-70) {Output = Output-70;}
        }
        
        if (Output >= 0) {
          analogWrite(pin1, round(Output));
          analogWrite(pin2, 0);
  
        } else {
          analogWrite(pin2, round(-Output));
          analogWrite(pin1, 0); 
       
        }
     
   }
 
  }
}

void encoder() {
  if(digitalRead(pinEncoder)==LOW){
    Position++;
  }else {
    Position--;
  }
}