Made by Adam T on post #10 http://www.vexforum.com/index.php/15195-flywheel-velocity-control-revisited with an error correction.

ExampleDualTBHCode.c

#pragma config(I2C_Usage, I2C1, i2cSensors)
#pragma config(Sensor, I2C_1,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Sensor, I2C_2,  ,               sensorQuadEncoderOnI2CPort,    , AutoAssign)
#pragma config(Motor,  port6,           Motor_FWR,     tmotorVex393HighSpeed_MC29, openLoop, reversed, encoderPort, I2C_1)
#pragma config(Motor,  port7,           Motor_FWL,     tmotorVex393HighSpeed_MC29, openLoop, encoderPort, I2C_2)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "Vex_Competition_Includes.c"   //Main competition background code...do not modify!

// Update inteval (in mS) for the flywheel control loop
#define FW_LOOP_SPEED              25

// Maximum power we want to send to the flywheel motors
#define FW_MAX_POWER              127

// encoder counts per revolution depending on motor
#define MOTOR_TPR_269           240.448
#define MOTOR_TPR_393R          261.333
#define MOTOR_TPR_393S          392
#define MOTOR_TPR_393T          627.2
#define MOTOR_TPR_QUAD          360.0

// encoder tick per revolution
float           ticks_per_rev;          ///< encoder ticks per revolution

// Encoder
long            encoder_countsR;         ///< current encoder count
long            encoder_counts_lastR;    ///< current encoder count
long            encoder_countsL;         
long            encoder_counts_lastL;   

// velocity measurement
float           motor_velocityR;         ///< current velocity in rpm
float           motor_velocityL;         
long            nSysTime_lastR;          ///< Time of last velocity calculation
long            nSysTime_lastL;          

// TBH control algorithm variables
long            target_velocityR;        ///< target_velocity velocity
long            target_velocityL;        
float           current_errorR;          ///< error between actual and target_velocity velocities
float           current_errorL;          ///< error between actual and target_velocity velocities
float           last_errorR;             ///< error last time update called
float           last_errorL;             
float           gain;                   ///< gain
float           driveR;                  ///< final drive out of TBH (0.0 to 1.0) RIGHT
float           driveL;                  /// LEFT
float           drive_at_zeroR;          ///< drive at last zero crossing RIGHT
float           drive_at_zeroL;          ///LEFT
long            first_crossR;            ///< flag indicating first zero crossing RIGHT
long            first_crossL;            /// LEFT
float           drive_approxR;           ///< estimated open loop drive RIGHT
float           drive_approxL;           ///LEFT

// final motor drive
long            motor_driveR;            ///< final motor control value RIGHT
long            motor_driveL;            /// LEFT

/*Set the flywheen motors RIGHT  */
void
FwMotorSetR( int valueR )
{
	motor[ Motor_FWR ] = valueR;
}

/*Set the flywheen motors LEFT */
void
FwMotorSetL( int valueL )
{
	motor[ Motor_FWL ] = valueL;
}


/*Get the flywheen motor encoder count RIGHT*/
long
FwMotorEncoderGetR()
{
	return( nMotorEncoder[ Motor_FWR ] );
}


/*Get the flywheen motor encoder count LEFT */
long
FwMotorEncoderGetL()
{
	return( nMotorEncoder[ Motor_FWL ] );
}


/*Set the controller position RIGHT */
void
FwVelocitySetR( int velocityR, float predicted_driveR )
{
	// set target_velocity velocity (motor rpm)
	target_velocityR = velocityR;

	// Set error so zero crossing is correctly detected
	current_errorR = target_velocityR - motor_velocityR;
	last_errorR    = current_errorR;

	// Set predicted open loop drive value
	drive_approxR  = predicted_driveR;
	// Set flag to detect first zero crossing
	first_crossR   = 1;
	// clear tbh variable
	drive_at_zeroR = 0;
}


/*Set the controller position LEFT  */
void
FwVelocitySetL( int velocityL, float predicted_driveL )
{
	// set target_velocity velocity (motor rpm)
	target_velocityL = velocityL;

	// Set error so zero crossing is correctly detected
	current_errorL = target_velocityL - motor_velocityL;
	last_errorL    = current_errorL;

	// Set predicted open loop drive value
	drive_approxL  = predicted_driveL;
	// Set flag to detect first zero crossing
	first_crossL   = 1;
	// clear tbh variable
	drive_at_zeroL = 0;
}

/*Calculate the current flywheel motor velocity*/
void
FwCalculateSpeed()
{
	int     delta_msR;
	int     delta_msL;
	int     delta_encR;
	int     delta_encL;

	// Get current encoder value
	encoder_countsR = FwMotorEncoderGetR();
	encoder_countsL = FwMotorEncoderGetL();

	// This is just used so we don't need to know how often we are called
	// how many mS since we were last here
	delta_msR = nSysTime - nSysTime_lastR;
	nSysTime_lastR = nSysTime;
	delta_msL = nSysTime - nSysTime_lastL;
	nSysTime_lastL = nSysTime;

	// Change in encoder count
	delta_encR = (encoder_countsR - encoder_counts_lastR);
	delta_encL = (encoder_countsL - encoder_counts_lastL);

	// save last position
	encoder_counts_lastR = encoder_countsR;
	encoder_counts_lastL = encoder_countsL;

	// Calculate velocity in rpm
	motor_velocityR = (1000.0 / delta_msR) * delta_encR * 60.0 / ticks_per_rev;
	motor_velocityL = (1000.0 / delta_msL) * delta_encL * 60.0 / ticks_per_rev;
}

/*Update the velocity tbh controller variables RIGHT*/
void
FwControlUpdateVelocityTbhR()
{
	// calculate error in velocity
	// target_velocity is desired velocity
	// current is measured velocity
	current_errorR = target_velocityR - motor_velocityR;

	// Calculate new control value
	driveR =  driveR + (current_errorR * gain);

	// Clip to the range 0 - 1.
	// We are only going forwards
	if( driveR > 1 )
		driveR = 1;
	if( driveR < 0 )
		driveR = 0;

	// Check for zero crossing
	if( sgn(current_errorR) != sgn(last_errorR) ) {
		// First zero crossing after a new set velocity command
		if( first_crossR ) {
			// Set drive to the open loop approximation
			driveR = drive_approxR;
			first_crossR = 0;
		}
		else
			driveR = 0.5 * ( driveR + drive_at_zeroR );

		// Save this drive value in the "tbh" variable
		drive_at_zeroR = driveR;
	}

	// Save last error
	last_errorR = current_errorR;
}


/*Update the velocity tbh controller variables */
void
FwControlUpdateVelocityTbhL()
{
	// calculate error in velocity
	// target_velocity is desired velocity
	// current is measured velocity
	current_errorL = target_velocityL - motor_velocityL;

	// Calculate new control value
	driveL =  driveL + (current_errorL * gain);

	// Clip to the range 0 - 1.
	// We are only going forwards
	if( driveL > 1 )
		driveL = 1;
	if( driveL < 0 )
		driveL = 0;

	// Check for zero crossing
	if( sgn(current_errorL) != sgn(last_errorL) ) {
		// First zero crossing after a new set velocity command
		if( first_crossL ) {
			// Set drive to the open loop approximation
			driveL = drive_approxL;
			first_crossL = 0;
		}
		else
			driveL = 0.5 * ( driveL + drive_at_zeroL );

		// Save this drive value in the "tbh" variable
		drive_at_zeroL = driveL;
	}

	// Save last error
	last_errorL = current_errorL;
}

/*Task to control the velocity of the flywheel */
task
FwControlTask()
{
	// Set the gain
	gain = 0.00025;

	// We are using Speed geared motors
	// Set the encoder ticks per revolution
	ticks_per_rev = MOTOR_TPR_393S;

	while(1)
	{
		// Calculate velocity
		FwCalculateSpeed();

		// Do the velocity TBH calculations
		FwControlUpdateVelocityTbhR() ;
		FwControlUpdateVelocityTbhL() ;

		// Scale drive into the range the motors need
		motor_driveR  = (driveR * FW_MAX_POWER) + 0.5;
		motor_driveL  = (driveL * FW_MAX_POWER) + 0.5;

		// Final Limit of motor values - don't really need this
		if( motor_driveR >  127 ) motor_driveR =  127;
		if( motor_driveR < -127 ) motor_driveR = -127;
		if( motor_driveL >  127 ) motor_driveL =  127;
		if( motor_driveL < -127 ) motor_driveL = -127;

		// and finally set the motor control value
		FwMotorSetR( motor_driveR );
		FwMotorSetL( motor_driveL );

		// Run at somewhere between 20 and 50mS
		wait1Msec( FW_LOOP_SPEED );
	}
}


task usercontrol()
{
	// Start the flywheel control task
	startTask( FwControlTask );

	// Main user control loop
	while(1)
	{
		// Different speeds set by buttons
		if( vexRT[ Btn8L ] == 1 )
		{
			FwVelocitySetR( 144, 0.55 );
			FwVelocitySetL( 144, 0.55 );
		}
		if( vexRT[ Btn8U ] == 1 )
		{	
			FwVelocitySetR( 120, 0.38 );
			FwVelocitySetL( 120, 0.38 );
		}
		if( vexRT[ Btn8R ] == 1 )
		{
			FwVelocitySetR( 50, 0.2 );
			FwVelocitySetL( 50, 0.2 );
		}
		if( vexRT[ Btn8D ] == 1 )
		{
			FwVelocitySetR( 00, 0 );
			FwVelocitySetL( 00, 0 );
		}


		// Don't hog the cpu :)
		wait1Msec(10);
	}
}

Thanks!