I have been working on a flywheel similar to 8059, and it has been shooting just short of the goal. I have also been working on a Velocity PID algorithm, however it doesn’t stop the wheel, and always assumes maximum speed. My RPM calculation has also been climbing wildly upwards. I can’t tell if anything is working or how it goes wrong because the debugger on robotC won’t update any of the PID variables except for “loops” in a timely manner. The flywheel also makes all speeds above 90 the same, so we cant increase the speed to get the extra distance.

```
int fRPM = 0;//set in drivercontrol for flywheel speed.
int Ferror = 0;
int Fpreverror = 0;
int FdeltaT = 15;
int Fprevtime = 0;
float integral[4] = {0,0,0,0};
float rollIntegral = 0;
float Fderiv = 0;
float trueRPM = 0;
//clearTimer(T3);
int loops = 0;
float curve = 0;
task fRPMcontrol()
{
motor[launcher4] = 0;
motor[launcher3] = 0;
motor[launcher2] = 0;
motor[launcher1] = 0;
float pgain = .0003;
float igain = .0000;
float dgain = .000;
/*int Ferror = 0;
int Fpreverror = 0;
int FdeltaT = 15;
int Fprevtime = 0;
float integral[4] = {0,0,0,0};
float rollIntegral = 0;
float Fderiv = 0;
float trueRPM = 0;
*/clearTimer(T3);
//int loops = 0;
//float curve = 0;
wait1Msec(15);
while(true)
{
/*if(fRPM ==0)
{
SensorValue[flywheel] = 0;
clearTimer(T3);
wait1Msec(30);
fSpeedControl(0);
Fprevtime = 0;
}
else
{*/
if(loops > 3){loops = 0;}
FdeltaT = time1(T3) - Fprevtime;
Fprevtime = time1(T3);
trueRPM = 11.6667 * (((SensorValue[flywheel] / 360) / (FdeltaT))/**1000*/ * 60);
Ferror = fRPM - trueRPM;
integral[loops] = Ferror;
loops++;
rollIntegral = (integral[0]+integral[1]+integral[2]+integral[3])/4;
Fderiv = Ferror - Fpreverror;
Fpreverror = Ferror;
curve = 6 *((Ferror *pgain) + (rollIntegral * igain) + (Fderiv * dgain));
if(curve >6)
{
curve = 6;
}
if(curve < -6)
{
curve = -6;
}
motor[launcher1] += motor[launcher1] + curve;
motor[launcher2] += motor[launcher2] + curve;
motor[launcher3] += motor[launcher3] + curve;
motor[launcher4] += motor[launcher4] + curve;
if(motor[launcher1] >120)
{
motor[launcher1] = 120;
motor[launcher2] = 120;
motor[launcher3] = 120;
motor[launcher4] = 120;
}
/*if(fRPM == 0)
{
motor[launcher1] = 0;
motor[launcher2] = 0;
motor[launcher3] = 0;
motor[launcher4] = 0;
}*/
//wait1Msec(30);
if( abs(fRPM - trueRPM) <=50 )
{
SensorValue[flyIndicator] = 0;
}
else
{
SensorValue[flyIndicator] = 1;
}
wait1Msec(30);
//}
}
}
```