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

C:/Users/Michael/Frc/2011/code/trclib/TrcAccel.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 _TRCACCEL_H
00017 #define _TRCACCEL_H
00018 
00019 #ifdef MOD_ID
00020     #undef MOD_ID
00021 #endif
00022 #define MOD_ID                  MOD_ACCEL
00023 #ifdef MOD_NAME
00024     #undef MOD_NAME
00025 #endif
00026 #define MOD_NAME                "TrcAccel"
00027 
00028 //
00029 // 50 calibration points at 20ms each will cost us a total of 1 second start
00030 // up time.
00031 //
00032 #ifndef ACCEL_NUM_CAL_PTS
00033     #define ACCEL_NUM_CAL_PTS   50
00034 #endif
00035 #ifndef ACCEL_CAL_INTERVAL
00036     #define ACCEL_CAL_INTERVAL  0.01    //10ms
00037 #endif
00038 
00046 class TrcAccel: public ADXL345_I2C
00047 {
00048 private:
00049     SEM_ID      m_semaphore;
00050     Notifier   *m_notifier;
00051     AllAxes     m_accelData;
00052     AllAxes     m_zeroOffset;
00053     AllAxes     m_deadBand;
00054     double      m_xVel;
00055     double      m_yVel;
00056     double      m_zVel;
00057     double      m_xDist;
00058     double      m_yDist;
00059     double      m_zDist;
00060     bool        m_fEnabled;
00061     UINT32      m_timestamp;
00062     
00069     void
00070     Integrator(
00071         void
00072         )
00073     {
00074         TLevel(HIFREQ);
00075         TEnter();
00076 
00077         CRITICAL_REGION(m_semaphore)
00078         {
00079             if (m_fEnabled)
00080             {
00081                 UINT32 timeCurr = GetMsecTime();
00082                 float period = (float)(timeCurr - m_timestamp)/1000.0;
00083                 m_timestamp = timeCurr;
00084                 m_accelData = GetAccelerations();
00085                 m_accelData.XAxis -= m_zeroOffset.XAxis;
00086                 m_accelData.YAxis -= m_zeroOffset.YAxis;
00087                 m_accelData.ZAxis -= m_zeroOffset.ZAxis;
00088                 m_accelData.XAxis = GRAVITY_CONSTANT*
00089                                     DEADBAND(m_accelData.XAxis,
00090                                              m_deadBand.XAxis);
00091                 m_accelData.YAxis = GRAVITY_CONSTANT*
00092                                     DEADBAND(m_accelData.YAxis,
00093                                              m_deadBand.YAxis);
00094                 m_accelData.ZAxis = GRAVITY_CONSTANT*
00095                                     DEADBAND(m_accelData.ZAxis,
00096                                              m_deadBand.ZAxis);
00097                 m_xVel += m_accelData.XAxis*period;
00098                 m_yVel += m_accelData.YAxis*period;
00099                 m_zVel += m_accelData.ZAxis*period;
00100                 m_xDist += m_xVel*period;
00101                 m_yDist += m_yVel*period;
00102                 m_zDist += m_zVel*period;
00103                 TSampling(("X(%f,%f,%f), Y(%f,%f,%f), Z(%f,%f,%f)",
00104                            m_accelData.XAxis*FEET_PER_METER,
00105                            m_xVel*FEET_PER_METER,
00106                            m_xDist*FEET_PER_METER,
00107                            m_accelData.YAxis*FEET_PER_METER,
00108                            m_yVel*FEET_PER_METER,
00109                            m_yDist*FEET_PER_METER,
00110                            m_accelData.ZAxis*FEET_PER_METER,
00111                            m_zVel*FEET_PER_METER,
00112                            m_zDist*FEET_PER_METER));
00113             }
00114         }
00115         END_REGION;
00116 
00117         TExit();
00118     }   //Integrator
00119 
00126     static
00127     void
00128     CallIntegrator(
00129         __in void *accel
00130         )
00131     {
00132         TLevel(HIFREQ);
00133         TEnterMsg(("accel=%p", accel));
00134         ((TrcAccel *)accel)->Integrator();
00135         TExit();
00136     }   //CallIntegrator
00137 
00138 public:
00139 
00153     void
00154     Calibrate(
00155         __in UINT32 numCalPts,
00156         __in float calInterval
00157         void
00158         )
00159     {
00160         AllAxes data;
00161         AllAxes zeroG;
00162         AllAxes min;
00163         AllAxes max;
00164 
00165         TLevel(INIT);
00166         TEnter();
00167 
00168         zeroG.XAxis = 0.0;
00169         zeroG.YAxis = 0.0;
00170         zeroG.YAxis = 0.0;
00171         min.XAxis = 1000.0;
00172         min.YAxis = 1000.0;
00173         min.ZAxis = 1000.0;
00174         max.XAxis = 0.0;
00175         max.YAxis = 0.0;
00176         max.ZAxis = 0.0;
00177         for (UINT32 i = 0; i < numCalPts; i++)
00178         {
00179             data = GetAccelerations();
00180 
00181             zeroG.XAxis += data.XAxis;
00182             zeroG.YAxis += data.YAxis;
00183             zeroG.ZAxis += data.ZAxis;
00184 
00185             if (data.XAxis < min.XAxis)
00186             {
00187                 min.XAxis = data.XAxis;
00188             }
00189             else if (data.XAxis > max.XAxis)
00190             {
00191                 max.XAxis = data.XAxis;
00192             }
00193 
00194             if (data.YAxis < min.YAxis)
00195             {
00196                 min.YAxis = data.YAxis;
00197             }
00198             else if (data.YAxis > max.YAxis)
00199             {
00200                 max.YAxis = data.YAxis;
00201             }
00202 
00203             if (data.ZAxis < min.ZAxis)
00204             {
00205                 min.ZAxis = data.ZAxis;
00206             }
00207             else if (data.ZAxis > max.ZAxis)
00208             {
00209                 max.ZAxis = data.ZAxis;
00210             }
00211 
00212             Wait(calInterval);
00213         }
00214 
00215         CRITICAL_REGION(m_semaphore)
00216         {
00217             m_zeroOffset.XAxis = zeroG.XAxis/numCalPts;
00218             m_zeroOffset.YAxis = zeroG.YAxis/numCalPts;
00219             m_zeroOffset.ZAxis = zeroG.ZAxis/numCalPts;
00220 
00221             m_deadBand.XAxis = max.XAxis - min.XAxis;
00222             m_deadBand.YAxis = max.YAxis - min.YAxis;
00223             m_deadBand.ZAxis = max.ZAxis - min.ZAxis;
00224 
00225             TInfo(("AccelZeroG:x=%f,y=%f,z=%f, AccelDeadBand:x=%f,y=%f,z=%f",
00226                    m_zeroOffset.XAxis, m_zeroOffset.YAxis, m_zeroOffset.ZAxis,
00227                    m_deadBand.XAxis, m_deadBand.YAxis, m_deadBand.ZAxis));
00228         }
00229         END_REGION;
00230 
00231         Reset();
00232 
00233         TExit();
00234     }   //Calibrate
00235 
00246     TrcAccel(
00247         __in UINT32 slot,
00248         __in float  period = 0.02
00249         ): ADXL345_I2C(slot)
00250     {
00251         TLevel(INIT);
00252         TEnterMsg(("slot=%d,period=%f", slot, period));
00253 
00254         m_semaphore = semBCreate(SEM_Q_PRIORITY, SEM_FULL);
00255         m_notifier = new Notifier(TrcAccel::CallIntegrator, this);
00256         m_accelData.XAxis = 0.0;
00257         m_accelData.YAxis = 0.0;
00258         m_accelData.ZAxis = 0.0;
00259         m_zeroOffset.XAxis = 0.0;
00260         m_zeroOffset.YAxis = 0.0;
00261         m_zeroOffset.ZAxis = 0.0;
00262         m_deadBand.XAxis = 0.0;
00263         m_deadBand.YAxis = 0.0;
00264         m_deadBand.ZAxis = 0.0;
00265         m_xVel = 0.0;
00266         m_yVel = 0.0;
00267         m_zVel = 0.0;
00268         m_xDist = 0.0;
00269         m_yDist = 0.0;
00270         m_zDist = 0.0;
00271         m_fEnabled = false;
00272         Calibrate(ACCEL_NUM_CAL_PTS, ACCEL_CAL_INTERVAL);
00273         m_notifier->StartPeriodic(period);
00274 
00275         TExit();
00276     }   //TrcAccel
00277 
00281     ~TrcAccel(
00282         void
00283         )
00284     {
00285         TLevel(INIT);
00286         TEnter();
00287 
00288         semFlush(m_semaphore);
00289         SAFE_DELETE(m_notifier);
00290 
00291         TExit();
00292     }   //~TrcAccel
00293 
00300     double
00301     GetMetricAccelX(
00302         void
00303         )
00304     {
00305         TLevel(HIFREQ);
00306         TEnter();
00307         TExitMsg(("=%f", m_accelData.XAxis));
00308         return m_accelData.XAxis;
00309     }   //GetMetricAccelX
00310 
00317     double
00318     GetAccelX(
00319         void
00320         )
00321     {
00322         TLevel(HIFREQ);
00323         TEnter();
00324         double value = m_accelData.XAxis*FEET_PER_METER;
00325         TExitMsg(("=%f", value));
00326         return value;
00327     }   //GetAccelX
00328 
00335     double
00336     GetMetricAccelY(
00337         void
00338         )
00339     {
00340         TLevel(HIFREQ);
00341         TEnter();
00342         TExitMsg(("=%f", m_accelData.YAxis));
00343         return m_accelData.YAxis;
00344     }   //GetMetricAccelY
00345 
00352     double
00353     GetAccelY(
00354         void
00355         )
00356     {
00357         TLevel(HIFREQ);
00358         TEnter();
00359         double value = m_accelData.YAxis*FEET_PER_METER;
00360         TExitMsg(("=%f", value));
00361         return value;
00362     }   //GetAccelY
00363 
00370     double
00371     GetMetricAccelZ(
00372         void
00373         )
00374     {
00375         TLevel(HIFREQ);
00376         TEnter();
00377         TExitMsg(("=%f", m_accelData.ZAxis));
00378         return m_accelData.ZAxis;
00379     }   //GetMetricAccelZ
00380 
00387     double
00388     GetAccelZ(
00389         void
00390         )
00391     {
00392         TLevel(HIFREQ);
00393         TEnter();
00394         double value = m_accelData.ZAxis*FEET_PER_METER;
00395         TExitMsg(("=%f", value));
00396         return value;
00397     }   //GetAccelZ
00398 
00405     double
00406     GetMetricVelX(
00407         void
00408         )
00409     {
00410         TLevel(HIFREQ);
00411         TEnter();
00412         TExitMsg(("=%f", m_xVel));
00413         return m_xVel;
00414     }   //GetMetricVelX
00415 
00422     double
00423     GetVelX(
00424         void
00425         )
00426     {
00427         TLevel(HIFREQ);
00428         TEnter();
00429         double value = m_xVel*FEET_PER_METER;
00430         TExitMsg(("=%f", value));
00431         return value;
00432     }   //GetVelX
00433 
00440     double
00441     GetMetricVelY(
00442         void
00443         )
00444     {
00445         TLevel(HIFREQ);
00446         TEnter();
00447         TExitMsg(("=%f", m_yVel));
00448         return m_yVel;
00449     }   //GetMetricVelY
00450 
00457     double
00458     GetVelY(
00459         void
00460         )
00461     {
00462         TLevel(HIFREQ);
00463         TEnter();
00464         double value = m_yVel*FEET_PER_METER;
00465         TExitMsg(("=%f", value));
00466         return value;
00467     }   //GetVelY
00468 
00475     double
00476     GetMetricVelZ(
00477         void
00478         )
00479     {
00480         TLevel(HIFREQ);
00481         TEnter();
00482         TExitMsg(("=%f", m_zVel));
00483         return m_zVel;
00484     }   //GetMetricVelZ
00485 
00492     double
00493     GetVelZ(
00494         void
00495         )
00496     {
00497         TLevel(HIFREQ);
00498         TEnter();
00499         double value = m_zVel*FEET_PER_METER;
00500         TExitMsg(("=%f", value));
00501         return value;
00502     }   //GetVelZ
00503 
00510     double
00511     GetMetricDistX(
00512         void
00513         )
00514     {
00515         TLevel(HIFREQ);
00516         TEnter();
00517         TExitMsg(("=%f", m_xDist));
00518         return m_xDist;
00519     }   //GetMetricDistX
00520 
00527     double
00528     GetDistX(
00529         void
00530         )
00531     {
00532         TLevel(HIFREQ);
00533         TEnter();
00534         double value = m_xDist*FEET_PER_METER;
00535         TExitMsg(("=%f", value));
00536         return value;
00537     }   //GetDistX
00538 
00545     double
00546     GetMetricDistY(
00547         void
00548         )
00549     {
00550         TLevel(HIFREQ);
00551         TEnter();
00552         TExitMsg(("=%f", m_yDist));
00553         return m_yDist;
00554     }   //GetMetricDistY
00555 
00562     double
00563     GetDistY(
00564         void
00565         )
00566     {
00567         TLevel(HIFREQ);
00568         TEnter();
00569         double value = m_yDist*FEET_PER_METER;
00570         TExitMsg(("=%f", value));
00571         return value;
00572     }   //GetDistY
00573 
00580     double
00581     GetMetricDistZ(
00582         void
00583         )
00584     {
00585         TLevel(HIFREQ);
00586         TEnter();
00587         TExitMsg(("=%f", m_zDist));
00588         return m_zDist;
00589     }   //GetMetricDistZ
00590 
00597     double
00598     GetDistZ(
00599         void
00600         )
00601     {
00602         TLevel(HIFREQ);
00603         TEnter();
00604         double value = m_zDist*FEET_PER_METER;
00605         TExitMsg(("=%f", value));
00606         return value;
00607     }   //GetDistZ
00608 
00612     void
00613     Reset(
00614         void
00615         )
00616     {
00617         TLevel(API);
00618         TEnter();
00619 
00620         CRITICAL_REGION(m_semaphore)
00621         {
00622             m_accelData.XAxis = 0.0;
00623             m_accelData.YAxis = 0.0;
00624             m_accelData.ZAxis = 0.0;
00625             m_xVel = 0.0;
00626             m_yVel = 0.0;
00627             m_zVel = 0.0;
00628             m_xDist = 0.0;
00629             m_yDist = 0.0;
00630             m_zDist = 0.0;
00631         }
00632         END_REGION;
00633 
00634         TExit();
00635     }   //Reset
00636 
00642     void
00643     SetEnabled(
00644         __in bool fEnabled
00645         )
00646     {
00647         TLevel(API);
00648         TEnter();
00649 
00650         m_fEnabled = fEnabled;
00651         if (fEnabled)
00652         {
00653             Reset();
00654             m_timestamp = GetMsecTime();
00655         }
00656 
00657         TExit();
00658     }   //SetEnabled
00659 };  //class TrcAccel
00660 
00661 #endif  //ifndef _TRCACCEL_H
 All Classes Files Functions Variables Defines