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--;
}
}
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
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
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/
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?