PID Single Flywheel Assistance

VEX V5 General Discussion Technical Discussion
1

Hey guys,
My team and I are using PID for our flywheel. Right now we are calculating our RPM using a quad encoder before going through the PID section of the code, which seems to delay the increase in motor power by a few ms after a ball is shot because the RPM calculation has a 20ms wait. I was wondering how everyone else calculates RPM using the quad encoder and incorporates it into the velocity controller such that the flywheel speeds up right after a ball is shot. Below is our code for how we calculate rpm:


#pragma config(Sensor, dgtl6,  flywheelE,      sensorQuadEncoder)
#pragma config(Motor,  port1,           conveyor,      tmotorVex393_HBridge, openLoop, reversed)
#pragma config(Motor,  port2,           flywheelTR,    tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port3,           flywheelBR,    tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port4,           flywheelTL,    tmotorVex393_MC29, openLoop, reversed)
#pragma config(Motor,  port5,           flywheelBL,    tmotorVex393_MC29, openLoop)
#pragma config(Motor,  port6,           intake,        tmotorVex393HighSpeed_MC29, openLoop, reversed)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

float Batt = 0;
float vel  = 0;
float vel2 = 0;
float targetRPM;
float targetPow;
float mPow=0,P,I;
float acc=0, velP=0;
float RPM = 0;
float encoder_1;
float encoder_2;
float err;

void CalculateFlywheelVelocity ()
{

		encoder_1 = SensorValue[flywheelE];
		RPM = -(encoder_1 - encoder_2)*2;
		encoder_2 = encoder_1;
		wait1Msec(20);
}
task SetFlywheelPower ()
{
	while(true)
	{
		CalculateFlywheelVelocity();
		PID_control();
	}
	wait1Msec(5);
}

Thanks for any help!