Advanced PID and motion profile control

VEX V5 General Discussion Technical Discussion
1

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.

To reiterate

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
https://github.com/jpearman/motionPid

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).

2 Likes
2

I’ll be excited to attempt to port this to robotC

3

Jpearman,

Thanks for all this. I am currently chewing on your codes. Never got too much experience with either conVEX or the way you code it – I just use Robotc and multitask, giving the drive value to the motor in another loop or so. IDK if it is a professional idea to define variables in the task directly – you seem to define variables centrally elsewhere.

Last time I ran into this, I did not pay much attention as I was still struggling to get the most basic lift driver integrated PID control to work. The idea is pretty simple – like what Pastoral Invasion did in their notebook – When joystick/buttons have no input, record a height and do PID to hold position.

After getting PID to work properly, I find what you referred to – the transition between driver control and PID is a huge pain. My lift is perhaps the worst case – 1:7 double reverse six bar, with tremendous amount of UTRB assist. Any small motion would be amplified, and rubber band is tuned to be a little more than normal just so we have maximum amount of assist. The result is that the lift jumps pretty bad like a pneumatic lift when switching between driver control and PID. If you have seen any Mariner Robotics’s videos about Washington and Canada competitions, you would see that Robosavage 2Z has similar issue – although ours is much more vigorous.

We are at a pretty awkward position right now – State Championship is over, we did not qualify but our skills score should get us there – but nothing confirmed. I thought I would pick up issues that I never had time to really deal with and study motion profile better to make my lift control prettier.

Thanks

1 Like
4

I volunteer too. I plan on tackling this next week as well so maybe we can share some code. I now have a few extra robots to use now that PA states is over. I also use Robot C and want to teach the kids that next level of control now that PID tuning is old hat to them.

I will have a couple DR4B robots, a conveyor on a fixed needle, and an 8 bar or two. So multiple styles to play with and see the effects!

Well, play with right after I get our US Open and worlds teams set up, and upgrade Robot C now that we’re in a good break of action…

5

You have to adapt your programming style for ROBOTC, it varies from a “standard” compiler in a number of ways. That’s partly why I wrote this code for ConVEX as it allowed a more conventional way of splitting structure and other definition into a header file that can be referenced from more than one place.

1 Like