I’m releasing the code I use for PID control. I had meant to clean this up a bit more and write some documentation before releasing but I never seem to get around to it so it’s “as is”.
This library is a generic implementation of PID control, it is not aimed at anything specific and can use either an encoder or potentiometer as the source of feedback. It is designed for position control rather than speed control but could be adapted easily enough.
To use the library you need to first call an initialize function with the PID constants and feedback sensor. This will return a pointer to an internal structure that the library uses to maintain the control variables. An update function then needs to be called on a periodic basis in a task, this will return a new drive variable that can be sent to the motors.
Here is an example showing its most simple use.
#pragma config(Sensor, dgtl1, arm_position, sensorQuadEncoder)
#pragma config(Motor, port1, ArmMotor, tmotorVex393, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
/*-----------------------------------------------------------------------------*/
/* Example for PID library */
/* James Pearman */
/* March 12 2013 */
/* */
/* One motor arm control using quad encoder */
/*-----------------------------------------------------------------------------*/
#include "PidLib.c"
// global to hold arm pid controller pointer
pidController *arm_pid;
/*-----------------------------------------------------------------------------*/
/* Task to update arm pid loop and control arm motor */
/*-----------------------------------------------------------------------------*/
task ArmControl()
{
while(1)
{
if( arm_pid != NULL )
{
// update pid control
PidControllerUpdate( arm_pid );
// drive motor
motor ArmMotor ] = arm_pid->drive_cmd;
}
wait1Msec(25);
}
}
/*-----------------------------------------------------------------------------*/
/* Send arm to position */
/*-----------------------------------------------------------------------------*/
void
ArmSetPosition( short position )
{
if( arm_pid != NULL )
arm_pid->target_value = position;
}
/*-----------------------------------------------------------------------------*/
/* Wait for arm to be in position */
/*-----------------------------------------------------------------------------*/
short
ArmWaitInPosition( short timeout = 3000 )
{
// default is 3 seconds
short count = timeout;
while(count >= 0 )
{
// wait so that pid has calculated error
wait1Msec(50);
// Check the pid error for the arm
if( (abs(arm_pid->error) < 50 ) )
return(1);
// decrease timeout
count -= 50;
}
return(0);
}
task main()
{
// Init - no bias
// Kp = 0.004, Ki = 0, Kd = 0.01
arm_pid = PidControllerInit( 0.004, 0.0, 0.01, arm_position, 0 );
// Start the arm pid controller
StartTask( ArmControl );
while(1)
{
// Send arm up
ArmSetPosition( 1000 );
// Wait for it to get there
ArmWaitInPosition();
// send arm down
ArmSetPosition( 100 );
// Wait for it to get there
ArmWaitInPosition();
wait1Msec(1000);
}
}The library has the following public functions.
pidController *PidControllerInit( float Kp, float Ki, float Kd, tSensors port, short sensor_reverse = 0 );
pidController *PidControllerInit( float Kp, float Ki, float Kd, float Kbias, tSensors port, short sensor_reverse = 0 );
short PidControllerUpdate( pidController *p );
and one private function not meant to be called by the user.
void PidControllerMakeLut();
This initializes my non-linear motor control LUT, you get that for free 
The library can be downloaded from here.
The next part is just for information.
Normally in my code the pointer to the pid controller will be part of a larger structure holding other variables associated with whatever is being controlled. For example, here is what I use as a top level arm controller.
/*-----------------------------------------------------------------------------*/
/* All the variables for arm control */
/*-----------------------------------------------------------------------------*/
typedef struct {
// local copy of pid feedback port
tSensors sensor_port;
// absolute arm limits
long upper_limit;
long lower_limit;
// pid controller ptr
pidController *arm_pid;
// Up to 4 motors for arm
short motor_num;
tMotor motors[kMaxArmMotors];
// override power to arm in case PTC trips
short motor_off;
// manual overide of PID control
short user_manual;
// vexnet controller
vexrtController controller;
// presets
vexPreset presets;
// limit switches if they are installed
tSensors limitSwitchLow;
tSensors limitSwitchHigh;
// sensor counts per degree after gearing
float CountsPerDeg;
} armController;
static armController theArm;
You can see that there are many other things associated with arm control beyond just the pid. The “real” arm control code handles things like upper and lower limits for the arm, preset positions (another library I may release sometime) and connections between the joystick control and arm movement.
Anyway, enjoy the library.