A very basic PID Controller for ROBOTC

pid.c

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port2,           FlywheelLeft,  tmotorVex393TurboSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port3,           FlywheelRight, tmotorVex393TurboSpeed_MC29, openLoop, encoderPort, I2C_1)
#pragma config(Motor,  port4,           DriveFrontLeft, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port5,           DriveFrontRight, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port6,           DriveBackLeft, tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port7,           DriveBackRight, tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port8,           ElevatorRight, tmotorVex393TurboSpeed_MC29, openLoop, reversed)
#pragma config(Motor,  port9,           ElevatorLeft,  tmotorVex393TurboSpeed_MC29, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//


#pragma platform(VEX2);

float RPSEncoder;

// For Vex 393 Turbo Motors
float ticksPerRev = 261.3;
// Our Flywheel Gear Ratio?
float launcherRatio = 11.667;


task readEncoderFlywheel() {
	long lastSysTime = nSysTime;
	int deltaSysTime;
	long lastEncoder = nMotorEncoder[FlywheelRight];
	int deltaEncoder;

	wait1Msec(20);
	while(true) {
		deltaSysTime = nSysTime - lastSysTime;
		lastSysTime = nSysTime;
		deltaEncoder = abs(nMotorEncoder[FlywheelRight] - lastEncoder);
		lastEncoder = nMotorEncoder[FlywheelRight];
		RPSEncoder = (deltaEncoder/ticksPerRev) * launcherRatio * (1000/deltaSysTime);

		wait1Msec(20);
	}
}

/*
	Use Kp to adjust power
	Use Ki to adjust error reaction
	Use Kd to limit error reaction
*/
float Kp = 3.0;
float Ki = 1;
float Kd = 1;
float dt = 2;
float integral;

float error;
float preError;
float derivative;
float preOutput;
float output;


float epsilon = 0;
/* Both target and current are in RotationsPerSecond */
float CalculatePID(float target, float current) {
	error = target - current;

	if(abs(error) > epsilon) {
		integral=integral+(error*dt);
	}

	if(integral > 600) {
		integral = 600;
	}
	if(integral < -600) {
		integral = -600;
	}

	derivative = (error-preError)/dt;
	preOutput = (Kp*error)+(Ki*integral)+(Kd*derivative);

	output = preOutput;
	if (output>Max) {
		output = Max;
	}
	if (output<Min) {
		output = Min;
	}

	if(target == 0) {
		output = 0;
		integral = 0;
	}

	return output;
}

task usercontrol() {

  startTask(readEncoderFlywheel);
  int pidRequestedValue = 0;
  int toggleup = 0;
  int toggledown = 0;
  float motorPower;
  while (true) {
    if(vexRT[Btn7U] == 1 && toggleup == 0) {
      if(toggleup == 0) {
        if(pidRequestedValue == 0) {
          pidRequestedValue = 15;
        } else {
          if(pidRequestedValue < 59 && pidRequestedValue >= 15) {
            pidRequestedValue += 0.5;
          }
        }
      }
      toggleup = 1;
    }

  if(vexRT[Btn7U] == 0) {
    toggleup = 0;
  }

  if(vexRT[Btn7D] == 1 && toggledown == 0) {
    if(toggledown == 0) {
      if(pidRequestedValue == 15) {
        pidRequestedValue = 0;
      } else {
        if(pidRequestedValue <= 55 && pidRequestedValue > 15) {
          pidRequestedValue -= 0.5;
        }
      }
    }
    toggledown = 1;
    }

  if(vexRT[Btn7D] == 0) {
    toggledown = 0;
  }

  if(vexRT[Btn7L] == 1) {
    pidRequestedValue = 25;
  }

  if(vexRT[Btn7R] == 1) {
    pidRequestedValue = 35;
  }

  motorPower = CalculatePID(pidRequestedValue, RPSEncoder);
  motorPower = abs(motorPower)
  motor[FlywheelLeft] = motorPower;
  motor[FlywheelRight] = motorPower;

  /** Refresh PID at 50Hz **/
  wait1Msec(20);
  }
}

Sorry about the terrible formatting, but I'm in a rush, ;)