When performing slow turns, the heading rate response can remain below the gyro deadzone for periods of many seconds. During this period, the gyro angle is not accumulating the true turn rate and the error is equal to amount the robot turned during this period. (i.e. the area under the heading rate curve).
The gyro deadzone is hidden from the user but is set by a #define nGyroJitterRange 4
which places its value at 2.8 degs/sec. The time spent below this rate multiplied by the average rate is the heading error. If the rate response is exponential… then the error is tau2.8 deg/sec where tau is the time constant of the response.
I.e. I use a PID way point tracker which commands a heading response with a tau = 1 second. The rate can be modeled as 100 deg/secexp(-time/tau). The area under the tail when the rate gets below the deadzone is 2.8 degs. So this error occurs for every turn and is cumulative if the turns are in the same direction.
Edit: One way to minimize this error is to delay the deadzone until the turn is finished. For an exponential turn rate response this is about 3 time constants to get about 99% of the rate response included. For a tau = 1 sec this is 3 seconds.
Since the deadzone is delayed for 3 seconds gyro drift due to noise can occur. For a typical drift rate of .1 deg/sec we can pick up a .3 deg error which would not have been there if the deadzone were active. So this is the trade off…the net savings makes it worth while.
I have some sample code that implements the above timed deadzone.
First in loadBuildOptions routine set the internal deadzone to zero by this define:
#define nGyroJitterRange 0
Then in my user code I let the RobotC gyro do its integration and use the gyro angle to derive my own rate for the period of say dt = 10 ms. I then process the rate with a timed deadzone and then reintegrate. Its a little weird but here is my code: You have to be careful of gyro rollovers unless you use the continuous version which I have assumed here.
float psi_est = 0; // my float heading estimate
float delta_psi = 0;//difference in heading for an iteration
int dt = 0; // iteration time
int start_time_delay = 0; // this is the time when the rate first falls below deadzone
//..............................................................
gyro_10 = SensorValue[gyro];
delta_psi = gyro_10 - gyro_10_last)/10.;
gyro_10_last = gyro_10;
dt = time1[T4];
ClearTimer(T4);
if(abs(delta_psi) > 2.8*dt/1000. )
// this is equivalent to #define nGyroJitterRange 4 in loadBuildOptions
// I have set #define nGyroJitterRange 0 so gyro has no deadzone.
{
psi_est = psi_est + delta_psi ;
start_time_decay = time1[T1];
}
else
{
if(time1[T1] < start_time_decay + 3000)
//this allows the deadzone to remain out for 3 extra seconds
{
psi_est = psi_est + delta_psi ;
}
else delta_psi = 0;
}
I had thought nGyroJitterRange was only used when the ROBOTC firmware was built and did not have any effect at runtime in our code, have you found otherwise?
I never make changes during runtime. I just change the define and let it compile. My code is perhaps unclear on that point. I have yet to hear from RobotC on the actual width of the deadzone (eg is it plus minus or total?)
What I meant was, I thought this definition was used only when the CMU guys compile the ROBOTC firmware (that we download once), in other words, I did not think it was possible to redefine this for user code and have it make any effect, I thought it was hard coded into the VM.
Oh. IDK… the define is in the loadBuildOptions() …doesn’t this affect a compile if we change the define there? Could be the deadzone is still in there. I haven’t completed my detailed testing since my gyro is out on loan. The testing I have done so far is with simulation only and it works fine there.
If you are right (which you usually are) I will have to use my own gyro processing routine to put this idea into practice.
If RobotC doesn’t give us any control here it will be on my request list for the next release.
Team80 asked me to show some of my simulation results. Here is a response comparison with and without the timed deadzone on a typical waypoint steering heading response. The curves show and improvement from 3.2 degrees to .3 degrees with a 3 second timer added. The error is almost zero with a 4 second timer.
Just for fun, I also added a typical PID heading loop response with different KD rate gains that cause the response to be more of less damped. With no rate feedback, the response overshoots and settles quickly causing only a small error. With the more damped response the error gets to about 2 degs. Again these are just simulated responses but they give you an idea about what happens if you use a highly damped heading response.
These responses have no timer. https://vexforum.com/attachment.php?attachmentid=7363&stc=1&d=1364949757
I found this code posted on the ROBOTC forums by Jesse Flot, I did not have time to study it yet but it does answer some of our questions. I was searching on SensorSmallBias as I remembered it was added to help with some of these sensors. Original thread is here.
#pragma config(Sensor, in3, gyro, sensorGyro)
//*!!Code automatically generated by 'ROBOTC' configuration wizard !!*//
///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
//
// Gyro Driver Test
//
// This program emulates the gyro value calculation of ROBOTC's internal driver. It only emulates the value calculation.
// It does not emulate the calculation of the bias values and relies upon the value calculated by the internal driver.
//
// When gyro driver starts up, it first calculates the internal bias value. The bias value is the analog value output
// by the gyroscope while it is perfectly still. The steps to calculate the bias.
//
// 1. Delay for 200 milliseconds. This is to allow the gyro to stabilize when it is first powered up. The datasheet
// indicates that this may take 50 milliseconds so we'll run it a little longer.
//
// 2. Measure the analog value every 0.001 seconds and calculate the sum over 1024 samples.
//
// 3. The bias can be calculated. Take the sum from (2) and divide by 1024.
//
// 4. Normally the remainder from (3) is non-zero. The remainder term is called the "small bias" by ROBOTC firmware.
//
// 5. Driver is now ready to calculate the gyro value.
//
// The gyro does not output an analog value representing angular position. Instead the value represents angular velocity.
// To calculate angular position, you have to integrate periodically measured samples. The internal ROBOTC driver does
// this is follows:
//
// 1. Every millisecond it sums the "difference" of the current analog gyro value with the calculated bias value. This
// effectively integrates the gyro velocity values into a gyro rotation value.
//
// 2. In (4) above there was a residual error (i.e. the small bias) calculated. So on every 1024 sums, the 'small bias'
// is subtracted from the accumulated sum.
//
// 3. When user application program requests the gyro value, the accumulated sum is divided by the 'SensorScale' setting
// to scale the result in tenths of a degree.
//
// 4. The analog gyro value has a lot of jitter; i.e. if the bias value is 1823 then the analog readings on a non-moving
// gyro may be 1820 to 1826. The jitter does not appear to be uniformly distributed around the bias value. So the driver
// has a "jitter filter" to ignore analog values that are within '4' (see '#define' for 'kJitterIgnoreRange') of the
// bias value. This eliminates the
//
// 5. Even with point (4) above, there is a small drift in the gyro. This is typical of these gyro ICs. Over several minutes
// the gyro value range -15 to +15 degrees (i.e. value of 150 in tenths of a degree) .
//
//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
long nNumbOfADCConversionCycles = 0; // Performed by special debugging hook in firmware
long nCycles = 0;
long nElapsedTime;
long nFilteredRawValueSum = 0;
long nGyroValue;
volatile long nFilteredRawValue;
long nNonJitterCycles = 0;
typedef enum
{
biasMinus6orLess,
biasMinus5,
biasMinus4,
biasMinus3,
biasMinus2,
biasMinus1,
bias,
biasPlus1,
biasPlus2,
biasPlus3,
biasPlus4,
biasPlus5,
biasPlus6orMore,
} TCountsIndices;
long nGyroRawAnalogValue;
long nGyroBias;
long nSensorSmallBias; // Performed by special debugging hook in firmware
long nGyroSensorScale;
long nGyroFullCount;
long nCounts(TCountsIndices) 13];
short nMaxValueCurr10Seconds;
short nMinValueCurr10Seconds;
short nPrev10SecondTime = -1;
short nCurr10SecondTime;
const int kHistogramSize = 600;
short nMaxValues[kHistogramSize];
short nMinValues[kHistogramSize];
long nLoopStartTime;
long nPrevTime;
long nCurrTime;
const int kJitterIgnoreRange = 4;
task main()
{
// Firmware retains sensor type setting between invocations of program. Explicitly setting to 'sensorNone' and
// then back to 'sensorGyro' will explicitly force the firmware driver to recalculate the biase settings. Which
// you may want to do via the Debugger's "STOP" and "START" buttons.
SensorType[gyro] = sensorNone;
SensorType[gyro] = sensorGyro;
memset(&nCounts[0], 0, sizeof(nCounts));
memset(&nMaxValues, 0, sizeof(nMaxValues));
memset(&nMinValues, 0, sizeof(nMinValues));
nNumbOfADCConversionCycles = 0;
nFilteredRawValueSum = 0;
while (SensorBias[gyro] == 0)
{}
SensorValue[gyro] = 0;
nGyroBias = SensorBias[gyro];
nSensorSmallBias = SensorSmallBias[gyro];
nGyroSensorScale = SensorScale[gyro];
nGyroFullCount = SensorFullCount[gyro];
nLoopStartTime = nPgmTime;
while (true)
{
short nDifference;
++nCycles;
nGyroValue = SensorValue[gyro];
nGyroRawAnalogValue = getSensorRawADValue(gyro);
nDifference = nGyroRawAnalogValue - nGyroBias;
if ((nDifference < -kJitterIgnoreRange) || (nDifference > +kJitterIgnoreRange))
{
nFilteredRawValueSum += nDifference;
++nNonJitterCycles;
if ((nNonJitterCycles % 1024) == 0)
nFilteredRawValueSum -= nSensorSmallBias;
}
if (nDifference < -6)
nDifference = -6;
else if (nDifference > +6)
nDifference = +6;
++nCounts[bias + nDifference];
nFilteredRawValue = nFilteredRawValueSum / nGyroSensorScale;
// Calculate statistics and value histogram
nCurrTime = nPgmTime;
nElapsedTime = nCurrTime - nLoopStartTime;
nCurr10SecondTime = nElapsedTime / 10000;
if (nCurr10SecondTime != nPrev10SecondTime)
{
if (nPrev10SecondTime >= 0)
{
nMaxValues[nPrev10SecondTime] = nMaxValueCurr10Seconds;
nMinValues[nPrev10SecondTime] = nMinValueCurr10Seconds;
}
nMaxValueCurr10Seconds = nGyroValue;
nMinValueCurr10Seconds = nGyroValue;
}
if (nGyroValue > nMaxValueCurr10Seconds)
nMaxValueCurr10Seconds = nGyroValue;
else if (nGyroValue < nMinValueCurr10Seconds)
nMinValueCurr10Seconds = nGyroValue;
nPrev10SecondTime = nCurr10SecondTime;
while (nCurrTime == nPrevTime)
{
nCurrTime = nPgmTime;
}
nPrevTime = nCurrTime;
//wait1Msec(1);
}
}
I did some testing using the CMU demo code modified to work with a gyro test rig I had put together. It works pretty well and is a good simulation, not really practical to add to normal code as it’s a real cpu hog due to the final while loop.
This can be replace by a wait1Msec(1) but it’s less accurate. Any other task using cpu time will also effect accuracy of the simulation. If the sim is run at a higher priority then lower priority tasks never get any time as the sim never yields.
The test rig I was using looks like this.
An IME encoded 269 motor driving a platform with the gyro through a 3:1 gear ratio (12 tooth on motor to 36 tooth under the platform). The code uses the encoder to swing the platform back and forth through 180 degrees. It won’t help test the deadband delay code but did help prove that the CMU sim was reasonable.
Like you I decided to do a bit of testing. Turns out that the program you posted is also in the Vex2/samples/gyro/Gyro Driver Test.c in ver 3.6. It probably has been there in earlier versions all this time.
Anyway, it answers my questions to RobotC folks… the dead zone is ± 4 counts and the update rate is 1 msec. The ncounts distribution for my test gyro showed that 4 counts above the bias is enough to keep the noise out. In fact , 4 counts is so rare I would use 3 instead.
I took out the statistics code and ran the routine without the
while (nCurrTime == nPrevTime) {…}
delay loop to see how much cpu was used. Turns out it ran in .04+ ms so I suppose that either they run it with periodic interrupts or possibly they have waitusec(10) function that they could insert to give other tasks a chance to run. It seems to me that if we used the gyro driver test code we could use a waitMsec(1) and then change the scaling to reflect the added delay of the code. Of course, the task interrupts would cause a slight variation but I think its the best we can do, right?
BTW… does the debug task cpu demand enter into that .04+ms or is it done separately by another processor?
My test rig is a little less sophisticated… I simply turn the gyro on its side or upside down to get 90 or 180 deg changes. My gyro show about 93 to 94 degs for a 90 deg turn. I will next try it without the 4 count deadzone and then maybe with the timer added. This will have to be done with maybe a batch of 10 to 20 runs to get a statistical average to compare.
It’s there in 3.51, not sure about anything earlier than than.
The built in code will run faster than anything we write as it’s compiled for the STM32 and is part of the virtual machine.
This is the best we can do in user code for now, perhaps CMU can make the deadzone a variable for us so the built in deadzone can be overridden.
The debug task is running on the same processor but again is native STM32 code and does not run in the same way as user tasks, think of it more as part of the real time kernel. As we are running our code in a virtual machine things like interrupts are not handled quite like you would think, I don’t have enough knowledge to explain how it works (or permission) but generally I don’t see debugging having a significant impact on code execution times.
Well, that’s what I have done in the past, I just wanted something where I could run many itterations with an accurate movement over several minutes. I don’t have anything useful to post yet, sometimes it’s surprisingly accurate, sometimes not. I wish the gyro chosen had a smaller full scale range, ST make another part with +/- 300 dps full scale which may have been more suitable for this application.
I also have one of these from Yost Engineering and want to try it running alongside the VEX gyro, not a project that’s going to get done any time soon but I’m interested to see if a (on paper) more sophisticated part would give any improvement.
Grrrr… don’t get me started on this. When the gyro first came out it was painfully obvious that it was going to be a problem for anything requiring precise rates such as a compass or a pendulum robot sensor. I spoke about it in a blog post in 2011.
.
I would like to know the Vex engineers thought process. (Vex comments welcome here).
Perhaps they were influenced by a guy who flew heliocopters:) I don’t know how much pre design feedback they got from the users…but I certainly would have lobbied for selectable gyro scaling. Perhaps most selectable gyros are SPI interface like this $30 allelectronicds gyro and they didn’t want to use that. They gave us a selectable accelerometer but didn’t reason that the gyro needed to be selectable too. The yost Imu/compass would be nice to have as a Vex sensor.
In User mode, perhaps the 1000deg/s gyro is warranted due to possible impact rates but in Autonomous where it is probably used the most, the max rate for typical size robot is say 200 deg/s in heading axis. Of course, someone could build a tiny wheel base robot with but I’m interested to see if a (on paper) more sophisticated part would give any improvement.
big wheels that might spin quicker … but not likely in competitions. Most people don’t use gyros on arms (where you might possibly see rates up to 1000 deg/s with a motor geared at 160rpm) so I wouldn’t design for that.
So…If you have a chance…lobby for a new gyro folks.
I am curious too… If you want, I have an accurate $400 Silicon Sensing gyro you could add to your testing.
It has an I2C interface as well as SPI, not sure we want to go there after the IME experience. Best thing would be to include it in the cortex_Mk2 Perhaps I will order one and try it out.
The Yost IMU is a nice unit but would be expensive if packaged for VEX with the usual markups etc. Perhaps one of the college teams could use it, I think it’s legal for them.
Ours is off the robot and in with spare parts where it will live the rest of it’s day’s. I would love to see something better and faster. We have had way too good of luck with encoders to waste any more time with this. I wish they had made it bigger and heavier so it would at least make a good paper weight.
The vex gyro may have it’s place on a robot, but not a competition robot in my opinion. We know where the sacks are so solving for X is the best way to deal with autonomous code.
I have to disagree, we have used it for accurate 90 degree turns and find it far superior to encoded wheels. I have also used it for what I call “field centric control”, the joystick movements adjust so that forwards is always away from the driver, this works really well although not many students wanted to use it. The problem Chris is describing affects slow movement, turning 90 degrees tends to have the gyro move out of the deadband quickly and the whole move takes perhaps 2 seconds. It’s a relative movement to a starting position that has been determined using alternative methods. Anyway, just my opinion, sensors can always be better but I still find the gyro a useful device.
Maybe in about 50 years when we are 1/10 as good as you at writing code we can try it again Until that time comes we will stick with what we have mastered. I may try to play with it some more after worlds, but we have let 1 difficult design eat up too much valuable time already. It may be beginners luck, but we have managed to score nearly 100% of the time in autonomous using encoders. I would love for us to master gyro’s enough to do a jaw dropping programming skills it would be neat to see a robot spend the entire minute scoring while never being touched or repositioned!
vamfun, I do want to personally apologize for not getting back to you sooner on this. Prepping for Worlds has taken up most of my time, and answering this question unfortunately took longer than it should have.
You are correct, the #define nGyroJitterRange 4 refers to a ±4 count deadzone and the update rate is 1 ms.
Again, I do want to apologize for the delay in a response, we usually try to have ROBOTC related questions answered within 24~48 hours. I am going to link the post in the ROBOTC Q&A forum to this thread for future reference’s sake.
Perhaps I will order one and try it out.
Until that time comes we will stick with what we have mastered. I may try to play with it some more after worlds, but we have let 1 difficult design eat up too much valuable time already. It may be beginners luck, but we have managed to score nearly 100% of the time in autonomous using encoders. I would love for us to master gyro’s enough to do a jaw dropping programming skills it would be neat to see a robot spend the entire minute scoring while never being touched or repositioned!