Gyroscope Code

[Kevin Watson]

For those who might be interested, I've just posted updated gyroscope interface code for IFI's robot controllers. The code has been significantly reworked to use the new ADC interface code (http://www.chiefdelphi.com/forums/showthread.php?t=39536), which allows the programmer to use other analog sensors alongside a gyroscope without affecting the performance or operation of the gyroscope. In addition, significant performance improvements can be realized using the new oversampling and deadband features. The code can be found here: http://kevin.org/frc. As always, if you find a bug in the code or a problem with the documentation, please let me know.

-Kevin

Here's the readme file:


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'
ADXRS401EB, ADXRS150EB and ADXRS300EB gyros. Data sheets for
these devices are included. By default, this software is
configured to work with a ADXRS150EB gyro, sampling at 800Hz,
downconverting to an update rate of 50Hz by averaging sixteen
samples per update. These parameters can be changed by editing
gyro.h and/or adc.h.

Version 0.5 of the gyro interface software has been altered
to use the ADC interface provided by adc.c/.h. This change
allows the programmer to use additional ADC channels for other
analog sensors without affecting the performance or operation
of your gyro. See the documentation included with adc.c/.h for
information on how to use this new functionality.
Another new feature is the ability to specify a measurement
deadband centered about the gyro bias. ADC measurements within
this deadband will not be used to calculate the gyro angle or
angular rate. This feature can have a significant impact at
minimizing short-term drift just after a bias calculation has
taken place. To use this feature, follow the instructions
embedded within gyro.h.

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 one of
the robot controller's analog inputs. These gyro evaluation
boards can be purchased from analog devices (www.analog.com (http://www.analog.com/)),
and Digi-Key (www.digikey.com (http://www.digikey.com/)). Another great source for
Analog Devices gyros is SparkFun Electronics (www.sparkfun.com (http://www.sparkfun.com/)).

For optimum performance, you'll need to calibrate the scaling
factor to match that of your gyro. 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 adversely
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 motionless, 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.

The included project files were built with MPLAB version 7.20.
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.

************************************************** **************
Eight things must be done before this software will work
correctly on your robot controller:

1) The gyro's rate output is wired to one of the analog inputs
of your robot controller and gyro.h/#define GYRO_CHANNEL is
updated with the analog channel your gyro is attached to.

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 gyro.c. The statement should look like this:
#include "gyro.h".

3) Initialize_Gyro() must be called from user_routines.c/
User_Initialization().

4) You must select the gyro you're using from a list in gyro.h
and if needed, remove the // in front of its #define.

5) The default angular unit is milliradians. If desired, this
can be changed to tenths of a degree by editing gyro.h

6) A gyro bias calculation must take place using the functions
Start_Gyro_Bias_Calc() & Stop_Gyro_Bias_Calc() described below.
This must be done several hundred milliseconds after the gyro
powers-up and is allowed to stabilize.

7) For optimal performance, you'll need to calibrate the gyro
scaling factor using the instructions above or those included
in gyro.h.

8) Follow the instructions found in adc_readme.txt for
installation instructions.

************************************************** **************
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

Start_Gyro_Bias_Calc()
Stop_Gyro_Bias_Calc()
These functions start/stop a new gyro bias calculation. For best
results, Start_Gyro_Bias_Calc() should be called about a second
after the robot powers-up and while the robot is perfectly still
with all vibration sources turned off (e.g., compressor). After
at least a second (longer is better), call Stop_Gyro_Bias_Calc()
to terminate the calibration. While a calibration is taking
place, gyro rate and angle are not updated. Once calibrated, a
call to Reset_Gyro_Angle() should take 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.

Process_Gyro_Data()
This function should be called when the ADC software reports
that new gyro data is available. Ideally this should be done
within the user_routines_fast.c/Process_Data_From_Local_IO()
function. See the enclosed copy of user_routines_fast.c for
an example of how to do this.

[Kevin Watson]

...As always, if you find a bug in the code or a problem with the documentation, please let me know.

Well, I just discovered that I left one step out of the installation instructions found in gyro_readme.txt. Here are the revised instructions:

Nine things must be done before this software will work
correctly on your robot controller:

1) The gyro's rate output is wired to one of the analog inputs
of your robot controller and gyro.h/#define GYRO_CHANNEL is
updated with the analog channel your gyro is attached to.

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 gyro.c. The statement should look like this:
#include "gyro.h".

3) Initialize_Gyro() must be called from user_routines.c/
User_Initialization().

4) Process_Gyro_Data() must be called when the ADC software
generates an update. An example of how to do this can be found
in user_routines_fast.c/Process_Data_From_Local_IO().

5) You must select the gyro you're using from a list in gyro.h
and if needed, remove the // in front of its #define.

6) The default angular unit is milliradians. If desired, this
can be changed to tenths of a degree by editing gyro.h

7) A gyro bias calculation must take place using the functions
Start_Gyro_Bias_Calc() & Stop_Gyro_Bias_Calc() described below.
This must be done several hundred milliseconds after the gyro
powers-up and is allowed to stabilize.

8) For optimal performance, you'll need to calibrate the gyro
scaling factor using the instructions above or those included
in gyro.h.

9) Follow the instructions found in adc_readme.txt for
installation instructions.

-Kevin

[Gdeaver]

Thanks for the contribution to the First effort. I have a What if question.

What if First includes the EasyC environment for the 2006 season? Could your code be wrapped up and included into it? How about your serial port code?

Just curios as to you opinion if First goes down this path.

[Kevin Watson]

What if First includes the EasyC environment for the 2006 season? Could your code be wrapped up and included into it? How about your serial port code?Yes, the code can be ported fairly easily.

-Kevin

[gmiller_1249]

Is it possible to use a Murata gyro with the innovation controllers. If so, what changes would have to be made to the code so the controller can read the output from the gyro?

[Kevin Watson]

Is it possible to use a Murata gyro with the innovation controllers. If so, what changes would have to be made to the code so the controller can read the output from the gyro?Yes, they can be used. The code changes are very simple to make. If you provide me with a part number, I can add it to the list of supported gyros.

-Kevin

[gmiller_1249]

Yes, they can be used. The code changes are very simple to make. If you provide me with a part number, I can add it to the list of supported gyros.

-Kevin

Murata ENV-05DB

[Kevin Watson]

Murata ENV-05DBI've updated the gyro code to support this device. As I don't have one to test with, please let me know if you have any problems. The code can be found here: http://kevin.org/frc

-Kevin

[Kevin Watson]

I've updated the gyro code with version 0.3 of the analog to digital conversion code (http://www.chiefdelphi.com/forums/showpost.php?p=422923&postcount=3). As always, the code can be found here: http://kevin.org/frc

-Kevin

[Kevin Watson]

Well, I finally posted (http://kevin.org/frc) the ADC and Gyro code for the 2006 robot controller. Sorry about the delay, but I ran into a nasty bug that caused me to spend a few days in the fourth and fifth levels of programmer hell. The bug was in the design of the PIC18F8722 (yes, a bug in the sillicon) and I had to figure out what was going on. Anyway, the gyro code works really, really well with the gyro in the kit of parts. The accelerometers also work really well, but I need to do more testing before I unleash that code.

-Kevin

[Jon236]

Well, I finally posted (http://kevin.org/frc) the ADC and Gyro code for the 2006 robot controller. Sorry about the delay, but I ran into a nasty bug that caused me to spend a few days in the fourth and fifth levels of programmer hell. The bug was in the design of the PIC18F8722 (yes, a bug in the sillicon) and I had to figure out what was going on. Anyway, the gyro code works really, really well with the gyro in the kit of parts. The accelerometers also work really well, but I need to do more testing before I unleash that code.

-Kevin
Thanks for all the hard work! With all the great sensors this year, how many interrupts can we use? Last year we use 4 pair of Hall sensors as quad counters, which worked well. The team is reluctant to move past success....so if I use those 4 pair, is there anything left on the 18F8722? And will using the camera code claim those interrupts as well?

Thans

Jon Mittelman
mentor
Team236

[Chriszuma]

Kevin, you're like my own personal Jesus.

[Dave75]

Well, I finally posted the ADC and Gyro code for the 2006 robot controller. ...
I just noticed that the gyro.h has DEFINEs to set the code to operate on a variety of gyro modules, but not the one that's in the 2006 KOP. According to US First's "2006_Sensor_Basic_Operation.pdf",
The actual Yaw Rate Gyro chip used on the 2006 YRG is the AD22304.
As far as I can tell, the only significant difference is the 80 degree/sec turning rate limit (vs. 150 dps on the ADXRS150).

Is there anything that needs to be adjusted in the code to handle this particular chip, or is it good enough just to leave the DEFINE the way it came?

Thanks again,
Dave

[scitobor 617]

I've been having trouble getting the KOP gyro to work. I attached the two PWM cables to it and installed the gryo code. The output I get from the program seems to be in no way related to what is happening to the gyro. :confused: Also, why are two PWM cables used on the gyro, is one a temparture sensor(I read that it maybe somewhere)? I'm really a bit confused and any help would be greatly appreciated.

[JBotAlan]

is one a temparture sensor(I read that it maybe somewhere)?

I heard the head programmer from 68 talk about this...yes, it appears to have a temperature sensor onboard. He told me it measures in degrees Kelvin, but I don't know any of the details because he is the one that tinkered with the code and got it working. The best guess we have is that the gyro readings differ based on the temp........??? Kinda strange...umm.... :confused:

JBotAlan

[Chriszuma]

I heard the head programmer from 68 talk about this...yes, it appears to have a temperature sensor onboard. He told me it measures in degrees Kelvin, but I don't know any of the details because he is the one that tinkered with the code and got it working. The best guess we have is that the gyro readings differ based on the temp........??? Kinda strange...umm.... :confused:

JBotAlan
So, speaking as the only person here who has read the spec sheet, this second pwm output gives absolute temperature. There are nice little graphs relating the reading to the temperature of the chip. In that regard it could be useful. I, however, have not tinkered with that particular output.
EDIT: I set up a calibration algorithm that gives an average of the first ten values it pulls from the gyro, and sets that as neutral (after 20 program loops to give it a chance to spinup) So I feel okay ignoring the temperature output.

[naor52]

i download the Gyro code and i think the result are not correct because when i move the gyro 90 degrees and back to 0 degrees then i not get the same values

what is the problem ??

-Naor

[Joel J]

I have a few questions again..

In adc.c, I notice that you divide the accumulated readings, to once again have a value that's between 0 and 1024:for(i=0; i < num_adc_channels; i++)
{
adc_result[i] = (long)(accum[i] >> adc_result_divisor);
} But in gyro.c I notice you are factoring in ADC_RANGE in your calculations: // Return the calculated gyro angle to the caller.
return(((gyro_angle * GYRO_SENSITIVITY * 5L) / (ADC_RANGE * ADC_UPDATE_RATE * 2L)) * GYRO_CAL_FACTOR); I guess I'm just a bit confused as to what is happening here. Is there a bit of math that I didn't think about?

I have it set right now to sample four channels at 3200Hz, and to collect 16 samples before an update. This sets ADC_RANGE to 4096 and ADC_UPDATE_RATE to 3200/(16 * 4), which is 50Hz. So the ADC_RANGE seems out of place. Is the down shift of the accumulator in adc.c not supposed to be there, or is it doing something other than what I think it is?

[Kevin Watson]

I guess I'm just a bit confused as to what is happening here. Is there a bit of math that I didn't think about?Ugh, I'll have to work through it. The gyro expression looks convoluted because it's hand-tuned to minimize integer division rounding error while not overflowing the numerator. While I'm looking at it, you might consider working through it yourself using dimensional analysis. Keep in mind that gyro_angle is not in angular units <grin>.

-Kevin

[Joel J]

nvm. I'll just wait for your response.

[Kevin Watson]

nvm. I'll just wait for your response.See the attached document for a dimensional analysis of the expression in Get_Gyro_Angle().

-Kevin

[kaszeta]

Ugh, I'll have to work through it. The gyro expression looks convoluted because it's hand-tuned to minimize integer division rounding error while not overflowing the numerator. While I'm looking at it, you might consider working through it yourself using dimensional analysis. Keep in mind that gyro_angle is not in angular units <grin>.


We had a similar issue today messing around with both the DAA and another <classified> sensor that give a 0-5 volt signal. We're already using the gyro, so we can't use Get_Analog_Value anymore, so instead we've move the code to use Get_ADC_Result(), and I was surprised to get 11-bit outputs (0-2047) on that instead of 10-bit values I was expecting.

[Kevin Watson]

We had a similar issue today messing around with both the DAA and another <classified> sensor that give a 0-5 volt signal. We're already using the gyro, so we can't use Get_Analog_Value anymore, so instead we've move the code to use Get_ADC_Result(), and I was surprised to get 11-bit outputs (0-2047) on that instead of 10-bit values I was expecting.Yes, this is documented in adc.h and probably adc_readme.txt. The code can oversample and decimate to give you higher resolution data.

-Kevin

[kaszeta]

Yes, this is documented in adc.h and probably adc_readme.txt. The code can oversample and decimate to give you higher resolution data.

It's documented in both. We just missed it the first time around since we thought we had adjusted the oversampling rate down to 2 (we hadn't, editted the header in another project directory), so we thought we had an extra bit of precision.

Thanks for all the help. The team's programming team has been very "feature aggressive" this year, so there is a lot of code I'm helping them test and keep track off, and we'd be in a much bigger mess without your help.

[Joel J]

Thanks for that breakdown! Now I understand.

You actually DO get more resolution from oversampling!

So, at 16 samples per update, I have a 12-bit ADC value, that is completely valid, and does not have to be divided any further. I looked at the code again, and you take 16 samples, then divide by 4, leaving 4*[0, 1023]. This is awesome. 12-bits is exactly the resolution that's required to be as accurate as I need to be in my application.

Previously I had thought that oversampling gave more "resolution" due to the fact that when the samples were completely averaged together, they would just eliminate more noise.

Now I think I understand better.

Thanks again.

[Chris Hibner]

You actually DO get more resolution from oversampling!

I wrote a quick whitepaper (http://www.chiefdelphi.com/forums/papers.php?s=&action=single&paperid=234) on this a few years ago. The paper gives a basic reasoning as to why it works along with a few plots to show it in action.

[Kevin Watson]

You actually DO get more resolution from oversampling!

So, at 16 samples per update, I have a 12-bit ADC value, that is completely valid, and does not have to be divided any further. I looked at the code again, and you take 16 samples, then divide by 4, leaving 4*[0, 1023]. This is awesome. 12-bits is exactly the resolution that's required to be as accurate as I need to be in my application.Yes, it is completely valid and doesn't need to be divided down further. If you look in adc.h you'll find code like this:

#ifdef ADC_SAMPLES_PER_UPDATE_16
#define ADC_SAMPLES_PER_UPDATE 16
#define ADC_RANGE 4096L
#define ADC_RESULT_DIVISOR 4-2 // 12-bit effective resolution
#endif


The line "#define ADC_RESULT_DIVISOR 4-2" is where I calculate the number of bits I need to shift right to perform the equivalent division. the 4 is the number of bits I need to shift right to get back to the 10 bit native resolution of the ADC. I subtract 2 from the 4 because I've gained 2 bits worth of resolution, for an effective 12-bits.

-Kevin

[Kevin Watson]

I wrote a quick whitepaper (http://www.chiefdelphi.com/forums/papers.php?s=&action=single&paperid=234) on this a few years ago. The paper gives a basic reasoning as to why it works along with a few plots to show it in action.Hey, this is a great paper. Any chance of expanding it to include a bit more on oversampling and decimation? If you're cool with it, I'll include a link to it in my own documentation.

-Kevin

[Makubesu]

I have a question about the code, how much would the accuracy suffer if I modified how long the gyroscope spends calculating the bias? Is there a graph somewhere which would explain this?

[jaustin]

Hi Kevin,
I've got the full sized RC running now and I'm trying to merge last year's pid.c/h and robot.c/h with this year's gyro.c/h, encoder.c/h and adc.c/h code. I've got the encoders working fine and adc appears to be working fine (I've hooked up a pot to analog port 2 and called Get_ADC_Result(2) and got good data). I've even called Get_ADC_Result(1) with the gyro hooked up to input 1 and got readings that varied as I moved the gyro around. But I am not getting anything out of gyro.c. The routine I created to start and stop the gyro bias calculation worked fine on the EDU controller with last year's adc and gyro codes. On this year's full size controller all I ever get is a bias of -1 and a heading of 0.

When I run the gyro only code it works fine. It does not like robot.c for some reason. Any ideas?

I should add that I actually started with the 2006 frc_gyro project and added the other files to it so I would not miss anything. I've also disabled the serial ports options related to rx1, tx2, & rx2.

Thanks again for all your efforts!

[Kevin Watson]

Hi Kevin,
I've got the full sized RC running now and I'm trying to merge last year's pid.c/h and robot.c/h with this year's gyro.c/h, encoder.c/h and adc.c/h code. I've got the encoders working fine and adc appears to be working fine (I've hooked up a pot to analog port 2 and called Get_ADC_Result(2) and got good data). I've even called Get_ADC_Result(1) with the gyro hooked up to input 1 and got readings that varied as I moved the gyro around. But I am not getting anything out of gyro.c. The routine I created to start and stop the gyro bias calculation worked fine on the EDU controller with last year's adc and gyro codes. On this year's full size controller all I ever get is a bias of -1 and a heading of 0.

When I run the gyro only code it works fine. It does not like robot.c for some reason. Any ideas?

I should add that I actually started with the 2006 frc_gyro project and added the other files to it so I would not miss anything. I've also disabled the serial ports options related to rx1, tx2, & rx2.

Thanks again for all your efforts!Jeff, can you zip-up your project and send it to me? If I get some time tomorrow, I'll have a look.

-Kevin

[jaustin]

OK, will do! Thanks Kevin!

[Kevin Watson]

Hi Kevin,
I've got the full sized RC running now and I'm trying to merge last year's pid.c/h and robot.c/h with this year's gyro.c/h, encoder.c/h and adc.c/h code. I've got the encoders working fine and adc appears to be working fine (I've hooked up a pot to analog port 2 and called Get_ADC_Result(2) and got good data). I've even called Get_ADC_Result(1) with the gyro hooked up to input 1 and got readings that varied as I moved the gyro around. But I am not getting anything out of gyro.c. The routine I created to start and stop the gyro bias calculation worked fine on the EDU controller with last year's adc and gyro codes. On this year's full size controller all I ever get is a bias of -1 and a heading of 0.

When I run the gyro only code it works fine. It does not like robot.c for some reason. Any ideas?

I should add that I actually started with the 2006 frc_gyro project and added the other files to it so I would not miss anything. I've also disabled the serial ports options related to rx1, tx2, & rx2.

Thanks again for all your efforts!Just to make sure, did you hook-up the gyro correctly? The gyro from the KOP wasn't labeled correctly. Have a look at this document (http://www.ifirobotics.com/docs/2006-sensors-yaw-rate-gyro-dual-axis-accelerometer-gear-tooth-sensor-b.pdf).

-Kevin

[jaustin]

Just to make sure, did you hook-up the gyro correctly? The gyro from the KOP wasn't labeled correctly. Have a look at this document (http://www.ifirobotics.com/docs/2006-sensors-yaw-rate-gyro-dual-axis-accelerometer-gear-tooth-sensor-b.pdf).

-Kevin

Yes, I'm using the ADXRS150 that I was using earlier on the EDU bot. It worked fine. I'm plugging it into analog input 1, black to black, etc. I ran it with the gyro only code and it worked great.

[jaustin]

Kevin,
No worries, we (actually Brian aka Devicenull) figured it out in another thread. The call to update the gyro is performed in Process_Data_From_Local_IO, which is not called during autonomous mode. I never knew that since I've been working with the EDU all this time and it does not seem to run that way. We had to move the update gyro function to the User_Autonomous_Code loop, and everything now works. Again, sorry to bug you with dumb problems!

[Kevin Watson]

Kevin,
No worries, we (actually Brian aka Devicenull) figured it out in another thread. The call to update the gyro is performed in Process_Data_From_Local_IO, which is not called during autonomous mode. I never knew that since I've been working with the EDU all this time and it does not seem to run that way. We had to move the update gyro function to the User_Autonomous_Code loop, and everything now works. Again, sorry to bug you with dumb problems!Yeah, I found that about a half hour ago. I wonder if this is causing problems for other teams? Another very minor thing I noticed is the lack of braces with the if(i == 10) statement in robot.c/cmd_gyro_bias( ). As far as I can tell, it's not really a bug, but you might have a look.

-Kevin

[royalfire]

I have a question about the code, how much would the accuracy suffer if I modified how long the gyroscope spends calculating the bias? Is there a graph somewhere which would explain this?
Good question!

[Steve Jacobson]

In an effort to learn how to implement an integrator in a PID controller I have studied integration with Kevin's Gyro code and the Gyro example in Eugene Brooks' paper "An Introduction to C Programming for FIRST Robotics Applications". I would expect rate gyro integration to look something like the following.

Angle = Angle_prev_frame [mrad] + angular_rate_gyro [mrad/sec] * dt[sec]

Where dt is the time between frames (in our case 26.2 msec).

Kevin's Gyro code has the following example.

// update the gyro rate
gyro_rate = temp_gyro_rate;

// integrate the gyro rate to derive the heading
gyro_angle += (long)temp_gyro_rate;

Eugene's code has the following example
#define GYROAVE 511
long newgyrovalue = 0;
long oldgyrovalue = 0;
long gyrointegral = 0;
newgyrovalue = Get_Analog_Value(rc_ana_in10);
gyrointegral += ((oldgyrovalue + newgyrovalue) / 2) - GYROAVE;
oldgyrovalue = newgyrovalue;

I understand the notation += means previous frame + new information; however, I don't see how they are taking the time between frames into account (dt). It seems as though in both cases they are simply adding the rate gyro output to the previous angle. It is obvious to me that due to my lack of experience with C, I am missing something. Can anyone explain how the time between frames is taken into account? I want to know how to apply this trick for the integrator in my PID controller.

[Joe Ross]

I would expect rate gyro integration to look something like the following.

Angle = Angle_prev_frame [mrad] + angular_rate_gyro [mrad/sec] * dt[sec]

Where dt is the time between frames (in our case 26.2 msec).

Kevin's Gyro code has the following example.

// update the gyro rate
gyro_rate = temp_gyro_rate;

// integrate the gyro rate to derive the heading
gyro_angle += (long)temp_gyro_rate;

Eugene's code has the following example
#define GYROAVE 511
long newgyrovalue = 0;
long oldgyrovalue = 0;
long gyrointegral = 0;
newgyrovalue = Get_Analog_Value(rc_ana_in10);
gyrointegral += ((oldgyrovalue + newgyrovalue) / 2) - GYROAVE;
oldgyrovalue = newgyrovalue;

I understand the notation += means previous frame + new information; however, I don't see how they are taking the time between frames into account (dt). It seems as though in both cases they are simply adding the rate gyro output to the previous angle. It is obvious to me that due to my lack of experience with C, I am missing something. Can anyone explain how the time between frames is taken into account? I want to know how to apply this trick for the integrator in my PID controller.

the gyro_angle variable uses dt of 1 because it's much faster to add then to add and multiply or divide. In Kevin's code, the real time is take account of in the Get_Gyro_Angle function by dividing by ADC_UPDATE_RATE.

[Kevin Watson]

In an effort to learn how to implement an integrator in a PID controller I have studied integration with Kevin's Gyro code and the Gyro example in Eugene Brooks' paper "An Introduction to C Programming for FIRST Robotics Applications". I would expect rate gyro integration to look something like the following.

Angle = Angle_prev_frame [mrad] + angular_rate_gyro [mrad/sec] * dt[sec]

Where dt is the time between frames (in our case 26.2 msec).

Kevin's Gyro code has the following example.

// update the gyro rate
gyro_rate = temp_gyro_rate;

// integrate the gyro rate to derive the heading
gyro_angle += (long)temp_gyro_rate;

Eugene's code has the following example
#define GYROAVE 511
long newgyrovalue = 0;
long oldgyrovalue = 0;
long gyrointegral = 0;
newgyrovalue = Get_Analog_Value(rc_ana_in10);
gyrointegral += ((oldgyrovalue + newgyrovalue) / 2) - GYROAVE;
oldgyrovalue = newgyrovalue;

I understand the notation += means previous frame + new information; however, I don't see how they are taking the time between frames into account (dt). It seems as though in both cases they are simply adding the rate gyro output to the previous angle. It is obvious to me that due to my lack of experience with C, I am missing something. Can anyone explain how the time between frames is taken into account? I want to know how to apply this trick for the integrator in my PID controller.As Joe points out above, I don't actually calculate the angle until the user asks for it when they call Get_Gyro_Angle( ). To calculate the angle, you've got to calculate the area under the curve, which folks usually visualize as a bunch of rectangles dt wide and rate high neatly stacked along the time (x) axis. To do this I'd have to have an expression like:

angle += angle + (rate * dt) [expression 1]

in the code, which might be okay on a 3.2 GHz Pentium 4, which is not only fast by virtue of it's clock rate, but also has hardware that can do the addition and multiplication in one clock period. I can't be so cavalier with our humble PIC18F8722, which can't hope to keep up with the Pentium 4. One of the optimizations I've done is to minimize the number of multiplications that need to be performed to track the angle. Let's take the expression above and rearrange it to show how this works:

angle += angle + (rate_n * dt)
angle = (rate_0 * dt) + (rate_1 * dt) + (rate_2 * dt) + ... + (rate_n * dt)
angle = dt * (rate_0 + rate_1 + rate_2 + ... + rate_n) [expression 2]

The way to visualize that last expression is to take each of those little rate times dt rectangles and stack them on top of each other along the rate (y) axis. So now you've got this rectangle with an area of dt * the sum of the rate samples. It's important to know that the area calculated with expression 1 is exactly the same as that calculated with expression 2.

So, just how is this done in the code? Well, take expression 2 and separate it into two calculations:

rate_sum = rate_0 + rate_1 + rate_2 + ... + rate_n [expression 3]

angle = rate_sum * dt [expression 4]

Expression 3 is done in Process_Gyro_Data( ) using this code:

gyro_angle += (long)temp_gyro_rate;

Expression 4 is done in Get_Gyro_Angle( ) and is buried in this calculation:

gyro_angle * GYRO_SENSITIVITY * 5L) / (ADC_RANGE * ADC_UPDATE_RATE)) * GYRO_CAL_FACTOR

I've done a dimensional analysis that shows that this convoluted expression does spit-out something in an angular unit. The document can be found here:

http://www.chiefdelphi.com/forums/showpost.php?p=442432&postcount=21

Hopefully this gives you some insight on how the code works.

-Kevin

[Joel J]

You can modify Kevin's code to do a trapezoidal approximation, to get a better approximation.

On line 55, replace:

int gyro_rate;

with:

int gyro_rate;
int gyro_rate_prev;

Use this as your Gyro_Angle:
long Get_Gyro_Angle(void)
{
// Return the calculated gyro angle to the caller.
return(((gyro_angle * GYRO_SENSITIVITY * 5L) / (ADC_RANGE * ADC_UPDATE_RATE * 2L)) * GYRO_CAL_FACTOR);
}And this as your Process_Gyro_Data: void Process_Gyro_Data(void)
{
int temp_gyro_rate;

// should the completed sample set be used to calculate the gyro bias?
if(calc_gyro_bias == 1)
{
// convert the accumulator to an integer and update gyro_bias
avg_accum += Get_ADC_Result(GYRO_CHANNEL);
avg_samples++;
}
else
{
// get the latest measured gyro rate
temp_gyro_rate = (int)Get_ADC_Result(GYRO_CHANNEL) - gyro_bias;

// update reported gyro rate and angle only if
// measured gyro rate lies outside the deadband
if(temp_gyro_rate < -GYRO_DEADBAND || temp_gyro_rate > GYRO_DEADBAND)
{
// update the gyro rate
gyro_rate = temp_gyro_rate;
}
else
{
gyro_rate = 0;
}

// integrate the gyro rate to derive the heading
gyro_angle += (long)temp_gyro_rate + (long)gyro_rate_prev;

// update the previous gyro rate
gyro_rate_prev = gyro_rate;
}
}
You may have to redo the calibration step (to get another scale value), but I think this is a good mod to make.

[Kevin Watson]

You can modify Kevin's code to do a trapezoidal approximation, to get a better approximation.When you say "better approximation" have you done the experiment to quantify how better it is? If so, is it significant enough to justify the added processing overhead? Does higher sampling frequency relative to the fixed bandwidth of the gyro make doing a trapezoidal approximation more or less attractive?

-Kevin

[Joel J]

I took a look at the assembly output and the modification ended up adding about 8 more instructions to the interrupt, which seemed acceptable to me. Maybe I'm wrong.

As for the actual difference between the rectangular integral and the trapezoidal one: I didn't do any serious testing, but I remember seeing a difference last season when I switched to the trapezoidal approximation. When I returned to zero (after doing a 180 degree turn), it got really darn close. When I opened, in MATLAB, the printf data I logged the graph for the gyro angle was nice and smooth. Not really a solid reason to switch over, I guess.

[Kevin Watson]

I took a look at the assembly output and the modification ended up adding about 8 more instructions to the interrupt, which seemed acceptable to me. Maybe I'm wrong.

As for the actual difference between the rectangular integral and the trapezoidal one: I didn't do any serious testing, but I remember seeing a difference last season when I switched to the trapezoidal approximation. When I returned to zero (after doing a 180 degree turn), it got really darn close. When I opened, in MATLAB, the printf data I logged the graph for the gyro angle was nice and smooth. Not really a solid reason to switch over, I guess.A fun experiment would be to calculate both simultaneously and compare the results using different sample rates and gyro output frequencies.

-Kevin

[Steve Jacobson]

One of the optimizations I've done is to minimize the number of multiplications that need to be performed to track the angle. Let's take the expression above and rearrange it to show how this works:

angle += angle + (rate_n * dt)
angle = (rate_0 * dt) + (rate_1 * dt) + (rate_2 * dt) + ... + (rate_n * dt)
angle = dt * (rate_0 + rate_1 + rate_2 + ... + rate_n) [expression 2]

Hopefully this gives you some insight on how the code works.

-Kevin

Ahhhh. it is all clear to me now. Thanks for the help everyone. Actually the optimized integration is probably better than multiplying by dt every frame because you don't get any truncation error associated with multiplying by 26.2 milliseconds.

By the way, I had trouble casting long variables into integers. My guess is that I probably exceeded the maximum integer value along the way somewhere. Is there anything special that I need to be aware of when casting a long into and int?

[Alan T]

I found a minor bug in the gyro code.

The problem I was having is the Get_Gyro_Rate was all over the place. It would bounce between + and - during a turn.

I added the long qualifiers to the GYRO_CAL_FACTOR and the problem went away.

#define GYRO_CAL_FACTOR 1000L/1000L

It must have been using an integer in there somewhere and it was overflowing. The problem didn't seem to affect the Get_Gyro_Angle routine.

alan

[Chaychay]

First of all, I would like to thank Mr. Watson for all the work he has put into FRC programming...you da man :] !

I have an ADXRS150EB Gyro. I loaded the standard EDU-gyro code from kevin.org onto an Edu controller and a bias of 1922 was calculated. The gyro is working perfectly, and all the angles I tested seemed reasonable.

Now I move the gyro to a 2005 RC, and ran the 2006 Gyro code (with library and linker files changed to 2005). The bias was calculated to be 966 :confused:. The part if find even more odd, is that at the bias of 966 the gyro still appeared to work, except that if i rotated 90 deg, it thought it rotated ~70 deg. When the gyro wasnt moving, the rate was 0. I've looked through the EDU vs. FRC code, and I can't see anything that would cause the biases to be different.

Notes:

1. No backup battery on FRC (main voltage was ~12.6)
2. Gyro is on a breadboard
3. No modifications made to code from kevin.org except change from milliradians to 1/10 deg
4. I tried the 2005 Gyro code and it worked perfectly. So its not my RC, there is something between the 2006 and 2005 Gyro code that I'm not seeing :o
5. I would be using a 2006 RC, but its being repaired by IFI at the moment...this code is intended for the 2006 RC, which is why I'm not using the 2005 Gyro code)

Any help in getting the gyro working with the 2006 Gyro code on a 2005 RC would be greatly appreciated. Thanks a ton!

[DanDon]

Did you check to see which gyro is defined in gyro.h?

[Chaychay]

Yup, I made sure it was the ADXRS150 that was defined. (Sorry I forgot to mention that :p )

[Chaychay]

(Please see my previous post; otherwise this one might not make much sense)

Ok...another weird thing is happening.

I wanted to continue coding, so I loaded the 2005 Gyro code (the code that works perfectly...no odd stuff happening).

By default, Mr. Waston has all the printf's for gyro data in user_routines. So I turn the robot ~90 degrees, and the gyro reports it turned ~1570 milliradians. Wicked......now:

I moved the gyro code that was in user_routines into the User_Autonomous_Code. (so the bias and all the output is now coming from autonomous mode). I also moved the Process_Gyro_Data as instructed. Now I turn the robot 90 degrees, and the gyro reports that it turned 1200 milliradians.

Does anyone have any idea as to why the output I got (from the same code) is different just because I moved it to a different location?

Again, any help would be much appreciated....I really am stumped.

Notes:

In a final attempt to prove that I wasn’t delusional :o ......I put the gyro data printf's in both autonomous and regular mode. (the bias was only being calculated once in regular mode). Now, I turned the robot ~90 degrees in regular mode, then turned autonomous on. I returned the robot to its orginal position and instead of reported ~0 heading, I got a heading of ~400 milliradians. :confused:

[jhallel]

Excuse me, Kevin, but during the Build of the Gyroscope code, an error is found stating GYRO_CAL_FACTOR is defined several times. However, after checking through the code, GYRO_CAL_FACTOR is defined once: double GYRO_CAL_FACTOR = 1000/1000;
What is the error?

[Kevin Watson]

Excuse me, Kevin, but during the Build of the Gyroscope code, an error is found stating GYRO_CAL_FACTOR is defined several times. However, after checking through the code, GYRO_CAL_FACTOR is defined once: double GYRO_CAL_FACTOR = 1000/1000;
What is the error?This isn't nearly enough information. We can help you out if you post code and/or a description of the changes you've made and the contents of the MPLAB build window containing the error text.

-Kevin

[Gary Bonner]

We hooked up our gyro today and are having some problems with it. We were running Kevin’s unmodified code and tried calibrating the scaling factor. After initializing, it returns 0 turn rate and 0 angle. If we rotate the gyro 180 degrees counter clockwise, it returns about -3100 milliradians. If we rotate it 180 degrees clockwise (after initializing), it reports about 2700 milliradians. If we rotate it 180 degrees back and forth, the angle value decreases an additional approx. 400 milliradians each cycle.

We also tried directly printing the gyro value from a Get_Analog_Value() statement while rotating the gyro by hand. At rest, we got a value of 505, turned rapidly in one direction, it would go to zero, but in the other direction, the highest it would go was about 780.

Are we missing something? Might we have a defective gyro?

Thanks.

[6600gt]

We hooked up our gyro today and are having some problems with it. We were running Kevin’s unmodified code and tried calibrating the scaling factor. After initializing, it returns 0 turn rate and 0 angle. If we rotate the gyro 180 degrees counter clockwise, it returns about -3100 milliradians. If we rotate it 180 degrees clockwise (after initializing), it reports about 2700 milliradians. If we rotate it 180 degrees back and forth, the angle value decreases an additional approx. 400 milliradians each cycle.

We also tried directly printing the gyro value from a Get_Analog_Value() statement while rotating the gyro by hand. At rest, we got a value of 505, turned rapidly in one direction, it would go to zero, but in the other direction, the highest it would go was about 780.

Are we missing something? Might we have a defective gyro?

Thanks.

You might be exceeding the 150 degrees/sec turn rate of the gyro...

[kaszeta]

Are we missing something? Might we have a defective gyro?

Thanks.

Several things to check:

1. Is the gyro level?
2. Are you letting calculate the bias correctly (letting it sit a bit before running it)?
3. Are you turning it too quickly?

[Gary Bonner]

Several things to check:

1. Is the gyro level?
The gyro was always level when we did the scaling factor calibration. When we were just printing it out and turning it by hand, we held it as level as we could.


2. Are you letting calculate the bias correctly (letting it sit a bit before running it)?
Yes, we never moved it until the bias was calculated


3. Are you turning it too quickly?
If we turned it extremely slowly (about 5 sec to turn 180 degrees), it seemed to work ok. Anything faster than that and it had errors and they seemed consistant, regardless of speed, and only in a clockwise direction.

[Kevin Watson]

The gyro was always level when we did the scaling factor calibration. When we were just printing it out and turning it by hand, we held it as level as we could.


Yes, we never moved it until the bias was calculated


If we turned it extremely slowly (about 5 sec to turn 180 degrees), it seemed to work ok. Anything faster than that and it had errors and they seemed consistant, regardless of speed, and only in a clockwise direction.Is this a 2007 ADXRS150 gyro? Open up adc.h and change the sample rate to 1600Hz and samples per update to 16 and report back.

-Kevin

[Gary Bonner]

Is this a 2007 ADXRS150 gyro? Open up adc.h and change the sample rate to 1600Hz and samples per update to 16 and report back.

-Kevin
Yes, it's this year's gyro. I'll try this when I get back to the shop tonight. We're using a 2004 RC, if that makes any difference.

Thanks.

[kaszeta]

If we turned it extremely slowly (about 5 sec to turn 180 degrees), it seemed to work ok. Anything faster than that and it had errors and they seemed consistant, regardless of speed, and only in a clockwise direction.

Okay, this helps. It's probably not a deadband issue, mounting issue, or calibration issue. The next thing to check is increasing the sampling rate and precision. (Oops, I just saw that Kevin recommended the same thing).

I'll run some tests on our 2007 kit gyro (we're not using it, we decided again to use a ADXRS300EB since we can really spin our robot. We lose some precision, but make it up in overall accuracy).

[Kevin Watson]

Yes, it's this year's gyro. I'll try this when I get back to the shop tonight. We're using a 2004 RC, if that makes any difference.As a backup plan, you should ask FIRST for a new gyro. Tell them that I suspect it's defective.

The 2004 RC should work fine assuming you compiled the code for the 18F8520, not the 18F8722.

-Kevin

[Gary Bonner]

Is this a 2007 ADXRS150 gyro? Open up adc.h and change the sample rate to 1600Hz and samples per update to 16 and report back.

-Kevin
We tried this and we get a bias of 2026. Rotated counter-clockwise, we fairly consistently get an angle of about 3180. Clockwise rotation gives inconsistent results, ranging from about 2900 to 3100.

We tried last year’s gyro, and it consistently gives angles of about 3260 in each direction and at both sampling rates. Also, when printing out the Get_Analog_Value() (using the default code), the values range from 0 to 1005.

[Kevin Watson]

We tried this and we get a bias of 2026. Rotated counter-clockwise, we fairly consistently get an angle of about 3180. Clockwise rotation gives inconsistent results, ranging from about 2900 to 3100.

We tried last year’s gyro, and it consistently gives angles of about 3260 in each direction and at both sampling rates. Also, when printing out the Get_Analog_Value() (using the default code), the values range from 0 to 1005.You've got a bad gyro. Ask FIRST for a new one and use the 2006 gyro until you receive the new one. Let me know if you run into any problems.

-Kevin

[Gary Bonner]

Thanks for your help.

[Ryan O]

Hi,

We're new to gyros, and it seems like it should be okay. My one question is what the output will be like. If I turn the gyro clockwise from zero, will I get positive angles, and negitive if counter? I haven't been able to find it in the whitepapers. Thanks for any help :)

[kaszeta]

Hi,

We're new to gyros, and it seems like it should be okay. My one question is what the output will be like. If I turn the gyro clockwise from zero, will I get positive angles, and negitive if counter? I haven't been able to find it in the whitepapers. Thanks for any help :)

It's in the spec sheet for the actual gyro, but if you mount it right-side up, clockwise is positive.

[Kevin Watson]

While porting code to Vex tonight I discovered a bug in the Process_Gyro_Data( ) function that could cause the bias to be calculated incorrectly. Instead of using the bias algorithm I used in the 2008 FRC code, I used this code and found the error:


if(Get_Gyro_Bias_Status() == GYRO_BIAS_NOT_DONE)
{
Start_Gyro_Bias_Calc();
printf("Start Time=%u\r\n", (unsigned int)Get_Time());
}
else if(Get_Gyro_Bias_Status() == GYRO_BIAS_BUFFER_FULL)
{
Stop_Gyro_Bias_Calc();
// reset the gyro heading angle
Reset_Gyro_Angle();
printf("End Time=%u\r\n", (unsigned int)Get_Time());
}
else if(Get_Gyro_Bias_Status() == GYRO_BIAS_READY)
{
printf("Time=%u Gyro Angle=%d\r\n", (unsigned int)Get_Time(), (int)Get_Gyro_Angle());
}



The bug in on this line of Process_Gyro_Data( ):

// is the circular queue now full?
if(Gyro_Queue_Index == GYRO_QUEUE_SIZE-1)

Remove the -1 to correct the bug:

// is the circular queue now full?
if(Gyro_Queue_Index == GYRO_QUEUE_SIZE)

-Kevin