So I want to start discussing some more advanced motor control functionality. Last year I hinted that I was experimenting with different ways of controlling the linear lift I had built, some of those experiments were promising, some didn’t work out at all, nothing was ever really finished to my satisfaction and the code has been sitting unfinished for some time. As we are nearing the end of the season I’m just going to throw it out and perhaps someone will develop it more in time for worlds.
When I designed the lift system I wanted more from the control than just sending values to the motors from the joystick. In the past I have used basic PID control but always found the transition from using presets to manual control to be problematic.
So the concept I’m presenting here (which I should say right up front is not new and is used in industrial control systems all the time) is to precede the positional PID control loop with a stage that generates a motion profile. The input to the motion profile stage is a target position, the variable that in the past has been the input to the PID controller, a maximum velocity and acceleration. The motion profile code more or less lets you say.
“I want to go from the current position to a new position with a maximum velocity of V and a maximum acceleration of A.”
The outputs of the motion profile code are position, velocity and acceleration that are then fed into the PID control loop.
Here is the block diagram that I used previously to illustrate this.
The motion profile generator creates output where the velocity follows a trapezoidal shape, that is, velocity accelerates in a linear fashion to a constant, maximum velocity. It holds that velocity until it determines that it should decelerate in a linear fashion to a stop. At this point the motor’s position should be at the target.
One of the complicated aspects of this is accounting for changing target positions and possibly a changing maximum velocity. Every time the motor position is sampled and the PID loop updated, the motion profile code has to calculate the position the motor would be at if it were decelerated to a stop from this point. In addition to this, the minimum and maximum limits of the motor’s position need to be accounted for and the motor decelerated if nearing them.
A typical motion profile created by this code may look as follows.
The dotted blue line represents a new target position being requested, 1000 counts.
The black line is velocity and shows the trapezoidal shape I described. The red line is acceleration and the solid blue line the target position that is the input to the PID control loop.
Sometimes the velocity does not reach the maximum and the shape of the velocity graph is triangular. Here is an example where that happens.
The dotted blue line is the input to the block I call the “trapezoidal motion profile generator”
The blue, black and red lines are the output from that block and the input to the “position PID controller”
So why go to all this trouble?
In the traditional code we have used before, when a new position is set the PID control loop sees a step response, that is, it sees a large instantaneous change that causes the error to become immediately large. This then causes the motor to usually be driven at maximum speed until the error reduces. We have used “slew rate” and other methods to limit the acceleration of the motor but this can interfere when trying to decelerate near to the target position.
The idea of the motion profile is to generate a position and velocity that the motor (and I really mean the entire system that the motor is connected to) can achieve with more knowledge of where it’s trying to get to. The PID loop usually sees only a small error that the proportional, derivative and integral contributions are trying to reduce. A new velocity feed forward term uses the velocity information we are feeding it to drive the motor and keep the error small. The guts of the PID calculation are as follows.
// calculate drive - no delta T in this version mp->p_pid.drive = (mp->p_pid.Kp * mp->p_pid.error) + (mp->p_pid.Ki * mp->p_pid.integral) + (mp->p_pid.Kd * mp->p_pid.derivative) + (mp->p_pid.Kvff * mp->mp_velocity) + mp->p_pid.Kbias;
Anyway, there’s more to discuss but I’m tired so it will have to wait for another day.
The code I was using is on github here, it’s all written for ConVEX but it’s the ideas that are important. The interesting parts are in motionPid.c
It’s been slightly tweaked from what I had on the lift but this is the essence of what I was using. Some of the functions in the code are left over from previous experiments (the velocity PID loop etc.) and there’s a few additional functions that are pulled from the smart motor library (which can not be used with this code).