IME Flywheel Contol For Easy C

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