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.
- Velocity error due to scheduler delays.
- 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);
}
}
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.
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.
- Put back in the wait1Msec call (or endTimeSlice) in the main task.
- Run the velocity task at a higher priority than the main task.
- 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.
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.
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.
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.
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);
}
