I have a reliable method for controlling the speed of a flywheel with IMEs. It allows our autonomous to reliably get 4 balls into the high goal. It’s written in Easy C, but is pretty easy to understand and adapt.

## The Code

```
void SpeedHold(float t)
{
int pwr;
//ds and ps are global variables (Float)
ds = IntegratedMotorEncoderSpeed() - ps;
pwr = 43.391*(t*t) - (4.3165*t) + 36.542 - 43 * (ds);
//This equation^ must be empirically determined
if (ds > 0)
{
ds = 0;
}
if (GetSpeed() < .5 * t)
{
pwr = 127;
}
SetMotor(4, -pwr);
SetMotor(7, pwr);
ps = GetSpeed();
}
```

## How it Works

So the premise is simple:

-If the speed is less than the target, move at full power (127)

-If the speed is at target, hold power

-as the ball passed through, increase power to maintain speed

I used a simple test program and Microsoft Excel to get the equation to relate speed and power. It’s a quadratic fit with R^2 > .99

Feel free to ask me anything about this code or C programming in general