Chief Delphi - Gyroscope Code

Chief Delphi (http://www.chiefdelphi.com/forums/index.php)

- Programming (http://www.chiefdelphi.com/forums/forumdisplay.php?f=51)

- - Gyroscope Code (http://www.chiefdelphi.com/forums/showthread.php?t=39537)

Re: Gyroscope Code

Quote:

Originally Posted by 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!

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

-Kevin

Re: Gyroscope Code

OK, will do! Thanks Kevin!

Re: Gyroscope Code

Quote:

Originally Posted by 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.

-Kevin

Re: Gyroscope Code

Quote:

Originally Posted by Kevin Watson

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.

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

Re: Gyroscope Code

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!

Re: Gyroscope Code

Quote:

Originally Posted by jaustin

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

Re: Gyroscope Code

Quote:

Originally Posted by 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?

Good question!

Re: Gyroscope Code

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.

Re: Gyroscope Code

Quote:

Originally Posted by Steve Jacobson

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.

Re: Gyroscope Code

Quote:

Originally Posted by Steve Jacobson

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/sh...2&postcount=21

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

-Kevin

Re: Gyroscope Code

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:

Code:

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:

Code:

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.

Re: Gyroscope Code

Quote:

Originally Posted by Joel J.

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

Re: Gyroscope Code

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.

Re: Gyroscope Code

Quote:

Originally Posted by Joel J.

A fun experiment would be to calculate both simultaneously and compare the results using different sample rates and gyro output frequencies.

-Kevin

Re: Gyroscope Code

Quote:

Originally Posted by Kevin Watson

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?