

Thats what makes FIRST so unique how culture and the time is reflected through our bots, but at the same time we all get along...with no problem.  Dan Richardson [more] 



Thread Tools  Rating:  Display Modes 
#31




Re: Gyroscope Code
Quote:
Kevin 
#32




Re: Gyroscope Code
OK, will do! Thanks Kevin!
Last edited by jaustin : 02102006 at 12:53 AM. Reason: Added Files 
#33




Re: Gyroscope Code
Quote:
Kevin 
#34




Re: Gyroscope Code
Quote:

#35




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! 
#36




Re: Gyroscope Code
Quote:
Kevin 
#37




Re: Gyroscope Code
Quote:

#38




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




Re: Gyroscope Code
Quote:

#40




Re: Gyroscope Code
Quote:
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 spitout 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 
#41




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); } 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; } } 
#42




Re: Gyroscope Code
Quote:
Kevin 
#43




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




Re: Gyroscope Code
Quote:
Kevin 
#45




Re: Gyroscope Code
Quote:
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? 
Thread Tools  
Display Modes  Rate This Thread 


Similar Threads  
Thread  Thread Starter  Forum  Replies  Last Post 
Out of the Box Camera Code  russell  Programming  9  10212009 04:28 AM 
Code suddenly fails to initialize  miketwalker  Programming  11  02192005 02:23 PM 
Team THRUST  Kevin's Code and Camera Code Combine  Chris_Elston  Programming  3  01312005 09:28 PM 
Sourceforge for Code Repository and other stuff  SilverStar  Programming  9  01152005 08:16 PM 
heres the code. y this not working  omega  Programming  16  03312004 02:18 PM 