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

C:/Users/Michael/Frc/2011/code/trclib/TrcGyro.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 _TRCGYRO_H
00017 #define _TRCGYRO_H
00018 
00019 #ifdef MOD_ID
00020     #undef MOD_ID
00021 #endif
00022 #define MOD_ID                  MOD_GYRO
00023 #ifdef MOD_NAME
00024     #undef MOD_NAME
00025 #endif
00026 #define MOD_NAME                "TrcGyro"
00027 
00028 #define PIDMODE_ANGLE           0
00029 #define PIDMODE_VELOCITY        1
00030 #define PIDMODE_ACCELERATION    2
00031 
00038 class TrcGyro: public Gyro
00039 {
00040 private:
00041     float       m_period;
00042     SEM_ID      m_semaphore;
00043     Notifier   *m_notifier;
00044     float       m_angle;
00045     float       m_angularVelocity;
00046     float       m_angularAcceleration;
00047     UINT32      m_modePID;
00048 
00055     void
00056     Differentiator(
00057         void
00058         )
00059     {
00060         TLevel(HIFREQ);
00061         TEnter();
00062 
00063         CRITICAL_REGION(m_semaphore)
00064         {
00065             float prevAngle = m_angle;
00066             float prevVelocity = m_angularVelocity;
00067             m_angle = GetAngle();
00068             m_angularVelocity = (m_angle - prevAngle)/m_period;
00069             m_angularAcceleration = (m_angularVelocity - prevVelocity)/
00070                                     m_period;
00071             TSampling(("angle=%f,anglevel=%f,angleaccel=%f",
00072                        m_angle, m_angularVelocity, m_angularAcceleration));
00073         }
00074         END_REGION;
00075 
00076         TExit();
00077     }   //Differentiator
00078 
00085     static
00086     void
00087     CallDifferentiator(
00088         __in void *gyro
00089         )
00090     {
00091         TLevel(HIFREQ);
00092         TEnterMsg(("gyro=%p", gyro));
00093         ((TrcGyro *)gyro)->Differentiator();
00094         TExit();
00095     }   //CallDifferentiator
00096 
00100     void
00101     GyroInit(
00102         void
00103         )
00104     {
00105         TLevel(INIT);
00106         TEnter();
00107 
00108         m_semaphore = semBCreate(SEM_Q_PRIORITY, SEM_FULL);
00109         m_notifier = new Notifier(TrcGyro::CallDifferentiator, this);
00110         m_angle = GetAngle();
00111         m_angularVelocity = 0.0;
00112         m_angularAcceleration = 0.0;
00113         m_modePID = PIDMODE_ANGLE;
00114         m_notifier->StartPeriodic(m_period);
00115 
00116         TExit();
00117     }   //GyroInit
00118 
00119 public:
00130     TrcGyro(
00131         __in UINT32 slot,
00132         __in UINT32 channel,
00133         __in float  period = 0.05
00134         ): Gyro(slot, channel),
00135            m_period(period)
00136     {
00137         TLevel(INIT);
00138         TEnterMsg(("slot=%d,channel=%d,period=%f", slot, channel, period));
00139         GyroInit();
00140         TExit();
00141     }   //TrcGyro
00142 
00152     TrcGyro(
00153         __in UINT32 channel,
00154         __in float  period = 0.05
00155         ): Gyro(channel),
00156            m_period(period)
00157     {
00158         TLevel(INIT);
00159         TEnterMsg(("channel=%d,period=%f", channel, period));
00160         GyroInit();
00161         TExit();
00162     }   //TrcGyro
00163 
00167     ~TrcGyro(
00168         void
00169         )
00170     {
00171         TLevel(INIT);
00172         TEnter();
00173 
00174         semFlush(m_semaphore);
00175         SAFE_DELETE(m_notifier);
00176 
00177         TExit();
00178     }   //~TrcGyro
00179 
00183     float
00184     GetAngle(
00185         void
00186         )
00187     {
00188         float value;
00189         TLevel(API);
00190         TEnter();
00191 
00192         CRITICAL_REGION(m_semaphore)
00193         {
00194             value = m_angle;
00195         }
00196         END_REGION;
00197 
00198         TExitMsg(("=%f", value));
00199         return value;
00200     }   //GetAngle
00201 
00205     float
00206     GetAngularVelocity(
00207         void
00208         )
00209     {
00210         float value;
00211         TLevel(API);
00212         TEnter();
00213 
00214         CRITICAL_REGION(m_semaphore)
00215         {
00216             value = m_angularVelocity;
00217         }
00218         END_REGION;
00219 
00220         TExitMsg(("=%f", value));
00221         return value;
00222     }   //GetAngularVelocity
00223 
00228     float
00229     GetAngularAcceleration(
00230         void
00231         )
00232     {
00233         float value;
00234         TLevel(API);
00235         TEnter();
00236 
00237         CRITICAL_REGION(m_semaphore)
00238         {
00239             value = m_angularAcceleration;
00240         }
00241         END_REGION;
00242 
00243         TExitMsg(("=%f", value));
00244         return value;
00245     }   //GetAngularAcceleration
00246 
00251     void
00252     SetPIDMode(
00253         __in UINT32 mode
00254         )
00255     {
00256         TLevel(API);
00257         TEnterMsg(("mode=%d", mode));
00258 
00259         CRITICAL_REGION(m_semaphore)
00260         {
00261             m_modePID = mode;
00262         }
00263         END_REGION;
00264 
00265         TExit();
00266     }   //SetPIDMode
00267 
00272     double
00273     PIDGet(
00274         void
00275         )
00276     {
00277         double value = 0.0;
00278         TLevel(HIFREQ);
00279         TEnter();
00280 
00281         CRITICAL_REGION(m_semaphore)
00282         {
00283             switch (m_modePID)
00284             {
00285             case PIDMODE_ANGLE:
00286                 value = m_angle;
00287                 break;
00288 
00289             case PIDMODE_VELOCITY:
00290                 value = m_angularVelocity;
00291                 break;
00292 
00293             case PIDMODE_ACCELERATION:
00294                 value = m_angularAcceleration;
00295                 break;
00296             }
00297         }
00298         END_REGION;
00299 
00300         TExitMsg(("=%f", value));
00301         return value;
00302     }   //PIDGet
00303 };  //class TrcGyro
00304 
00305 #endif  //ifndef _TRCGYRO_H
 All Classes Files Functions Variables Defines