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

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

Go to the documentation of this file.
00001 #if 0
00002 
00003 
00004 
00005 
00006 
00007 
00008 
00009 
00010 
00011 
00012 
00013 
00014 #endif
00015 
00016 #ifndef _ACCEL_H
00017 #define _ACCEL_H
00018 
00019 #include "..\HTDriversV1.6\drivers\HTAC-driver.h"
00020 
00021 #pragma systemFile
00022 
00023 #ifdef MOD_ID
00024     #undef MOD_ID
00025 #endif
00026 #define MOD_ID                  MOD_ACCEL
00027 
00028 //
00029 // Constants.
00030 //
00031 #define ACCELF_USER_MASK        0x00ff
00032 #ifdef HTSMUX_STATUS
00033   #define ACCELF_HTSMUX         0x0080
00034 #endif
00035 #define ACCEL_COUNT_PER_G       200
00036 #define ACCEL_NUM_CAL_SAMPLES   50
00037 #define ACCEL_CAL_INTERVAL      10
00038 
00039 //
00040 // Type definitions.
00041 //
00042 typedef struct
00043 {
00044     int sensorID;
00045     int accelFlags;
00046     int xZeroOffset;
00047     int yZeroOffset;
00048     int zZeroOffset;
00049     int xDeadBand;
00050     int yDeadBand;
00051     int zDeadBand;
00052 } ACCEL;
00053 
00062 void
00063 AccelCal(
00064     __out ACCEL &accel,
00065     __in  int numSamples,
00066     __in  int calInterval
00067     )
00068 {
00069     int i;
00070     int xRaw, yRaw, zRaw;
00071     int xMin, yMin, zMin;
00072     int xMax, yMax, zMax;
00073     bool fSMux;
00074 
00075     TFuncName("AccelCal");
00076     TLevel(API);
00077 
00078     accel.xZeroOffset = 0;
00079     accel.yZeroOffset = 0;
00080     accel.zZeroOffset = 0;
00081     accel.xDeadBand = 0;
00082     accel.yDeadBand = 0;
00083     accel.zDeadBand = 0;
00084     xMin = yMin = zMin = 1023;
00085     xMax = yMax = zMax = 0;
00086     fSMux = (accel.accelFlags & ACCELF_HTSMUX) != 0;
00087     for (i = 0; i < numSamples; i++)
00088     {
00089 #ifdef HTSMUX_STATUS
00090         if (fSMux &&
00091             HTACreadAllAxes((tMUXSensor)accel.sensorID, xRaw, yRaw, zRaw) ||
00092             !fSMux &&
00093             HTACreadAllAxes((tSensors)accel.sensorID, xRaw, yRaw, zRaw))
00094 #else
00095         if (HTACreadAllAxes((tSensors)accel.sensorID, xRaw, yRaw, zRaw))
00096 #endif
00097         {
00098             accel.xZeroOffset += xRaw;
00099             accel.yZeroOffset += yRaw;
00100             accel.zZeroOffset += zRaw;
00101 
00102             if (xRaw < xMin)
00103             {
00104                 xMin = xRaw;
00105             }
00106             else if (xRaw > xMax)
00107             {
00108                 xMax = xRaw;
00109             }
00110 
00111             if (yRaw < yMin)
00112             {
00113                 yMin = yRaw;
00114             }
00115             else if (yRaw > yMax)
00116             {
00117                 yMax = yRaw;
00118             }
00119 
00120             if (zRaw < zMin)
00121             {
00122                 zMin = zRaw;
00123             }
00124             else if (zRaw > zMax)
00125             {
00126                 zMax = zRaw;
00127             }
00128         }
00129         wait1Msec(calInterval);
00130     }
00131 
00132     accel.xZeroOffset /= numSamples;
00133     accel.yZeroOffset /= numSamples;
00134     accel.zZeroOffset /= numSamples;
00135 
00136     accel.xDeadBand = xMax - xMin;
00137     accel.yDeadBand = yMax - yMin;
00138     accel.zDeadBand = zMax - zMin;
00139 
00140     TExit();
00141     return;
00142 }   //AccelCal
00143 
00151 void
00152 AccelInit(
00153     __out ACCEL &accel,
00154     __in  int sensorID,
00155     __in  int accelFlags
00156     )
00157 {
00158     TFuncName("AccelInit");
00159     TLevel(INIT);
00160     TEnter();
00161 
00162     accel.sensorID = sensorID;
00163     accel.accelFlags = accelFlags & ACCELF_USER_MASK;
00164     AccelCal(accel, ACCEL_NUM_CAL_SAMPLES, ACCEL_CAL_INTERVAL);
00165 
00166     TExit();
00167     return;
00168 }   //AccelInit
00169 
00178 bool
00179 AccelGetX(
00180     __in  ACCEL &accel,
00181     __out float &value
00182     )
00183 {
00184     bool rc;
00185     int raw;
00186     int dummy;
00187 
00188     TFuncName("AccelGetX");
00189     TLevel(API);
00190     TEnter();
00191 
00192 #ifdef HTSMUX_STATUS
00193     rc = (accel.accelFlags & ACCELF_HTSMUX)?
00194             HTACreadAllAxes((tMUXSensor)accel.sensorID, raw, dummy, dummy):
00195             HTACreadAllAxes((tSensors)accel.sensorID, raw, dummy, dummy);
00196 #else
00197     rc = HTACreadAllAxes((tSensors)accel.sensorID, raw, dummy, dummy));
00198 #endif
00199     if (rc == true)
00200     {
00201         value = (float)(DEADBAND(raw - accel.xZeroOffset, accel.xDeadBand))/
00202                 ACCEL_COUNT_PER_G;
00203     }
00204 
00205     TExitMsg(("x=%5.1f", value));
00206     return rc;
00207 }   //AccelGetX
00208 
00217 bool
00218 AccelGetY(
00219     __in  ACCEL &accel,
00220     __out float &value
00221     )
00222 {
00223     bool rc;
00224     int raw;
00225     int dummy;
00226 
00227     TFuncName("AccelGetY");
00228     TLevel(API);
00229     TEnter();
00230 
00231 #ifdef HTSMUX_STATUS
00232     rc = (accel.accelFlags & ACCELF_HTSMUX)?
00233             HTACreadAllAxes((tMUXSensor)accel.sensorID, dummy, raw, dummy):
00234             HTACreadAllAxes((tSensors)accel.sensorID, dummy, raw, dummy);
00235 #else
00236     rc = HTACreadAllAxes((tSensors)accel.sensorID, dummy, raw, dummy));
00237 #endif
00238     if (rc == true)
00239     {
00240         value = (float)(DEADBAND(raw - accel.yZeroOffset, accel.yDeadBand))/
00241                 ACCEL_COUNT_PER_G;
00242     }
00243 
00244     TExitMsg(("x=%5.1f", value));
00245     return rc;
00246 }   //AccelGetY
00247 
00256 bool
00257 AccelGetZ(
00258     __in  ACCEL &accel,
00259     __out float &value
00260     )
00261 {
00262     bool rc;
00263     int raw;
00264     int dummy;
00265 
00266     TFuncName("AccelGetZ");
00267     TLevel(API);
00268     TEnter();
00269 
00270 #ifdef HTSMUX_STATUS
00271     rc = (accel.accelFlags & ACCELF_HTSMUX)?
00272             HTACreadAllAxes((tMUXSensor)accel.sensorID, dummy, dummy, raw):
00273             HTACreadAllAxes((tSensors)accel.sensorID, dummy, dummy, raw);
00274 #else
00275     rc = HTACreadAllAxes((tSensors)accel.sensorID, dummy, dummy, raw));
00276 #endif
00277     if (rc == true)
00278     {
00279         value = (float)(DEADBAND(raw - accel.zZeroOffset, accel.zDeadBand))/
00280                 ACCEL_COUNT_PER_G;
00281     }
00282 
00283     TExitMsg(("x=%5.1f", value));
00284     return rc;
00285 }   //AccelGetZ
00286 
00298 bool
00299 AccelGetAllAxes(
00300     __in  ACCEL &accel,
00301     __out float &xValue,
00302     __out float &yValue,
00303     __out float &zValue
00304     )
00305 {
00306     bool rc;
00307     int xRaw, yRaw, zRaw;
00308 
00309     TFuncName("AccelGetAllAxes");
00310     TLevel(API);
00311     TEnter();
00312 
00313 #ifdef HTSMUX_STATUS
00314     rc = (accel.accelFlags & ACCELF_HTSMUX)?
00315             HTACreadAllAxes((tMUXSensor)accel.sensorID, xRaw, yRaw, zRaw):
00316             HTACreadAllAxes((tSensors)accel.sensorID, xRaw, yRaw, zRaw);
00317 #else
00318     rc = HTACreadAllAxes((tSensors)accel.sensorID, xRaw, yRaw, zRaw));
00319 #endif
00320     if (rc == true)
00321     {
00322         xValue = (float)(DEADBAND(xRaw - accel.xZeroOffset, accel.xDeadBand))/
00323                  ACCEL_COUNT_PER_G;
00324         yValue = (float)(DEADBAND(yRaw - accel.yZeroOffset, accel.yDeadBand))/
00325                  ACCEL_COUNT_PER_G;
00326         zValue = (float)(DEADBAND(zRaw - accel.zZeroOffset, accel.zDeadBand))/
00327                  ACCEL_COUNT_PER_G;
00328     }
00329 
00330     TExit();
00331     return rc;
00332 }   //AccelGetAllAxes
00333 
00334 #endif  //ifndef _ACCEL_H
 All Data Structures Files Functions Variables Defines