Example gyro code released.

[Kevin Watson]

I've written example gyro interface code and posted it here: http://kevin.org/frc. This code has been substantially revised since the version released earlier with the PID and scripting code. A substantial amount of documentation has also been written to support this code. As the programming interface has changed, this code will not work with the current version of robot.c. An updated version of robot.c will be released soon.

Here's the readme.txt:

The source code in gyro.c/.h contains a driver and supporting 
software to interface a variety of inexpensive gyros to your
robot controller. This software was tested with Analog Devices'
ADXRS150EB and ADXRS300EB gyros. Data sheets for these devices 
are included. Support for the ADXRS401EB is also included. By
default, this software is configured to work with a ADXRS150EB
gyro, sampling at 400Hz, downconverting to an update rate of
50Hz by averaging eight samples per update. These parameters
can be changed by editing gyro.h.
This software makes the assumption that when it's running, it 
solely controls the analog to digital conversion hardware and 
that only the gyro, on analog input one, is being sampled.
Calling Disable_Gyro() will disable the gyro software and
allow you to use the ADC for other purposes.
This source code will work with the Robovation (A/K/A EDU-RC) 
robot controller and the FIRST Robotics robot controller.
Wiring-up the ADXRS401EB, ADXRS150EB and ADXRS300EB gyro
evaluation boards is straightforward. Grab a PWM cable and cut
off the male end and strip the three wires back a centimeter 
or so. With a low wattage soldering iron, solder the white wire 
to the RATEOUT pin, solder the black wire to the AGND pin, the 
red wire to the AVCC pin, and finally connect a jumper wire 
between the AVCC and PDD pins. Plug the female end into the 
robot controller's analog input 1.
For optimum performance, you'll need to calibrate the scaling
factor to match that of your gyro's. One way to calibrate your 
gyro is to mount it very securely to a hefty, square or 
rectangular object. Mounting the gyro to a hefty object will 
help dampen higher frequency vibrations that can adversly 
effect your measurements. Place the mounted gyro against 
another square object and start the included demonstration 
software. To get good results, the mount must be absolutely 
still when the "Calibrating Gyro Bias..." message appears on 
the terminal screen. After a few seconds, the gyro angular rate 
and angle will be sent to the terminal screen. If the angle 
drifts rapidly while the mounted gyro is motonless, you need 
to restart the software to acquire a new gyro bias measurement. 
Again, gyros are very sensitive and must be still while the bias 
is calculated. Once the gyro is running with little drift, 
rotate the mount 180 degrees and note the reported angle. If the 
angular units are set to tenths of a degree, the ideal reported 
angle is 1800. If set to milliradians, the ideal angle 1s 3142 
(Pi times a thousand). For every tenth of a percent that the 
angle is high, decrease the GYRO_CAL_FACTOR numerator by one.
Conversly, for every tenth of a percent low, increase the 
numerator by one. Repeat until you're satisfied with the 
accuracy.
If you find that the Calc_Gyro_Bias() function doesn't do
a good enough job of calculating a bias for your particular
gyro, another option is to average the gyro bias for a much
longer period of time and manually set the gyro bias using
the function Set_Gyro_Bias(). An example that shows how to
do this is commented out in the accompaning user_routines.c/
Process_Data_From_Master_uP(). With a perfectly still gyro,
running this example code will display a running average gyro 
bias that should converge to one value over time. Enter this 
value into the call to Set_Gyro_bias() and comment out or 
remove the call to Calc_Gyro_Bias(). Now perform the gyro 
scaling factor calibration described above. 
The included project files were built with MPLAB version 6.60.
If your version of MPLAB complains about the project version, 
the best thing to do is just create a new project with MPLAB's 
project wizard. Include every file except: FRC_alltimers.lib 
and ifi_alltimers.lib and you should be able to build the code.
****************************************************************
Here's a description of the functions in gyro.c:

Initialize_Gyro()
This function initializes the gyro software. It should be called 
from user_routines.c/User_Initialization().
 
Get_Gyro_Rate()
This function returns the current angular rate of change in 
units of milliradians per second. If desired, the angular unit 
can be changed to tenths of a degree per second by modifying
the angular units #define entry in gyro.h

Get_Gyro_Angle()
This function returns the change in heading angle, in 
milliradians, since the software was initialized or 
Reset_Gyro_Angle() was called. If desired, the angular unit
can be changed to tenths of a degree by modifying the angular
units #define entry in gyro.h

Calc_Gyro_Bias()
This function forces a new gyro bias calculation to take place.
For best results, this should be called about a half second 
after the robot powers-up and while the robot is perfectly still 
with all vibration sources turned off (e.g., compressor). It's a
good idea to call this function whenever you know the robot will
be completely still for about twenty-five milliseconds. For
example, call this function just after you've completed a
straight line drive, and just before doing a turn-in-place.

Get_Gyro_Bias()
This function returns the current calculated gyro bias. For
extra precision, this software internally maintains a gyro bias
value that is the sum of GYRO_SAMPLES_PER_UPDATE samples of the 
gyro's analog output. By default, GYRO_SAMPLES_PER_UPDATE is
set to a value of eight in gyro.h

Set_Gyro_Bias()
This function can be called to manually set the gyro bias. For
extra precision, this software internally maintains a gyro bias
value that is the sum of GYRO_SAMPLES_PER_UPDATE samples of the 
gyro's analog output. By default, GYRO_SAMPLES_PER_UPDATE is
set to a value of eight in gyro.h

Reset_Gyro_Angle()
This function resets the integrated gyro heading angle to zero.

Disable_Gyro()
This function should be called when the gyro functionality is
no longer needed, thus reclaiming the time spent servicing the
timer 2 interrupt.

Initialize_ADC()
This function initializes the analog to digital conversion
hardware. This is called by Initialize_Gyro() above and 
shouldn't be called directly.

Initialize_Timer_2()
This function initializes and starts timer2. This is called by
Initialize_Gyro() above and shouldn't be called directly.

Timer_2_Int_Handler()
This function is automatically called when timer 2 causes an
interrupt and shouldn't be called directly.

****************************************************************
Six things must be done before this software will work 
correctly on the FRC-RC:
1) The gyro's rate output is wired to analog input 1.
2) A #include statement for the gyro.h header file must be 
included at the beginning of each source file that calls the 
functions in this source file. The statement should look like 
this: #include "gyro.h".
3) Initialize_Gyro() must be called from user_routines.c/
User_Initialization().
4) The timer 2 interrupt handler, Timer_2_Int_Handler(), must
be installed in User_Routines_Fast.c/InterruptHandlerLow().
See the accompanying copy of User_Routines_Fast.c to see how
this is done.
5) You must select the gyro you're using from a list in gyro.h
and if needed, remove the // in front of it's #define.
6) For optimal performance, you'll need to calibrate the gyro 
scaling factor using the instructions above or those included
in gyro.h.	

Eight things must be done before this software will work 
correctly on the EDU-RC:
1) The gyro's rate output is wired to analog input 1.
2) IO1 must be configured as an "INPUT" in user_routines.c/
User_Initialization().
3) The call to Set_Number_of_Analog_Channels() must configure
at least one analog channel.
4) A #include statement for the gyro.h header file must be 
included at the beginning of each source file that calls the 
functions in this source file. The statement should look like 
this: #include "gyro.h".
5) Initialize_Gyro() must be called from user_routines.c/
User_Initialization().
6) The timer 2 interrupt handler, Timer_2_Int_Handler(), must
be installed in User_Routines_Fast.c/InterruptHandlerLow().
See the accompanying copy of User_Routines_Fast.c to see how
this is done.
7) You must select the gyro you're using from a list in gyro.h
and if needed, remove the // in front of it's #define.
8) For optimal performance, you'll need to calibrate the gyro 
scaling factor using the instructions above or those included
in gyro.h.

-Kevin

[Squall]

Thanks a lot! That's all I can say!

[thoughtful]

Nice! good thing we already ordered our gyro

Once again thankyou for the great code

[Tom Bottiglieri]

How could I use this with the accelerometer?

Does anyone know the sample rate for it?

[Kevin Watson]

Has anyone tried this code yet? I'd like to get a sense of how (if) it's working for teams.

-Kevin

[about_abot]

Originally Posted by Kevin Watson

Has anyone tried this code yet? I'd like to get a sense of how (if) it's working for teams.

-Kevin

Kevin,

First of all, thank you for your undying effort keeping this huge event on track. I just started this year as a programming mentor and do not know the ins and outs of what has happened over the last few years with the code.

The programming student on the team is new, and he is a little anxious with everything. He first saw "C" just a few weeks ago and is feeling a little overwhelmed. I am guiding him through how to layout the functionality he needs to meet the required game rules and translate all of that into working code.

In one of your previous posts, you stated you were working on a new robot.c (and I hope a new robot.h). Do you have any idea when we would see that? Having that will help immensely since my role is to help and not perform the actual work instead of the student doing it himself.

Respectfully,

Gary D.

[Kevin Watson]

Originally Posted by about_abot

In one of your previous posts, you stated you were working on a new robot.c (and I hope a new robot.h). Do you have any idea when we would see that? Having that will help immensely since my role is to help and not perform the actual work instead of the student doing it himself.

As the FIRST folks haven't sent our demobot back for us to test updates on, I'll post the changes (hopefully) sometime this weekend when my team has their 'bot moving around. If you want to make the changes yourself, only two things need to be done to get robot.c to work with the new gyro code:

1) Because I now return angular units expressed in milliradians instead of degrees, line 198 needs to be changed from:

heading = Gyro_Angle()*PI_MRAD/180L;

to:

heading = Get_Gyro_Angle();

2) The programming interface was expanded and many functions were renamed. To fix this, I would just drop the new version of gyro.c/.h into your build directory and let the compiler find the deltas for you (there will only be a few).

-Kevin

[about_abot]

Originally Posted by Kevin Watson

As the FIRST folks haven't sent our demobot back for us to test updates on, I'll post the changes (hopefully) sometime this weekend when my team has their 'bot moving around. If you want to make the changes yourself, only two things need to be done to get robot.c to work with the new gyro code:

1) Because I now return angular units expressed in milliradians instead of degrees, line 198 needs to be changed from:

heading = Gyro_Angle()*PI_MRAD/180L;

to:

heading = Get_Gyro_Angle();

2) The programming interface was expanded and many functions were renamed. To fix this, I would just drop the new version of gyro.c/.h into your build directory and let the compiler find the deltas for you (there will only be a few).

-Kevin

Great! Thank you for your help. I will let you know if we get it to work this weekend.

:-)

Gary

[Kevin Watson]

Originally Posted by about_abot

...I will let you know if we get it to work this weekend.

Cool. I have no idea if people are actually using the code I've published over the last month or so and would very much like feedback on performance, ease of use, etc...

-Kevin

[Tom Bottiglieri]

Kevin, the gyro code has worked fine for us. We are able to track heading, and stick to a heading. We arent using the scripting, for lack of need.

As soon as we get our encoders in, I will post how the PID works out.

[Kevin Watson]

Originally Posted by Tom Bottiglieri

Kevin, the gyro code has worked fine for us. We are able to track heading, and stick to a heading. We arent using the scripting, for lack of need.

As soon as we get our encoders in, I will post how the PID works out.

Very cool. Which gyro are you using? Which method are you using to calculate the gyro bias? Calc_Gyro_Bias() or the manual method I described? Are you calculating the bias once at startup or are you actively tracking it with frequent calls to Calc_Gyro_Bias()? Sorry, 'bout all the questions, but I'd like to get a sense of how much drift people are seeing from a larger sample of gyros. I have some serious magic that I can throw at the problem if people can't use their gyros because of too much drift.

-Kevin

[marccenter]

Is the BEI gyro really illegal to use? I am a rookie engineer this year and see that Kevin Watson has included configuration for it in his navigation code. Our team coach handed me the sensor, from last year's kit, and I have begun wiring it in. IF illegal, why? I saw that the BEI gyro can be purchased on the web. Wouldn't a team just
count it's price in the electronics price of it's parts or is there some rule about past electronics parts. It seems awfully silly to have to not be able to reuse last year's electronic parts.

[marccenter]

Originally Posted by Kevin Watson

Has anyone tried this code yet? I'd like to get a sense of how (if) it's working for teams.

-Kevin

Kevin,
I have tried the ADXRS300EB and the BEI gyrochip AQRS-0075-xxx
(two different units) with some problems.
I have measured a nominal 2.5 volts for both sensors and when
physically swinging both gryo's in opposite directions see the voltage
swing up/down as expected. So, from a gross standpoint, gyro's are
operational.
The software outputs a gyro bias of 4144? When, I swing the
gryo 180 degrees in one direction, slowly, and return to zero I cannot
get the gyro to indicate zero.
Have set #define to ADXRS300EB setting and #define to AQRS0075
and played with scaling factor without success.
Turned out long term Bias calculation - close to short term bias.
After powering up and gyro reading 0 degrees for 4 or 5 samples,
I swing the gryo 180 degrees slowly and return to zero.
Any suggestions, Kevin.

BTW, I really believe the ADXRS300 EB scaling is not 5 mv /degree/sec
but 7.5 mV/degree/sec. Plus/minus 300 degrees over 4.5 volt range,
is 300 degrees over 2.25 volt range (2.5 to 4.75 or 2.5 to 0.25 volts).
or 2250 mV/300 degrees which is 7.5 mV/deg.

Scaling for Plus/Minus 75 degrees/ second is 30 mV/deg/sec
Scaling for Plus/Minus 150 degrees/second is 15 mV/deg/sec
scaling for plus/minus 300 degrees/second is 7.5 mV/deg/sec

[Kevin Watson]

Originally Posted by marccenter

The software outputs a gyro bias of 4144?

To gain a few extra bits of precision, I oversample the analog output by a factor of eight and maintain that precision throughout all calculations. The bias should be around 8*512=4096. Sample rate and number of samples per update are both configurable by changing a few #defines in gyro.h.

Originally Posted by marccenter

When, I swing the gryo 180 degrees in one direction, slowly, and return to zero I cannot get the gyro to indicate zero.

How far from zero is it?

Originally Posted by marccenter

Any suggestions, Kevin.

Yes, just for grins, run the sampling rate up to 1600Hz and change the samples per update to 32 and try again. I've found it helpful to use a pushbutton(s) to force a bias calculation and/or heading angle reset. This code in the 38Hz control loop will do this for you:

static unsigned char old_io3_state = 1;
static unsigned char old_io4_state = 1;

// did user just push the bias calculation button?
if(rc_dig_in03 == 0 && old_io3_state == 1)
{
Start_Gyro_Bias_Calc();
printf("Calibrating...\n");
}

// did the user just release the bias calculation button?
if(rc_dig_in03 == 1 && old_io3_state == 0)
{
Stop_Gyro_Bias_Calc();
Reset_Gyro_Angle();
}

// did user just push the reset button?
if(rc_dig_in04 == 0 && old_io4_state == 1)
{
// reset the gyro angle to zero
Reset_Gyro_Angle();
}

// save the current button states
old_io3_state = rc_dig_in03;
old_io4_state = rc_dig_in04;

Originally Posted by marccenter

BTW, I really believe the ADXRS300 EB scaling is not 5 mv /degree/sec but 7.5 mV/degree/sec. Plus/minus 300 degrees over 4.5 volt range, is 300 degrees over 2.25 volt range (2.5 to 4.75 or 2.5 to 0.25 volts).
or 2250 mV/300 degrees which is 7.5 mV/deg.

Scaling for Plus/Minus 75 degrees/ second is 30 mV/deg/sec
Scaling for Plus/Minus 150 degrees/second is 15 mV/deg/sec
scaling for plus/minus 300 degrees/second is 7.5 mV/deg/sec

Nope. It's 5.0 (+/- 0.4) mV/deg/sec.

-Kevin

[marccenter]

Originally Posted by Kevin Watson

How far from zero is it?

-Kevin

Kevin, downloaded your new 01/30/05 code and everything seems to work right now. Bias was 32382. (512 * 64 samples per update = 32768)
32768(2.5 volts) -32282(Bias)=486. 486/64 = 7.59 a/d counts * 5 mV/count = 38 mV above 2.50. Null bias should equal 2.538 volts. Verfied with DVM. Hooray!
Sampling rate was 1600 Hz, samples per update
rate set to 64, ADXRS300EB utilized, #define TENTHS__OF_A_DEGREE.
#define GYRO_CAL_FACTOR = 1000/1000 (no change).

Simple 180 degree spin test executed. Learn bias at 0 degrees, verify
for 4/5 readings on terminal that angle = 0 degrees. Next, spun gyro 180 degrees - reading about 900 counts (90 degrees * 10 counts/degree).
Return to 0 degree position - read 0 counts (plus/minus 30 - remember,
very coarse test).

Next, tried the GEI GYROCHIP AQRS_00075. Changed the #define GYROCHIP_75 at top of gyro.h. Same spin test gave similiar results. Spun
360 degrees and back - similiar results. Sampling rate 1600 Hz, samples per update rate equal 64, #define TENTHS_OF_A_DEGREE, #define GYRO_CAL_FACTOR = 1000/1000 (no change).

Originally Posted by Kevin Watson

Yes, just for grins, run the sampling rate up to 1600Hz and change the samples per update to 32 and try again.

-Kevin

Kevin,
Did you work some new magic in the new 01/30/05 software release, or do
you think the 1600 Hz, samples = 64 is what made this work? I did not
have time last night to experiment with various settings. I tried the
01/22/05 software and found my head bloody from beating it against the
wall to make it work. BTW, thanks for responding to previous e-mail and
publishing 01/30/05 release.