There have been a couple of questions lately about PID control, in particular there is an open thread in the RobotC technical discussion but only Jesse can post answers there, so I thought it was time to show some code doing a simple implementation. I don’t claim to be an expert in this area, the math and implementation are pretty simple but tuning can be tricky and depends on the physical aspects of the system being controlled and it’s desired response.
There are some problems in implementing PID control on the cortex as the motor speed can only be updated at around 60Hz at best (ports 1 & 10) and in practice somewhat slower than that if using the other ports needing the motor controller. Commercial motor controllers would typically run at a higher frequency than this but that’s not an option for us.
The following code controls one motor and uses one encoder as feedback. A variable called “pidRequestedValue” can be set and the motor will run until the encoder count matches this value. The code allows joystick channel2 (right analog vertical) to change pidRequestedValue therefore causing the motor to run until the encoder count again matches.
The PID control runs as a task with a fixed wait time at the end of each pass through the loop. This gives us a constant delta time when comparing to the theoretical equations. As the derivative and integral terms are being calculated at a constant interval this delta time becomes irrelevant and is folded into the Kd and Ki constants.
The code has a bunch of definitions at the beginning used to set motor direction, encoder scaling (for example you could scale so the units were inches rather than raw counts), the maximum motor speeds and the limit for integrating the error.
#pragma config(Sensor, dgtl1, myEncoder, sensorQuadEncoder)
#pragma config(Motor, port10, myMotor, tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
// PID using optical shaft encoder
//
// Shaft encoder has 360 pulses per revolution
//
#define PID_SENSOR_INDEX myEncoder
#define PID_SENSOR_SCALE 1
#define PID_MOTOR_INDEX myMotor
#define PID_MOTOR_SCALE -1
#define PID_DRIVE_MAX 127
#define PID_DRIVE_MIN (-127)
#define PID_INTEGRAL_LIMIT 50
// These could be constants but leaving
// as variables allows them to be modified in the debugger "live"
float pid_Kp = 2.0;
float pid_Ki = 0.04;
float pid_Kd = 0.0;
static int pidRunning = 1;
static float pidRequestedValue;
/*-----------------------------------------------------------------------------*/
/* */
/* pid control task */
/* */
/*-----------------------------------------------------------------------------*/
task pidController()
{
float pidSensorCurrentValue;
float pidError;
float pidLastError;
float pidIntegral;
float pidDerivative;
float pidDrive;
// If we are using an encoder then clear it
if( SensorType PID_SENSOR_INDEX ] == sensorQuadEncoder )
SensorValue PID_SENSOR_INDEX ] = 0;
// Init the variables - thanks Glenn :)
pidLastError = 0;
pidIntegral = 0;
while( true )
{
// Is PID control active ?
if( pidRunning )
{
// Read the sensor value and scale
pidSensorCurrentValue = SensorValue PID_SENSOR_INDEX ] * PID_SENSOR_SCALE;
// calculate error
pidError = pidSensorCurrentValue - pidRequestedValue;
// integral - if Ki is not 0
if( pid_Ki != 0 )
{
// If we are inside controlable window then integrate the error
if( abs(pidError) < PID_INTEGRAL_LIMIT )
pidIntegral = pidIntegral + pidError;
else
pidIntegral = 0;
}
else
pidIntegral = 0;
// calculate the derivative
pidDerivative = pidError - pidLastError;
pidLastError = pidError;
// calculate drive
pidDrive = (pid_Kp * pidError) + (pid_Ki * pidIntegral) + (pid_Kd * pidDerivative);
// limit drive
if( pidDrive > PID_DRIVE_MAX )
pidDrive = PID_DRIVE_MAX;
if( pidDrive < PID_DRIVE_MIN )
pidDrive = PID_DRIVE_MIN;
// send to motor
motor PID_MOTOR_INDEX ] = pidDrive * PID_MOTOR_SCALE;
}
else
{
// clear all
pidError = 0;
pidLastError = 0;
pidIntegral = 0;
pidDerivative = 0;
motor PID_MOTOR_INDEX ] = 0;
}
// Run at 50Hz
wait1Msec( 25 );
}
}
/*-----------------------------------------------------------------------------*/
/* */
/* main task */
/* */
/*-----------------------------------------------------------------------------*/
task main()
{
// send the motor off somewhere
pidRequestedValue = 1000;
// start the PID task
StartTask( pidController );
// use joystick to modify the requested position
while( true )
{
// maximum change for pidRequestedValue will be 127/4*20, around 640 counts per second
// free spinning motor is 100rmp so 1.67 rotations per second
// 1.67 * 360 counts is 600
pidRequestedValue = pidRequestedValue + (vexRT Ch2 ]/4);
wait1Msec(50);
}
}
PID will attempt to zero-out an error value. Error is a difference between two values. The trick of PID is to pick what you’re going to call error.
For driving straight, you could make one side the “master” and one the “slave” that tries to match the master’s reading. In this case, error is master reading - slave reading.
You could also calculate speed in a separate task using a timer and an infinite loop. Then error would be speed master - speed slave.
There’s other ways, too, but you this should get you started in the thought process.
Or, you could make whichever side is lagging the master, because if you’re running at full power and your fixed master side is getting too far ahead, you can’t do anything to correct the error (you’re already at max power on the lagging side).
So basically increase the power of whatever side is lagging, then cap the side that is getting too far ahead at 128 / (how much you are increasing the power of the lagging side). This will allow you to correct error even when the input is at 128 (by slowing down the side that is lagging), but it will also increase the power of the lagging side if the power is not near 128 (it’s generally better to increase power to correct than to decrease).
This might be considered unrelated, but how good do you guys feel the task control is on the cortex? I’m wondering if anyone has any specific success/failure stories, especially in the case of PID control.
Do you mean on the Cortex, or in ROBOTC? Here’s what the ROBOTC support site says about task control:
In my experience, most regular operations will run fine with ROBOTC multitasking. But if you have lots of code running, or you have very time intensive operations, then the tasks start slowing down a bit. If you do have requirements that ROBOTC multitasking can’t meet, then I suggest writing your own cooperative multitasking.
One thing you have to consider is task priority, I notice most of the code posted on the forums does not use this very much but it is something that should be considered and used. When all tasks have the same priority they will be serviced in a round robin fashion and may not get the cpu time they need. If a task is given a higher priority it will run when it needs to, something like a PID control loop should probably do this. Some tasks can be given a lower priority, I would do this for perhaps a task displaying information on the LCD. The biggest limitation of PID control on the cortex is the latency in motor command for motor ports 2 through 9 (my opinion only). Ports 1 & 10 are theoretically better but when all other factors are also considered, gear train backlash etc., it may not make that much difference.
I’ve been meditating upon this section of jpearman’s code and I’m having troubles understanding how the pidRequestedValue actually works. It seems to me that if you start up this code and the driver does not touch the joystick, then the machine will dutifully go to its assigned position. Okay, no problem. But once the driver touches the joystick, pidRequestedValue accumulates values on every loop until it’s like, let’s just say, twice its original value. But if the driver then releases the joystick because she is fairly close to where she wants the robot to be, then the vexRT[Ch2] value therefore drops to zero, but the pidRequestedValue will still remain twice its original value and the PID task will keep driving the motor until it reaches that doubled pidRequestedValue. In other words, I don’t see where there is a kind of “restoring force” in the equation to trim back the position that accumulates in pidRequestedValue. It seems to me that the only way this approach would work is to make the loop very slow so that it does not “out run” the ever-changing intentions of the driver.
I wrote some code based on this earlier today and the “automatic” PID portion worked fine, but this joystick portion caused the robot arm to swing to either one extreme or the other as soon as I touched the joystick. It appears that the pidRequestedValue maxes out and there does not seem to be anything to “de-accumulate” the pidRequestedValue. Is my understanding of this joystick operation completely wrong? Is there some other way it needs to be configured?
.
.
.
// use joystick to modify the requested position
while( true )
{
// maximum change for pidRequestedValue will be 127/4*20, around 640 counts per second
// free spinning motor is 100rmp so 1.67 rotations per second
// 1.67 * 360 counts is 600
pidRequestedValue = pidRequestedValue + (vexRT Ch2 ]/4);
wait1Msec(50);
}
.
.
.
I have integrated motor encoders. When I setup the motors and sensors in robotc I see a checkbox for PID. Does it really use PID control and is there any difference between using the built in pid control and this?
I think the built-in PID works only with the integrated motor encoders. I haven’t used the integrated motor encoders, so I don’t know for sure. I believe jpearman’s code is meant for do-it-yourself PID using, for example, the red encoders and pots, etc…
You have to remember that this is/was example code and may not be appropriate for you specific robot. You have to consider gearing and the maximum velocity that your arm (or whatever) can achieve.
The loop monitoring the joystick runs every 50mS in the example, each time around the maximum amount that can be added to pidRequestedValue is 32 (127/4) so pidRequestedValue will change by 640 every second (32 * (1000/50)).
The original code was using the red quad encoder. it has 360 counts per revolution and the motor running with no load will do about 1.7 rev/sec. So with the motor directly connected to the encoder it will run at about 600 counts/second.
So I had approximately matched the maximum velocity of the motor/encoder to the change in requested value.
If the motor on your arm is running slower and/or the gearing means that the encoder delta count/second is much less than 640 then you get the behavior that you described and will need to scale back the change in pidRequestedValue/second to match it.
This is very basic and old simple code intended to demonstrate the math behind PID, it was not “production” code. I’ve been working on some much more sophisticated control but things get really complicated very quickly.
Okay, that’s good to know. I was afraid I was misunderstanding something more fundamental - like the joysticks have some other mode I wasn’t aware of.
Yes, I was aware of that - and I didn’t mean to sound critical of your code. I was worried that I didn’t understand something basic about this. Knowing I need to just scale things back is a great relief. It’s been a long time since I’ve played with PIDs so I figured I was, as usual, missing something totally obvious.
For arms and PID in driver control, We have found that using push buttons for preset heights with PID control is good and then the joystick control overrides the PID and turns it off letting the joystick power the arm directly. The adjustment of the target via the joystick got to be a bit tough to drive with.
Have a variable for arm_PID_armed or something like that and when the joystick reads outside a control band, let it reset arm_PID_armed to off where the background PID loop drops off its control.
When a button is pressed, force the arm_PID_armed to on and let PID do it’s thing. A reset of the PID is also helpful as the integral build up may be very far away from the current position.
Thanks. Some very good advice. Right now, their robot is a kluge that is hard to drive, so having some presets will probably help take some of the load off their brains.
Just curious: what do you mean by “reset the PID”? In jpearman’s code, I think he limits how much integral error builds up so the integral error does not “wind up” to some absurd value. Is there some way the integral error can be/should be reset to zero? perhaps when the arm is “close enough” to its target point?
