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).
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.
Thank you for the information on how to calculate velocity! This is exactly what we needed a week ago when we were spending hours writing our TBH code. In your code you use
static
and
static long
. Do you mind explaining what they do? I don’t have much experience in programming, and am still learning. Thank you in advance.
I can give a quick summary.
You are probably familiar with global variables, you define them outside of any functions and they retain their values for the whole time the program is running. You can see them in the debugger window for global variables. Local variables are defined inside of a function (or task), they do not retain their value between calls to the function and use a different type of memory for storage. You can have several local variables in different functions that have the same name, for example, you may have a variable called “x” in more than one function. A static variable is like a mixture of the two, if defined inside a function it retains its value between calls but you could reuse its name in another one. I use static variables in the example code as I need to remember what the value is each time that function is called. They also keep your code cleaner, the variable is defined at the place where it is used, it makes the code more portable (ie. you could copy the function to another program and it would still work, it is self contained).
Variables can have different types, int, float, char etc. long is just another type of variable, it is guaranteed to be of a particular size, that is, it can hold values in a certain defined range. On the cortex (using ROBOTC V4.5X) a long is the same as an int, however, that is not always the case. I use the type long as I know that the IME can give back values that can become large and a long variable is suitable to hold them. An int would have worked just as well with ROBOTC V4.52, that was not the case with ROBOTC V3.XX where an int could only hold values in the range -32768 to 32767 (16 bit values).
Is there any reason an IME would be reading slower in RPM than a QuadEncoder directly geared down from the flywheel? I have a 1:35 single flywheel with an IME and A quad encoder geared 4:1 directly from the flywheel. Is there any reason there would be an almost 200 RPM difference?
I realize that the code I’m posting is complicated for beginners, however, I’m pushing out yet another version. This is (more or less) the code I would probably use for myself. It’s the “best” algorithm from post #1 with a few extra enhancements.
It uses a structure to collect all the motor related data together. The velocity calculation code takes a pointer to said structure, this allows the function to be reused for more than one motor, a deficiency of the original code.
It understands quad encoders a little better and automatically sets everything up depending on how the program has been configured.
It includes the estimated current calculation from my smart motor library. Knowing how much current the motor is using is useful in keeping those flywheel motors working within their limits.
code is here.
The demo shows how to configure it to monitor two independent motors.
The next step would be to add PID or TBH control variables to the motor control structure and the necessary additional functions.
Just for interest, here is the demo program logging velocity and current when I sent a control value of 100 to a single motor. Notice how the current spikes up to about 1.6A as the motor starts (this was a free running motor, no load) and then settles down to about 240mA as the motor reaches its final speed.
It works with either… as long as you have your motors and sensors properly setup and defined in the “Motor and Sensor Setup” tool, @jpearman has included coding that will automatically make the proper adjustments to the code depending on what type of motor and/or sensor you are using. Absolutely beautiful code!
I am really hoping to meet @jpearman at Worlds this year… right now I imagine him as a highly modified centaur where the horse half is made out of robot parts! He does some amazing work and I love the fact he is so willing to share his expertise with all of us. Thanks again James!
Thanks tabor! I had seen those values in almost all the velocity control programs I’ve come across on the forum, and I had always wondered where they came from
This thread being necroed made me realize the picture had been replaced.
@GreenFire729 that says something about where people get their code more than anything else. For general use programs like James’ it makes a lot of sense to have it detect motor type and use its encoder ticks per revolution to calculate rpm. Most specific case programs I have seen still have those in there even when they aren’t being used. I have seen a few PID programs who have James ’ TBHVelocity calculate function. Obviously it’s fine, calculating a decent rpm calculation is easy but everyone should be able to learn from a little more rugged system like James’.
I tried last year and failed.