Blast from the Past (TBH Tuning for Nothing But Net)

Hey, so I’m tuning my TBH algorithm on my Nothing But Net robot in preparation for TSA Nationals in Nashville. Something doesn’t seem right to me in this graph:
Blue is the set flywheel velocity in RPM (at 2800 RPM)
Green is the flywheel velocity in RPM.
Golden Yellow is the commanded motor power times 28 (so that it can be seen on the graph. Max is 3556 (127) ).

From what I understand, the TBH algorithm should switch over from open to closed loop control once the flywheel velocity crosses the setpoint, so that means the yellow line should drop at the point where the green line first meets or exceeds 2800 RPM. But according to this graph, the TBH is switching over before that setpoint is reached. Is this normal or if not, why is this happening? I’m using a predicted drive of 0.55 and a gain of 0.00080.

Can you post your code? Hard to see without the code…

Any moving averages of the velocity int he graph but not in the TBH?

Or, just go with bang-bang. Works great

Oh, and be sure to say to team 91C Cyclops of the Vexmen in Nashville! They won PA TSA’s and will be going to TSA Nationals.