An example implementation of a PID controller in RobotC for VEX.

254_PID_Example.c

#pragma config(Sensor, dgtl1,  armEncoder,     sensorQuadEncoder)
#pragma config(Motor,  port1,           armMotor,      tmotorVex393_HBridge, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX2)
#pragma competitionControl(Competition)
#include "Vex_Competition_Includes.c"

//******************************************************************************
//	254_PID_Example.c
//
//	Code sample to help explain PID in RobotC
//	Written by Team 254
//******************************************************************************

/*!
 *  PIDController: struct to hold all the necessary values and components for PID
 */
typedef struct {
	tSensors input_sensor;
	float target;
	float kP;
	float kI;
	float kD;
	float error;
	float previous_error;
	float integral;
	float integral_threshold;
	float derivative;
} PIDController;

/*!
 * initializePIDController: Function to help create a PID Controller component.
 * @param &controller A pointer to the PIDController instance to initialize.
 * @param target_sensor The sensor which will supply this controller with values
 * @param kP The proportional constant for this controller
 * @param kI The integral constant for this controller
 * @param kD The derivative constant for this controller
 * @param integral_threshold The threshold for ignoring the integral term
 */
void initalizePIDController(PIDController &controller, tSensors target_sensor, float kP, float kI = 0, float kD = 0, float integral_threshold = 10) {
	controller.input_sensor = target_sensor;
	controller.kP = kP;
	controller.kI = kI;
	controller.kD = kD;
	controller.integral = 0;
	controller.integral_threshold = integral_threshold;

	//Set the controller's target to the current position to prevent unexpected movement.
	controller.target = SensorValue[controller.input_sensor];
}

/*!
 * updatePIDController: Function to run the PID algorithm on a controller
 * @param &controller A pointer to the PIDController instance to update.
 * @param target The desired target value of the controller's sensor
 * @param stop Optional. Number of milliseconds to delay this function by.
 * @return The output of the PID algorithm to power motors.
 */
int updatePIDController(PIDController &controller, float target, int delay = 0) {
	//Save the previous error for the derivative
	controller.previous_error = controller.error;

	//Calculate a new error by finding the difference between the current position
	//and the desired position.
	controller.error = controller.target - SensorValue[controller.input_sensor];

	//Begin summing the errors into the integral term if the error is below a threshold,
	//and reset it if not. This is to prevent the integral from growing too large.
	if(abs(controller.error) < controller.integral_threshold) {
		controller.integral += controller.error;
	}
	else {
		controller.integral = 0;
	}

	//Calculate the derivative by finding the change between the current error and
	//last update's error
	controller.derivative = controller.error - controller.previous_error;

	//Delay the function from returning. This is useful when it this is the last
	//PIDControllerUpdate in a task, and we don't need to calculate over small
	//time periods.
	wait1Msec(delay);

	//Combine all the parts of the PID function into the PID algorithm and return the value.
	return controller.kP*controller.error + controller.kI*controller.integral + controller.kD*controller.derivative;
}

//Create an instance of the PIDController
PIDController armController;

void pre_auton() {
  bStopTasksBetweenModes = true;
}

task autonomous()
{
	//Initialize the armController (referencing the armEncoder) with the values:
	// kP: 0.3
	// kI: 0.2
	// kD: 0.1
  initalizePIDController(armController, armEncoder, 0.3, 0.2, 0.1, 40);
}

task usercontrol()
{
  while (true)
  {
		//Call the updatePIDController function to move the arm to an
		//armEncoder position of 300. Delay by 25ms since we don't need
		//to update the motor over a small time interval.
    motor[armMotor] = updatePIDController(armController, 300, 25);
  }
}