This code could be used in any feedback loop. I wrote it all in theory, using a simple simulator for testing… Robot still not back from FedEx yet…
Testing is well appreciated. Feel free to use if desired.
–NOTE-- The following source was designed to run on a PC (Actually, Cygwin+KDevelop). Tweak accordingly. You only need the do_discrete_control and its global variables on the robot – the rest was for PC simulation.
/*
*
* Proportional-Integral-Derivitive Closed-Loop feedback algorithm.
*
*
* By John Dong, Team 245 (Adam-Bots)
*
* [email][email protected][/email]
*
* www.adamsrobotics.com
*
*
*
*/
#ifdef HAVE_CONFIG_H
#include <config.h>
#endif
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
/* These keep track of previous errors */
int error=0, Lerror=0, LLerror=0;
int last_out=0;
/*THIS is the bulk of the program. Pass in actual and wanted values,
* and get back the optimal PWM value.
*
*
* Constants to tweak are kp, ki, and kd, for each of the 3 bands.
*
* Set a constant to ZERO in order to disable the band.
*
* (You may want to disable the derivative band, as it's just
* a dampener. May not be worth calculation cost)
*
*
*/
int do_discrete_control(int actual, int wanted, int kp, int ki , int kd)
{
int out; //Store output.
LLerror=Lerror;
Lerror=error;
error=wanted-actual;
out=last_out+kp*(error-Lerror)+ki*error+kd*(error-2*Lerror+LLerror);
if(out>127) out=127;
if(out<-127) out=-127;
return out;
}
#define KP 4
#define KI 5
#define KD 0
int main(int argc, char *argv])
{
int i=0;
int pwm,pos;
pwm=0; pos=0;
for(i=0;i<600;i++)
{
pwm=do_discrete_control(pos,250,KP,KI,KD);
pos+=(double)pwm*.1;
printf("# %d POS: %d PWM:%d
",i,pos,pwm);
if(i==500)
{
scanf("%d",&pos);
i=0;
}
}
getchar();
return EXIT_SUCCESS;
}
Why is Ki multiplied by the current error? There should be a variable keeping track of the running sum of the error. That variable would be multiplied by Ki.
yes…you want to integrate i add up all the values then divide by total time (multiplying each new i by time shouldn’t matter because its an “assumed” even loop time…or use interrupts timers or whatever…) …then multiply by Ki… don’t even bother multiplying by .025 or whatever the loop time is …just by 1 its simpler
this is what I did for our also Experimental PID loop (PI so far actuallY):
error=realv-targetv;
errorint=(error+errorint/time)/intprop; //intprop does the same thing as your Ki term
time++;
also it seems that last_OUT isn’t being changed anywhere since definition (it always=0)
can you please explain how the deriviatve portion works? … I am kinda clueless to how it works…and does anything …basically the theory behind it…and why it helps and what exactly it is
PID stands for Proportional Integral Derivative control. Yes, basically it is a “fancy way” of getting “to a certain position.” It’s what is called closed loop control – you have a sensor telling you where you are, you have a setpoint telling you where you want to be, and you try to get the two values to become equal by modifying an output (to a motor, for instance). There’s another thread out there on the matter, and google’ll provide a lot more info on the matter … it can get complicated, but most applications related to FIRST should be fairly straightforward. Note that you don’t have to include all the different elements of a PID control; for instance, for our Arm this year, a simple P control worked quite well, whereas for another appendage we had to use something a bit more complicated.
With regard to the I part of PID, be very careful.
There is a phenomon called “integral wind up” that is not a theoretical problem but a practical implementation problem.
Essentially, you need to “turn off” the integration when the controller output is saturated or limited (for example when your robot is disabled).
We had a nasty bug that we had fits trying to kill. Our robot would come out of a disabled state and do a major twitch that hurt our robot and almost hurt some workers on a number of occassions. We finally killed the bug, but it was very sutble and was quite difficult to discover. It turned out to be a case of integral wind up.
oops, sorry, I switched kp and ki when typing in a hurry. kp is ki and ki is kp :ahh: :ahh:
Time constants are ‘included’ in the k’s.
I thought that if anyone needed a code snippet for PID, they probably wouldn’t know exactly what effect each constant has, and probably would just “guess and check”
Reguarding the addup – the loop goes through and adds the current output to previous outputs – so errors are summed in there. I found this in a whitepaper about PID (I lost the url, so can’t post it here). It was the “most optimal and stable” implementation, according to the author.
Integral windup can definately become a problem if you leave your robot on, and something that is connected to your PID loop sensor moves (when the RC is on but the robot is powerless to do anything about it, it will build the integral uselessly, until reactivation when people get hurt). We struggled for a while trying to get the RC to realize when the OI was on or off, but if you can manage to reset everything when the OI comes on (not just resetting the RC), that’s a good way to go. You can also limit your integral, as this will help you if you ever get stuck on something during real testing (you won’t break your motors). If you don’t have a failsafe implemented though, make sure to reset your robot every time before turning your OI back on!
The OI does know what state it is in. It knows when it is in competition mode, disable mode, or autonomous mode. Your control loops should be aware of which state the sytem is in and provide code for transitioning between each state. You can use different controls for each state, but anything with memory like integrators should be delt with as the system changes from state to state. All memory type variables should be initialized to a known state while not being used in a specific state so it is ready to be used when the new state starts.