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
- 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.
1/K_P will evaluate to either
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
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.