Looking at the code in Embedded Robotics by Thomas Bräunl, DOI: 10.1.1.474.8129 this appears to be an 'optimised' (i.e. difficult to follow) version of an integer PID controller for velocity control.
It sounds like you want control position rather than velocity though, so you need a position PID controller, as described by Chuck♦, which is more directly applicable to your problem and easier to understand.
If you do want a velocity controller however, read on...
The code in question is on page 93, Program 5.7: PID controller code:
static int r_old=0, e_old=0, e_old2=0;
...
e_func = v_des - v_act; /* error in velocity */
r_mot = r_old + Kp*(e_func-e_old) + Ki*(e_func+e_old)/2
+ Kd*(e_func - 2* e_old + e_old2);
/* motor output */
r_mot = min(r_mot, +100); /* limit motor output */
r_mot = max(r_mot, -100); /* limit motor output */
r_old = r_mot; /* store this motor output */
e_old2 = e_old; /* store last velocity error */
e_old = e_func; /* store this velocity error */
In this snippet I've restored the static
line (which is important as it shows this is an integer controller and not a floating point controller), left the original max
functions in place, as they are easier to understand, and added comments.
In this code, the v_des
is the desired velocity per controller period, the v_act
is the current actual velocity per controller period.
Your changes appear to be:
- The addition of the
_r
suffix (presumably to designate the right motor, and I assume there is similar code for the left motor)
- Replacing the
max
function with a pair of hard coded if
statements
- Defining $K_P$ in terms of it's reciprocal.
The problem is the last change combined with the fact that we are using integer arithmetic.
Calculating 1/K_P
will evaluate to either 0
for K_P>1
) and 1
for K_P==1
, assuming sensible values of $K_P>0$.
So you are effectively turning your Proportional Integral Derivative controller into a Integral Derivative controller for any value of $K_P$ other than 1
.
If you went back to using K_P
directly rather than trying to use (1/K_P)
then you should be able to tune your motor for velocity control.