trclib/gyro.h
author Michael Tsang
Thu Feb 24 21:21:27 2011 -0800 (2011-02-24)
changeset 76 fc93e20c4205
parent 57 1f7259a6fd7f
permissions -rw-r--r--
Final code
     1 #if 0
     2 /// Copyright (c) Titan Robotics Club. All rights reserved.
     3 ///
     4 /// <module name="gyro.h" />
     5 ///
     6 /// <summary>
     7 ///     This module contains the library functions for the gyro sensor.
     8 /// </summary>
     9 ///
    10 /// <remarks>
    11 ///     Environment: RobotC for Lego Mindstorms NXT.
    12 /// </remarks>
    13 #endif
    14 
    15 #ifndef _GYRO_H
    16 #define _GYRO_H
    17 
    18 #include "..\HTDriversV1.6\drivers\HTGYRO-driver.h"
    19 
    20 #pragma systemFile
    21 
    22 #ifdef MOD_ID
    23     #undef MOD_ID
    24 #endif
    25 #define MOD_ID                  MOD_GYRO
    26 
    27 //
    28 // Constants.
    29 //
    30 #define GYROF_USER_MASK         0x00ff
    31 #define GYROF_INVERSE           0x0001
    32 #ifdef HTSMUX_STATUS
    33   #define GYROF_HTSMUX          0x0080
    34 #endif
    35 
    36 #define GYRO_NUM_CAL_SAMPLES    50
    37 #define GYRO_CAL_INTERVAL       10
    38 
    39 //
    40 // Macros
    41 //
    42 #define GyroGetTurnRate(p)      (p.turnRate)
    43 #define GyroGetHeading(p)       (p.heading)
    44 
    45 //
    46 // Type definitions.
    47 //
    48 typedef struct
    49 {
    50     int   sensorID;
    51     int   gyroFlags;
    52     int   zeroOffset;
    53     int   deadBand;
    54     long  timestamp;
    55     int   turnRate;
    56     float heading;
    57 } GYRO;
    58 
    59 /**
    60  *  This function calibrates the gyro for zero offset and deadband.
    61  *
    62  *  @param gyro Points to the GYRO structure to be initialized.
    63  *  @param numSamples Specifies the number of calibration samples.
    64  *  @param calInterval Specifies the calibration interval in msec.
    65  */
    66 void
    67 GyroCal(
    68     __out GYRO &gyro,
    69     __in  int numSamples,
    70     __in  int calInterval
    71     )
    72 {
    73     int i;
    74     int turnRate;
    75     int min, max;
    76 
    77     TFuncName("GyroCal");
    78     TLevel(API);
    79 
    80     gyro.zeroOffset = 0;
    81     gyro.deadBand = 0;
    82     min = 1023;
    83     max = 0;
    84 
    85     for (i = 0; i < numSamples; i++)
    86     {
    87 #ifdef HTSMUX_STATUS
    88         turnRate = (gyro.gyroFlags & GYROF_HTSMUX)?
    89                         HTGYROreadRot((tMUXSensor)gyro.sensorID):
    90                         HTGYROreadRot((tSensors)gyro.sensorID);
    91 #else
    92         turnRate = HTGYROreadRot((tSensors)gyro.sensorID);
    93 #endif
    94         gyro.zeroOffset += turnRate;
    95 
    96         if (turnRate < min)
    97         {
    98             min = turnRate;
    99         }
   100         else if (turnRate > max)
   101         {
   102             max = turnRate;
   103         }
   104 
   105         wait1Msec(calInterval);
   106     }
   107 
   108     gyro.zeroOffset /= numSamples;
   109     gyro.deadBand = max - min;
   110 
   111     TExit();
   112     return;
   113 }   //GyroCal
   114 
   115 /**
   116  *  This function performs the gyro task where it integrates the turn rate
   117  *  into a heading value.
   118  *
   119  *  @param gyro Points to the GYRO structure.
   120  */
   121 void
   122 GyroTask(
   123     __inout GYRO &gyro
   124     )
   125 {
   126     long currTime;
   127 
   128     TFuncName("GyroTask");
   129     TLevel(TASK);
   130     TEnter();
   131 
   132     currTime = nPgmTime;
   133 #ifdef HTSMUX_STATUS
   134     gyro.turnRate = (gyro.gyroFlags & GYROF_HTSMUX)?
   135                         HTGYROreadRot((tMUXSensor)gyro.sensorID):
   136                         HTGYROreadRot((tSensors)gyro.sensorID);
   137 #else
   138     gyro.turnRate = HTGYROreadRot((tSensors)gyro.sensorID);
   139 #endif
   140     gyro.turnRate -= gyro.zeroOffset;
   141     gyro.turnRate = DEADBAND(gyro.turnRate, gyro.deadBand);
   142     if (gyro.gyroFlags & GYROF_INVERSE)
   143     {
   144         gyro.turnRate = -gyro.turnRate;
   145     }
   146     gyro.heading += (float)gyro.turnRate*(currTime - gyro.timestamp)/1000;
   147     gyro.timestamp = currTime;
   148 
   149     TExit();
   150     return;
   151 }   //GyroTask
   152 
   153 /**
   154  *  This function resets the gyro heading.
   155  *
   156  *  @param gyro Points to the GYRO structure to be reset.
   157  */
   158 void
   159 GyroReset(
   160     __out GYRO &gyro
   161     )
   162 {
   163     TFuncName("GyroReset");
   164     TLevel(API);
   165     TEnter();
   166 
   167     GyroTask(gyro);
   168     gyro.heading = 0;
   169 
   170     TExit();
   171     return;
   172 }   //GyroReset
   173 
   174 /**
   175  *  This function initializes the gyro sensor.
   176  *
   177  *  @param gyro Points to the GYRO structure to be initialized.
   178  *  @param sensorID Specifies the ID of the gyro sensor.
   179  *  @param gyroFlags Specifies the gyro flags.
   180  */
   181 void
   182 GyroInit(
   183     __out GYRO &gyro,
   184     __in  int sensorID,
   185     __in  int gyroFlags
   186     )
   187 {
   188     TFuncName("GyroInit");
   189     TLevel(INIT);
   190     TEnter();
   191 
   192     gyro.sensorID = sensorID;
   193     gyro.gyroFlags = gyroFlags & GYROF_USER_MASK;
   194 #ifdef HTSMUX_STATUS
   195     if (gyro.gyroFlags & GYROF_HTSMUX)
   196     {
   197         HTGYROstartCal((tMUXSensor)sensorID);
   198     }
   199     else
   200     {
   201         HTGYROstartCal((tSensors)sensorID);
   202     }
   203 #else
   204     HTGYROstartCal((tSensors)sensorID);
   205 #endif
   206     GyroCal(gyro, GYRO_NUM_CAL_SAMPLES, GYRO_CAL_INTERVAL);
   207     gyro.timestamp = nPgmTime;
   208     GyroReset(gyro);
   209 
   210     TExit();
   211     return;
   212 }   //GyroInit
   213 
   214 #endif  //ifndef _GYRO_H