TRC Library for FTC (2011) 2011
TRC Library Code Project

C:/Users/Michael/Ftc/2011/code/trclib/gyro.h

Go to the documentation of this file.
00001 #if 0
00002 
00003 
00004 
00005 
00006 
00007 
00008 
00009 
00010 
00011 
00012 
00013 #endif
00014 
00015 #ifndef _GYRO_H
00016 #define _GYRO_H
00017 
00018 #include "..\HTDriversV1.6\drivers\HTGYRO-driver.h"
00019 
00020 #pragma systemFile
00021 
00022 #ifdef MOD_ID
00023     #undef MOD_ID
00024 #endif
00025 #define MOD_ID                  MOD_GYRO
00026 
00027 //
00028 // Constants.
00029 //
00030 #define GYROF_USER_MASK         0x00ff
00031 #define GYROF_INVERSE           0x0001
00032 #ifdef HTSMUX_STATUS
00033   #define GYROF_HTSMUX          0x0080
00034 #endif
00035 
00036 #define GYRO_NUM_CAL_SAMPLES    50
00037 #define GYRO_CAL_INTERVAL       10
00038 
00039 //
00040 // Macros
00041 //
00042 #define GyroGetTurnRate(p)      (p.turnRate)
00043 #define GyroGetHeading(p)       (p.heading)
00044 
00045 //
00046 // Type definitions.
00047 //
00048 typedef struct
00049 {
00050     int   sensorID;
00051     int   gyroFlags;
00052     int   zeroOffset;
00053     int   deadBand;
00054     long  timestamp;
00055     int   turnRate;
00056     float heading;
00057 } GYRO;
00058 
00066 void
00067 GyroCal(
00068     __out GYRO &gyro,
00069     __in  int numSamples,
00070     __in  int calInterval
00071     )
00072 {
00073     int i;
00074     int turnRate;
00075     int min, max;
00076 
00077     TFuncName("GyroCal");
00078     TLevel(API);
00079 
00080     gyro.zeroOffset = 0;
00081     gyro.deadBand = 0;
00082     min = 1023;
00083     max = 0;
00084 
00085     for (i = 0; i < numSamples; i++)
00086     {
00087 #ifdef HTSMUX_STATUS
00088         turnRate = (gyro.gyroFlags & GYROF_HTSMUX)?
00089                         HTGYROreadRot((tMUXSensor)gyro.sensorID):
00090                         HTGYROreadRot((tSensors)gyro.sensorID);
00091 #else
00092         turnRate = HTGYROreadRot((tSensors)gyro.sensorID);
00093 #endif
00094         gyro.zeroOffset += turnRate;
00095 
00096         if (turnRate < min)
00097         {
00098             min = turnRate;
00099         }
00100         else if (turnRate > max)
00101         {
00102             max = turnRate;
00103         }
00104 
00105         wait1Msec(calInterval);
00106     }
00107 
00108     gyro.zeroOffset /= numSamples;
00109     gyro.deadBand = max - min;
00110 
00111     TExit();
00112     return;
00113 }   //GyroCal
00114 
00121 void
00122 GyroTask(
00123     __inout GYRO &gyro
00124     )
00125 {
00126     long currTime;
00127 
00128     TFuncName("GyroTask");
00129     TLevel(TASK);
00130     TEnter();
00131 
00132     currTime = nPgmTime;
00133 #ifdef HTSMUX_STATUS
00134     gyro.turnRate = (gyro.gyroFlags & GYROF_HTSMUX)?
00135                         HTGYROreadRot((tMUXSensor)gyro.sensorID):
00136                         HTGYROreadRot((tSensors)gyro.sensorID);
00137 #else
00138     gyro.turnRate = HTGYROreadRot((tSensors)gyro.sensorID);
00139 #endif
00140     gyro.turnRate -= gyro.zeroOffset;
00141     gyro.turnRate = DEADBAND(gyro.turnRate, gyro.deadBand);
00142     if (gyro.gyroFlags & GYROF_INVERSE)
00143     {
00144         gyro.turnRate = -gyro.turnRate;
00145     }
00146     gyro.heading += (float)gyro.turnRate*(currTime - gyro.timestamp)/1000;
00147     gyro.timestamp = currTime;
00148 
00149     TExit();
00150     return;
00151 }   //GyroTask
00152 
00158 void
00159 GyroReset(
00160     __out GYRO &gyro
00161     )
00162 {
00163     TFuncName("GyroReset");
00164     TLevel(API);
00165     TEnter();
00166 
00167     GyroTask(gyro);
00168     gyro.heading = 0;
00169 
00170     TExit();
00171     return;
00172 }   //GyroReset
00173 
00181 void
00182 GyroInit(
00183     __out GYRO &gyro,
00184     __in  int sensorID,
00185     __in  int gyroFlags
00186     )
00187 {
00188     TFuncName("GyroInit");
00189     TLevel(INIT);
00190     TEnter();
00191 
00192     gyro.sensorID = sensorID;
00193     gyro.gyroFlags = gyroFlags & GYROF_USER_MASK;
00194 #ifdef HTSMUX_STATUS
00195     if (gyro.gyroFlags & GYROF_HTSMUX)
00196     {
00197         HTGYROstartCal((tMUXSensor)sensorID);
00198     }
00199     else
00200     {
00201         HTGYROstartCal((tSensors)sensorID);
00202     }
00203 #else
00204     HTGYROstartCal((tSensors)sensorID);
00205 #endif
00206     GyroCal(gyro, GYRO_NUM_CAL_SAMPLES, GYRO_CAL_INTERVAL);
00207     gyro.timestamp = nPgmTime;
00208     GyroReset(gyro);
00209 
00210     TExit();
00211     return;
00212 }   //GyroInit
00213 
00214 #endif  //ifndef _GYRO_H
 All Data Structures Files Functions Variables Defines