This topic keeps coming up, so here is some code to help.
The linked file has three different methods that can be used depending on how complex the code is you want to try and understand.
The first just uses the getMotorVelocity function that we introduced in V4.52. This is a good simple way but has a couple of drawbacks. 1. there will be some jitter, that is, the returned value will move around a little. 2. It will give strange values when the motor speed is low. 3. It does not include any directional information, the values returned will always be positive. 4. It only works with IMEs.
The second method is essentially what I have provided before. Velocity is calculated using the change in encoder counts over a period of time. This is also good, however, it suffers from noise (velocity errors) if you have multiple IMEs connected.
The third method uses the getEncoderAndTimeStamp function and also filters the resultant velocity.
Link to the demo code .
Here is a graph showing differences (I had three IMEs connected).
See this thread to understand why, with 3 IMEs connected, the “better” method still has jitter.
The functions also take a “gear ratio” parameter. If you want to calculate actual flywheel velocity add this parameter in, for example, you have a 60:12 external gearing (motor drives 12 tooth, 12 tooth drives 60 tooth), set the gear ratio as 5.0 (or 60.0/12.0) and the returned velocity will be that of the flywheel, not sure there is much point to this, it does not help with closed loop control.