Okapi AsyncVelPIDController setup problems

I’ve been trying to set up a flywheel PD for a while, with no success.

I know tuning flywheel PDs can be difficult and time consuming, that’s not the issue. For some reason, no matter how I’ve tried configuring okapi’s AsyncVelControllerBuilder, the resulting controller approaches the same RPM value, irrespective of the controller’s target.

Technically that’s not quite true. kF seems to work as expected, although pure feedforward isn’t the best solution, and I can get it to not turn on at all if I zero out everything, but otherwise it always goes to ~400 RPM. The rate at which that RPM is reached is changed based on kP, but not the destination value.

flywheel_controller = okapi::AsyncVelControllerBuilder()
        .withGearset({okapi::AbstractMotor::gearset::blue, (64./(6.*84.))}) // 64:84 3600 rpm cartridge
        .withVelMath(okapi::VelMathFactory::createPtr(okapi::imev5BlueTPR * 64./(6.*84.))) // Ticks per WHAT revolution? Motor or end result?

If I take away the velMath and gains, making it the integrated encoder, it works perfectly fine, which makes me think the issue lies somewhere in that change.

The gains definitely aren’t just too large - I’ve decreased them to the point where it stopped registering, then brought them just above that.

Did I set up the VelMathFactory wrong? You’re meant to input the ticks per rotation, but it doesn’t specify if it’s the TPR of the motor shaft, or the output shaft. I’ve tried both, and it didn’t change anything. I’ve actually stripped out the gear ratio entirely, telling the motor it’s 1:1, and it still had the same issue. I also tried assigning the velMath pointer to an external variable and that wasn’t allowed, so I’m not sure what else the issue could be.

One other thing that’s occurred to me is that something’s going wrong with assigning the target RPM. However, when it’s a pure feedforward controller it responds to changes in input appropriately, and that wasn’t causing issues with the integrated version.

Pure feedforward with different inputs:

Anyone who has experience with this sort of thing, am I doing something obviously wrong with this setup? I could just create my own PID, but even if I do I’d like to know what’s going wrong here.