Notifications
Clear all
Topic starter
2026-09-21 12:09 am
Writing a PID controller has been on my bucket list for awhile, but I was always a little wary about how I was going to add it to the cooperative scheduler that I’ve used in a lot of my robots.
For example, here is a snippet of the main loop.
while (1) {
unsigned char winner;
acquire_sensors();
cruise();
avoid();
contact_kin();
stasis();
charge_home();
winner = arbitrate();
move_dual(winner);
}
return 0;
}
You can see that the timing of the loop is indeterminate but PID needs a fixed interval between calls!
My goals were pretty simple:
- I wanted the two wheels to match speeds without resorting to a predetermined equation that maps speed to PWM duty cycle.
- I wanted the robot to climb door mats, rugs, & flooring transitions without getting hung up.
It took two or three tries.
After one failed attempt at implementing PID using a timer interrupt I decided to start over with just a proportional controller. In my Internet searching I happened across some lectures by Professor Clark at CS Pomona. I eventually ported his lecture on ‘Programming a proportional motor controller’ and got it working on a Pico.
https://cs.pomona.edu/classes/cs181r/book/07-MotorControl.html
One piece of his algorithm that took me a while to get right was the function that calculates the new speed in meters/second and outputs it as a speed factor. Why a speed factor? I believe it’s because you can’t take the output from the proportional controller and use it directly to control the motors because of the case where error = 0. Eg, if desired speed = measured speed then error = 0 and that would translate to a PWM duty cycle of 0%. Which is not what you want.
After the speed factor is calculated it is passed to a function that maps the value to a duty cycle percentage. The speed factor sign sets the direction and the absolute value the speed.
// Map a float in the range of in_min to in_max to a float in the range of out_min to out_max
float mapFtoF(float x, float in_min, float in_max, float out_min, float out_max) {
return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min;
}
void set_right_pwm_speed(float speed_factor) {
int8_t pwm_duty_cycle = 0;
if (speed_factor < 0.0f)
pwm_duty_cycle = -1.0f * mapFtoF(fabs(speed_factor), 0.0f, 0.1f, 0.0f, 100.0f);
else
pwm_duty_cycle = mapFtoF(fabs(speed_factor), 0.0f, 0.1f, 0.0f, 100.0f);
setM2Speed(slice_Motors, chan_M2, pwm_duty_cycle);
return;
}
Here’s the compute speed factor function.
// PD control
typedef struct {
float kp; // gains
float kd;
float prev_error; // for the derivative term
float speed_factor; // latest speed factor
} PD;
PD motorA_pd;
PD motorB_pd;
...
float compute_speed_factor(PD *pd, float desired_speed, float measured_speed, float time_delta) {
float u = 0.0f;
float error = 0.0f;
desired_speed = constrain(desired_speed, -max_speed, max_speed);
error = desired_speed - measured_speed;
float D = pd->kd * (error - pd->prev_error) / time_delta;
pd->prev_error = error;
// u = constrain(pd->kp * error, -max_speed_step, max_speed_step);
u = constrain(pd->kp * error + D, -max_speed_step, max_speed_step);
pd->speed_factor = constrain(pd->speed_factor + u, -1.0, 1.0);
return pd->speed_factor;
}
This is a paraphrase of what Professor Clark said in the video: Kp is a constant and could be assigned any units we want. In this case we are assigning the units to convert meters/second into the speed factor [-1.0, 1.0]. He describes speed factor as scaled speed.
One last thing worth pointing out about this function is that the newly minted speed factor u is added to the previous speed factor before returning it to the caller. This covers the case where error = 0 because the desired speed and the measured speed are the same. It just adds 0 to the speed factor and the same duty cycle gets used again. Constant speed.
Tom
To err is human.
To really foul up, use a computer.
Lee G reacted
Topic starter
2026-09-22 12:28 am
Tuning the proportional gain.
To tune the algorithm I needed to grab data while the robot was running around in my environment. Fortunately my two motors are closely matched in the forward direction, so I could use the same gain value for each. For example, here’s a graph while running with no load.
And here’s a graph taken while the robot was running on the floor.
Interesting difference. All of the no load runs looked similar except for the rise time it took to come up to the set point speed (0.2 meters/second). While all the load runs had oscillations. The larger the proportional gain the steeper the slope.
By the way, here’s a shot of the robot.
My problem with all this was that I kind of enjoy analyzing data so I needed to cut to the chase and come up with some criteria to use. What I finally settled on was the time it took to reach the set point speed on a wooden floor. It had to take less than 500ms to reach 0.2 meters/second.
I settled on a gain of 0.066 for both motors. My reason for starting with this setting was because the motors response matched up pretty evenly and the slope wasn’t crazy steep. Under load it hits the set point speed in ~300ms.
Adding the derivative term.
I tried this proportional gain setting in my robot control program and it was ok when the environment was open with just a few obstacles (like in the kitchen), but when going over a floor mat one or both wheels would overshoot the set point and that would start some strange oscillations. Also on that same surface coming out of a turn could be very jerky.
In all of the documents that I’ve read about PID motor controllers they all mention a proportional term steady-state error where the control signal never quite reaches the set point speed. Integral is the one term that can decrease the steady-state error. Well for me, I’ve never seen a steady-state error in any of the work I’ve done with PID controllers! So I must be doing something wrong… Anyway, because I’m not seeing this error I thought that I could skip adding the integral and try adding the derivative term to decrease proportional overshoot. It did help but it also increased the rise time slightly.
Under load it hits the set point speed in ~400ms and runs a whole lot smoother.
Here is a log of my PD controller coming up to speed. It shows how the speed factor is added to the previous speed factor to arrive at a new duty cycle.
Here is another log showing how the controller goes from running straight to a left spin and then back to running straight.
Tom
To err is human.
To really foul up, use a computer.
Lee G reacted
Topic starter
2026-09-23 12:21 am
Controlling the controller.
As I said before I was a bit wary about how to integrate PID into my goto robot controller program, a cooperative scheduler. Running in a straight line is one thing, but running different behaviors seemed like it was going to be another set of compromises to discover. I was even wondering if it was possible to do an immediate stop! Fortunately it didn’t turn out that bad.
- The main loop had to be changed to be periodic.
uint64_t pg_startTime = 0;
void tSleep(uint8_t hertz) {
uint32_t cycleUS;
cycleUS = 1000000 / hertz; // eg. 1,000,000 / 20hz = 50,000 us
uint64_t timeRemaining = (pg_startTime + cycleUS) - time_us_64();
//printf("Time remaining %f ms\n", timeRemaining/1000.0);
if (timeRemaining > 0)
sleep_us(timeRemaining);
pg_startTime = time_us_64();
return;
}
...
pg_startTime = time_us_64();
while (1) {
unsigned char winner;
acquire_sensors();
cruise();
avoid();
escape();
winner = arbitrate();
tSleep(20); // a loop frequency of 20Hz
move_dual(winner);
}
return 0;
}
This technique isn’t perfect but neither was using a Pico SDK timer interrupt.
- Some of the turns issued by the behaviors had to be tightened up (less forward speed) because there is now a small lag between the command and the final result.
- The callback function that gets fired by the wheel encoders had to be changed to provide the PD controller a way to measure the current speed.
- Lots of testing.
Although I still have to implement Professor Clark’s max_speed_step constraint I love the way that the PD controller ramps up the motor speeds when the robot is slowed down by something. It was worth the effort.
Video.
To err is human.
To really foul up, use a computer.
Lee G reacted
2026-09-23 10:53 pm
@thrandell Thats a well behaved bot, your cooperative scheduling technique is obviously working well. Thanks for the good info on the use on getting your PID sorted. 😀
Topic starter
2026-09-24 9:12 pm
Thanks @byron. I usually run out of patience watching videos, but Professor Clark’s video on proportional controllers hit the mark for me.
To err is human.
To really foul up, use a computer.