Flywheel Velocity Controller

I am trying to create another iteration of my program shooting the four preloads as fast and accurate as possible. I want to release the ball from the top of the intake once the flywheel speeds up towards the optimal speed set by the algorithm. How do I find the speed that the flywheel is currently in and when it is in a relevantly close to the target speed. I am using a TBH loop, along with the FwVelocity control based on a space of 1 second.

There should be a variable called


that you can use. Just a note - there’s probably a fair amount of jitter in your RPM calculation, so use a range to determine when to outtake, and outtake for longer than just when the RPM is in your ideal range. Alternatively, you might want to figure out what the RPM is just before you want to shoot and outtake then so that the RPM is right when the ball is shot.