#define left_velocity 10
#define K_PRO 3 /* proportional constant */
#define K_DRV 3 /* derivative constant */
int Lvel, Rvel; /* requested input velocities, ticks per 1/20 sec */
int leftacc, rightacc; /* accumulators for motor output values */
int leftlast, rightlast; /* history values for calculating derivative */
void speedcontrol() /* PID controller*/
{
int lf, rt;
lf = (Lvel - left_velocity)*256;
leftacc += ((lf/K_PRO) + ((lf - leftlast)/K_DRV));
leftlast = lf;
pwmL(leftacc/256);
}