Velocity calculation traps

I hinted in another thread that there are some potential programming scenarios that will cause errors in velocity calculations. There are two issues that I want to address in this thread.

  1. Velocity error due to scheduler delays.
  2. Velocity error due to use of multiple IMEs

Quick recap on velocity calculation, this line of code is pulled from the flywheel code I posted a few months ago.


// Calculate velocity in rpm
motor_velocity = (1000.0 / delta_ms) * delta_enc * 60.0 / ticks_per_rev;

We have two variables in this calculation, delta_ms, the change in time and delta_enc, the change in encoder count. If either of these is inaccurate then the calculation of velocity will be inaccurate.

Baseline for these examples is a single IME being sampled every 20mS. The motor is geared for speed and there is no load. motor control value was 75, there is no PID or TBH, this is just open loop control. When running as a single task and using the datalog capability of ROBOTC we have a baseline calculation as follows. (not real code, this just shows the concept)


task main()
{
    while(1) {
      driverControl();
      calculateVelocity();
      
      wait1Msec(20);
    }
}

rpm_base.jpg

The red line shows delta_ms and is constant at 20mS. The blue line shows delta_enc and has a nominal value of about 21 counts, it varies between 19 and 22 counts, this fluctuation in delta count translates directly into a velocity fluctuation. I’m using a motor with speed gears and the resultant velocity varies between 145rpm and 168rpm.

Error due to scheduler delay.
It’s typical to put the velocity calculation in a separate task, this is how much of the code I have previously posted is configured. I also often see students omit the wait1Msec call in the usercontrol task, the layout of code could be as follows.

task velocity()
{
    while(1) {
      calculateVelocity();
      
      wait1Msec(20);
      }
}

task main()
{
    startTask( velocity );
    
    while(1) {
      driverControl();
    }
}

In this case wait1Msec has been omitted from main, lets see what the graph of velocity calculation would look like if we assume that delta_ms is still constant at 20mS.
rpm_task_delay.jpg
Notice how delta_enc is now changing over a (slightly) wider range and how the average has shifted to a higher value. This results in even more fluctuation in the velocity calculation. What has happened is that the assumed 20mS delay in the velocity calculation task has increased to perhaps 21 or 22mS. This has happened because the main task is not allowing the ROBOTC scheduler to switch tasks as often, ROBOTC will allow the main task to run for some period of time and will then force a check to see if other tasks need to run, only at that point will the velocity task run and the delay can often be longer than 20mS. There are three ways to solve this.

  1. Put back in the wait1Msec call (or endTimeSlice) in the main task.
  2. Run the velocity task at a higher priority than the main task.
  3. Use a calculated elapsed time in the velocity task rather than assuming it was a fixed delay.

Lets see what happens when we use technique 3.
rpm_task_delay_var.jpg

Now the delta_ms variable (red line) is changing between 20mS and 23mS as the task is allowed to run by the scheduler. The calculation of velocity is now back at the baseline level of the first graph.

edit: I should add that most of the recent code I posted does this already. I would also add that all three techniques should really be used, keep the wait1Msec in all tasks, no need to run most of them constantly. Increase the priority of the velocity calculation task up a bit, “normal” priority is 7, so start the velocity calculation at perhaps 10.


    startTask( FwControlTask, 10 );

To read more on ROBOTC multi-tasking refer back to my old posts from 2011.
https://vexforum.com/index.php/conversation/post/53786
https://vexforum.com/index.php/conversation/post/53799
https://vexforum.com/index.php/conversation/post/53858
https://vexforum.com/index.php/conversation/post/53860
https://vexforum.com/index.php/conversation/post/54070

Errors due to multiple IMEs
IMEs share a common communications path, only one IME can be communicated with at a time. When several IMEs are daisy chained together, ROBOTC will ask each in turn for the current value of its encoder count, if three IMEs are connected this means that each IME encoder value will be updated (approximately) every 3mS. So what does this mean for our velocity calculation code? Lets go back to the baseline code and run the experiment with three IMEs connected and configured instead of one, here is the resultant graph.

rpm_3enc.jpg

Wow, encoder counts and velocity calculation has degraded considerably, velocity is now fluctuating between 137 and 183 rpm, nothing else changed.

What is happening is that, although our delta time is remaining constant, the encoder count we retrieve may have not been read for some time (up to 3mS in the worst case with this example) due to the sequential nature of reading the encoder count from each IME in turn. As more IMEs are added this characteristic will change. It will also change as the time in the wait1Msec call is modified, for example, if that were increased by 1mS I would expect an improvement as 21mS is a multiple of 3. The best solution is to use the functionality that ROBOTC gives us to retrieve a timestamp that is appropriate for the encoder value. We do this with the function getEncoderAndTimeStamp, here is the resultant graph when using that call instead of nMotorEncoder.

rpm_3enc_time.jpg

The delta_ms variable is now swinging more than before, but the velocity calculation is back (more or less) to the baseline level we had in the first graph.

Filtering.
The final improvement we can make to this calculation is to filter the calculated velocity to remove the sample by sample variations in encoder count that we read. Here is one example that uses a simple IIR filter where only 20% of every new calculated velocity is used. There are other types of filter that could be used, but that discussion is for another day.
rpm_3enc_time_fil.jpg

The final velocity calculation code was as follows, quite similar to that which I posted in the past but with the minor improvements to allow better operation with multiple IMEs and simple filtering of the calculated value.


// assumption is that this code is called every 20mS
// all variables are global
void
FwCalculateSpeedWithEncTime()
{
    float motor_velocity_t;
    
    // Get current encoder value and time
    getEncoderAndTimeStamp(Motor_FW1, encoder_counts, encoder_timestamp);
    
    // This is just used so we don't need to know how often we are called
    // how many mS since we were last here
    delta_ms = encoder_timestamp - encoder_timestamp_last;
    encoder_timestamp_last = encoder_timestamp;

    // Change in encoder count
    delta_enc = (encoder_counts - encoder_counts_last);

    // save last position
    encoder_counts_last = encoder_counts;

    // Calculate velocity in rpm
    motor_velocity_t = (1000.0 / delta_ms) * delta_enc * 60.0 / ticks_per_rev;
    motor_velocity = (motor_velocity * 0.8) + (motor_velocity_t * 0.2);
}

1 Like

Wow. Thanks for this information; it’s really helpful. We’re using TBH for our flywheel and the jitter in the data is such that, for a setpoint of 140 RPM, we’ll see values change from 140 to 156 in a single 20 ms interval, which obviously isn’t logical. I’ll have to try some of these tips next time I’m on the lab - and I should note that I now include a wait1Msec call in my driver control task because of you.

Thank you for all your work James. I need to read this about 5 more times to fully understand it (coding is not my thing), but I have no doubt it’s going to be helpful for us at our next practice.

OK, so I just tested this. It seems like it indeed reduced the average RPM jitter on our flywheel to about 5-6 RPM of jitter, vs. about 16 before. Thanks!

Another one of the all time great posts by James!

How long does the IIR filtered velocity take to react when you put a ball in there?

Most likely too long. It may not be the best filter for this application and will be a tradeoff between removing the noise and detecting the transients when a ball hits the flywheel. Something to experiment with. Here’s a graph with the above data filtered a couple of different ways in excel and some artificial impulses added to the data. x scale is mS, notice what happens when I added an artificial impulse at 2.5 seconds in.

rpm_filter_example.jpg

I would also add that this looks smoother than the final graph in post #1, that’s probably due to calculations here being floating point where datalogging in ROBOTC can only store integer values.

1 Like

Excellent stuff, everyone should start with these corrections before fiddling with their control anymore. Garbage In Garbage Out (GIGO).

Didn’t know this even existed, our HELP file for RobotC stopped working long ago.

The distribution looks like even this would help a good bit with a little less phase delay:

motor_velocity = (motor_velocity * 0.5) + (motor_velocity_t * 0.5);

Or just (last + present)/2 if you don’t want the IIR.

This post has inspired my team to dive into using the debug stream. We have never used it before and just learned how to use it tonight (I know that’s sad, we should have used it years ago). We now have a spreadsheet full of data and some pretty ugly flywheel rpm graphs (lots of dips and spikes). It is so useful for the kids to be able to see that information visually as a graph. Over the next week we will be trying to get our speed more consistent thanks to this post.

Once again, thanks @jpearman