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);
}
}
```