#if 0

 /// Copyright (c) Titan Robotics Club. All rights reserved.

 ///

 /// <module name="TrcAccel.h" />

 ///

 /// <summary>

 ///     This module contains the definition and implementation of the TrcAccel

 ///     class.

 /// </summary>

 ///

 /// <remarks>

 ///     Environment: Wind River C++ for National Instrument cRIO based Robot.

 /// </remarks>

 #endif

 #ifndef _TRCACCEL_H

 #define _TRCACCEL_H

 #ifdef MOD_ID

     #undef MOD_ID

 #endif

 #define MOD_ID                  MOD_ACCEL

 #ifdef MOD_NAME

     #undef MOD_NAME

 #endif

 #define MOD_NAME                "TrcAccel"

 //

 // 50 calibration points at 20ms each will cost us a total of 1 second start

 // up time.

 //

 #ifndef ACCEL_NUM_CAL_PTS

     #define ACCEL_NUM_CAL_PTS           50

 #endif

 #ifndef ACCEL_CAL_INTERVAL

     #define ACCEL_CAL_INTERVAL          0.02    //20ms

 #endif

 #ifndef ACCEL_DEADBAND_THRESHOLD

     #define ACCEL_DEADBAND_THRESHOLD    0.01

 #endif

 /**

  * This class defines and implements the TrcAccel object. The TrcAccel object

  * inherits the ADXL345_I2C accelerometer object from the WPI library. This

  * object periodically sample the accelerometer value for the acceleration

  * value. It also integrates the acceleration value to calculate the velocity

  * and then integrates the velocity to calculate the distance value.

  */

 class TrcAccel: public ADXL345_I2C

 {

 private:

     SEM_ID      m_semaphore;

     Notifier   *m_notifier;

     AllAxes     m_accelData;

     AllAxes     m_offsetZeroG;

     double      m_velX;

     double      m_velY;

     double      m_velZ;

     double      m_distX;

     double      m_distY;

     double      m_distZ;

     bool        m_fEnabled;

     UINT32      m_timestamp;

     /**

      * This function is called periodically by the a timer callback to

      * process the accelerometer data. It integrates the data with time

      * to calculate the velocity, integrates it again to calculate

      * distance.

      */

     void

     Integrator(

         void

         )

     {

         TLevel(HIFREQ);

         TEnter();

         CRITICAL_REGION(m_semaphore)

         {

             if (m_fEnabled)

             {

                 UINT32 timeCurr = GetMsecTime();

                 float period = (float)(timeCurr - m_timestamp)/1000.0;

                 m_timestamp = timeCurr;

                 m_accelData = GetAccelerations();

                 m_accelData.XAxis -= m_offsetZeroG.XAxis;

                 m_accelData.YAxis -= m_offsetZeroG.YAxis;

                 m_accelData.ZAxis -= m_offsetZeroG.ZAxis;

                 m_accelData.XAxis = GRAVITY_CONSTANT*m_accelData.XAxis;

                 m_accelData.YAxis = GRAVITY_CONSTANT*m_accelData.YAxis;

                 m_accelData.ZAxis = GRAVITY_CONSTANT*m_accelData.ZAxis;

 #if 0

                 m_accelData.XAxis = GRAVITY_CONSTANT*

                                     DEADBAND(m_accelData.XAxis,

                                              ACCEL_DEADBAND_THRESHOLD);

                 m_accelData.YAxis = GRAVITY_CONSTANT*

                                     DEADBAND(m_accelData.YAxis,

                                              ACCEL_DEADBAND_THRESHOLD);

                 m_accelData.ZAxis = GRAVITY_CONSTANT*

                                     DEADBAND(m_accelData.ZAxis,

                                              ACCEL_DEADBAND_THRESHOLD);

 #endif

                 m_velX += m_accelData.XAxis*period;

                 m_velY += m_accelData.YAxis*period;

                 m_velZ += m_accelData.ZAxis*period;

                 m_distX += m_velX*period;

                 m_distY += m_velY*period;

                 m_distZ += m_velZ*period;

                 TSampling(("X(%f,%f,%f), Y(%f,%f,%f), Z(%f,%f,%f)",

                            m_accelData.XAxis*FEET_PER_METER,

                            m_velX*FEET_PER_METER,

                            m_distX*FEET_PER_METER,

                            m_accelData.YAxis*FEET_PER_METER,

                            m_velY*FEET_PER_METER,

                            m_distY*FEET_PER_METER,

                            m_accelData.ZAxis*FEET_PER_METER,

                            m_velZ*FEET_PER_METER,

                            m_distZ*FEET_PER_METER));

             }

         }

         END_REGION;

         TExit();

     }   //Integrator

     /**

      * This function is called when the timer expired. It will call the

      * non-static worker function.

      *

      * @param timer Points to the TrcAccel object to call the worker function.

      */

     static

     void

     CallIntegrator(

         __in void *accel

         )

     {

         TLevel(HIFREQ);

         TEnterMsg(("accel=%p", accel));

         ((TrcAccel *)accel)->Integrator();

         TExit();

     }   //CallIntegrator

 public:

     /**

      * Constructor: Create an instance of the TrcAccel object. It initializes

      * the object and starts the periodic timer.

      *

      * @param slot Specifies the slot of the digital module on which the I2C

      *        port is used for the accelerometer.

      * @param period Specifies the sampling time for doing calculations. This

      *        period is used to integrate acceleration into speed and distance

      *        travelled.

      */

     TrcAccel(

         __in UINT32 slot,

         __in float  period = 0.02

         ): ADXL345_I2C(slot)

     {

         TLevel(INIT);

         TEnterMsg(("slot=%d,period=%f", slot, period));

         m_semaphore = semBCreate(SEM_Q_PRIORITY, SEM_FULL);

         m_notifier = new Notifier(TrcAccel::CallIntegrator, this);

         m_accelData.XAxis = 0.0;

         m_accelData.YAxis = 0.0;

         m_accelData.ZAxis = 0.0;

         m_offsetZeroG.XAxis = 0.0;

         m_offsetZeroG.YAxis = 0.0;

         m_offsetZeroG.ZAxis = 0.0;

         m_velX = 0.0;

         m_velY = 0.0;

         m_velZ = 0.0;

         m_distX = 0.0;

         m_distY = 0.0;

         m_distZ = 0.0;

         m_fEnabled = false;

         m_notifier->StartPeriodic(period);

         TExit();

     }   //TrcAccel

     /**

      * Destructor: Destroy an instance of the TrcAccel object.

      */

     ~TrcAccel(

         void

         )

     {

         TLevel(INIT);

         TEnter();

         semFlush(m_semaphore);

         SAFE_DELETE(m_notifier);

         TExit();

     }   //~TrcAccel

     /**

      * This function returns the current acceleration value of the X axis in

      * the unit of meters per second square.

      *

      * @return Returns the X acceleration value.

      */

     double

     GetMetricAccelX(

         void

         )

     {

         TLevel(HIFREQ);

         TEnter();

         TExitMsg(("=%f", m_accelData.XAxis));

         return m_accelData.XAxis;

     }   //GetMetricAccelX

     /**

      * This function returns the current acceleration value of the X axis in

      * the unit of feet per second square.

      *

      * @return Returns the X acceleration value.

      */

     double

     GetAccelX(

         void

         )

     {

         TLevel(HIFREQ);

         TEnter();

         double value = m_accelData.XAxis*FEET_PER_METER;

         TExitMsg(("=%f", value));

         return value;

     }   //GetAccelX

     /**

      * This function returns the current acceleration value of the Y axis in

      * the unit of meters per second square.

      *

      * @return Returns the Y acceleration value.

      */

     double

     GetMetricAccelY(

         void

         )

     {

         TLevel(HIFREQ);

         TEnter();

         TExitMsg(("=%f", m_accelData.YAxis));

         return m_accelData.YAxis;

     }   //GetMetricAccelY

     /**

      * This function returns the current acceleration value of the Y axis in

      * the unit of feet per second square.

      *

      * @return Returns the Y acceleration value.

      */

     double

     GetAccelY(

         void

         )

     {

         TLevel(HIFREQ);

         TEnter();

         double value = m_accelData.YAxis*FEET_PER_METER;

         TExitMsg(("=%f", value));

         return value;

     }   //GetAccelY

     /**

      * This function returns the current acceleration value of the Z axis in

      * the unit of meters per second square.

      *

      * @return Returns the Z acceleration value.

      */

     double

     GetMetricAccelZ(

         void

         )

     {

         TLevel(HIFREQ);

         TEnter();

         TExitMsg(("=%f", m_accelData.ZAxis));

         return m_accelData.ZAxis;

     }   //GetMetricAccelZ

     /**

      * This function returns the current acceleration value of the Z axis in

      * the unit of feet per second square.

      *

      * @return Returns the Z acceleration value.

      */

     double

     GetAccelZ(

         void

         )

     {

         TLevel(HIFREQ);

         TEnter();

         double value = m_accelData.ZAxis*FEET_PER_METER;

         TExitMsg(("=%f", value));

         return value;

     }   //GetAccelZ

     /**

      * This function returns the current velocity value of the X axis in

      * the unit of meters per second.

      *

      * @return Returns the X velocity value.

      */

     double

     GetMetricVelX(

         void

         )

     {

         TLevel(HIFREQ);

         TEnter();

         TExitMsg(("=%f", m_velX));

         return m_velX;

     }   //GetMetricVelX

     /**

      * This function returns the current velocity value of the X axis in

      * the unit of feet per second.

      *

      * @return Returns the X velocity value.

      */

     double

     GetVelX(

         void

         )

     {

         TLevel(HIFREQ);

         TEnter();

         double value = m_velX*FEET_PER_METER;

         TExitMsg(("=%f", value));

         return value;

     }   //GetVelX

     /**

      * This function returns the current velocity value of the Y axis in

      * the unit of meters per second.

      *

      * @return Returns the Y velocity value.

      */

     double

     GetMetricVelY(

         void

         )

     {

         TLevel(HIFREQ);

         TEnter();

         TExitMsg(("=%f", m_velY));

         return m_velY;

     }   //GetMetricVelY

     /**

      * This function returns the current velocity value of the Y axis in

      * the unit of feet per second.

      *

      * @return Returns the Y velocity value.

      */

     double

     GetVelY(

         void

         )

     {

         TLevel(HIFREQ);

         TEnter();

         double value = m_velY*FEET_PER_METER;

         TExitMsg(("=%f", value));

         return value;

     }   //GetVelY

     /**

      * This function returns the current velocity value of the Z axis in

      * the unit of meters per second.

      *

      * @return Returns the Z velocity value.

      */

     double

     GetMetricVelZ(

         void

         )

     {

         TLevel(HIFREQ);

         TEnter();

         TExitMsg(("=%f", m_velZ));

         return m_velZ;

     }   //GetMetricVelZ

     /**

      * This function returns the current velocity value of the Z axis in

      * the unit of feet per second.

      *

      * @return Returns the Z velocity value.

      */

     double

     GetVelZ(

         void

         )

     {

         TLevel(HIFREQ);

         TEnter();

         double value = m_velZ*FEET_PER_METER;

         TExitMsg(("=%f", value));

         return value;

     }   //GetVelZ

     /**

      * This function returns the current distance value of the X axis in

      * the unit of meters.

      *

      * @return Returns the X distance value.

      */

     double

     GetMetricDistX(

         void

         )

     {

         TLevel(HIFREQ);

         TEnter();

         TExitMsg(("=%f", m_distX));

         return m_distX;

     }   //GetMetricDistX

     /**

      * This function returns the current distance value of the X axis in

      * the unit of feet.

      *

      * @return Returns the X distance value.

      */

     double

     GetDistX(

         void

         )

     {

         TLevel(HIFREQ);

         TEnter();

         double value = m_distX*FEET_PER_METER;

         TExitMsg(("=%f", value));

         return value;

     }   //GetDistX

     /**

      * This function returns the current distance value of the Y axis in

      * the unit of meters.

      *

      * @return Returns the Y distance value.

      */

     double

     GetMetricDistY(

         void

         )

     {

         TLevel(HIFREQ);

         TEnter();

         TExitMsg(("=%f", m_distY));

         return m_distY;

     }   //GetMetricDistY

     /**

      * This function returns the current distance value of the Y axis in

      * the unit of feet.

      *

      * @return Returns the Y distance value.

      */

     double

     GetDistY(

         void

         )

     {

         TLevel(HIFREQ);

         TEnter();

         double value = m_distY*FEET_PER_METER;

         TExitMsg(("=%f", value));

         return value;

     }   //GetDistY

     /**

      * This function returns the current distance value of the Z axis in

      * the unit of meters.

      *

      * @return Returns the Z distance value.

      */

     double

     GetMetricDistZ(

         void

         )

     {

         TLevel(HIFREQ);

         TEnter();

         TExitMsg(("=%f", m_distZ));

         return m_distZ;

     }   //GetMetricDistZ

     /**

      * This function returns the current distance value of the Z axis in

      * the unit of feet.

      *

      * @return Returns the Z distance value.

      */

     double

     GetDistZ(

         void

         )

     {

         TLevel(HIFREQ);

         TEnter();

         double value = m_distZ*FEET_PER_METER;

         TExitMsg(("=%f", value));

         return value;

     }   //GetDistZ

     /**

      * This function resets the acceleration, velocity and distance values.

      */

     void

     Reset(

         void

         )

     {

         TLevel(API);

         TEnter();

         CRITICAL_REGION(m_semaphore)

         {

             m_accelData.XAxis = 0.0;

             m_accelData.YAxis = 0.0;

             m_accelData.ZAxis = 0.0;

             m_velX = 0.0;

             m_velY = 0.0;

             m_velZ = 0.0;

             m_distX = 0.0;

             m_distY = 0.0;

             m_distZ = 0.0;

         }

         END_REGION;

         TExit();

     }   //Reset

     /**

      * This function sets the accelerometer to enable or disable state.

      *

      * @param fEnabled If true, enables the accelerometer, false otherwise.

      */

     void

     SetEnabled(

         __in bool fEnabled

         )

     {

         TLevel(API);

         TEnter();

         m_fEnabled = fEnabled;

         if (fEnabled)

         {

             Reset();

             m_timestamp = GetMsecTime();

         }

         TExit();

     }   //SetEnabled

     /**

      * This function is called to calibrate the zero G point. It assumes the

      * accelerometer is sitting still at level ground during the calibration.

      * It samples a number of points on all axes and averages them to be the

      * zero G point for the axes. Note that the Z-axis is not really at zero G

      * when it is still. Z-axis should be 1G at level ground. But for the

      * purpose of measuring relative G's on all axes, we calibrate zero G for

      * all axes.

      */

     void

     CalibrateZeroG(

         void

         )

     {

         AllAxes data;

         AllAxes zeroG;

         TLevel(INIT);

         TEnter();

         zeroG.XAxis = 0.0;

         zeroG.YAxis = 0.0;

         zeroG.YAxis = 0.0;

         for (UINT32 i = 0; i < ACCEL_NUM_CAL_PTS; i++)

         {

             data = GetAccelerations();

             zeroG.XAxis += data.XAxis;

             zeroG.YAxis += data.YAxis;

             zeroG.ZAxis += data.ZAxis;

             Wait(ACCEL_CAL_INTERVAL);

         }

         CRITICAL_REGION(m_semaphore)

         {

             m_offsetZeroG.XAxis = zeroG.XAxis/ACCEL_NUM_CAL_PTS;

             m_offsetZeroG.YAxis = zeroG.YAxis/ACCEL_NUM_CAL_PTS;

             m_offsetZeroG.ZAxis = zeroG.ZAxis/ACCEL_NUM_CAL_PTS;

             TInfo(("AccelZeroG:x=%f,y=%f,z=%f",

                    m_offsetZeroG.XAxis, m_offsetZeroG.YAxis,

                    m_offsetZeroG.ZAxis));

         }

         END_REGION;

         Reset();

         TExit();

     }   //CalibrateZeroG

 };  //class TrcAccel

 #endif  //ifndef _TRCACCEL_H