turn_sensor.cpp

00001 #include "turn_sensor.h"
00002 #include "l3g.h"
00003 
00004 void TurnSensor::reset()
00005 {
00006     angleUnsigned = 0;
00007 }
00008 
00009 void TurnSensor::start()
00010 {
00011     timer.start(); 
00012     rate = 0;  
00013     angleUnsigned = 0;
00014     gyroLastUpdate = timer.read_us();
00015 }
00016 
00017 void TurnSensor::update()
00018 {
00019     if (l3gZAvailable() == 1)
00020     {
00021         int32_t gz = l3gZReadCalibrated();
00022         if (gz < -500000)
00023         {
00024             // error
00025             return;
00026         }        
00027 
00028         // The gyro on this robot is mounted upside down; account for that here so that
00029         // we can have counter-clockwise be a positive rotation.
00030         gz = -gz;        
00031        
00032         rate = gz;
00033         
00034         // First figure out how much time has passed since the last update (dt)
00035         uint16_t m = timer.read_us();
00036         uint16_t dt = m - gyroLastUpdate;
00037         gyroLastUpdate = m;
00038 
00039         // Multiply dt by turnRate in order to get an estimation of how
00040         // much the robot has turned since the last update.
00041         // (angular change = angular velocity * time)
00042         int32_t d = (int32_t)rate * dt;
00043 
00044         // The units of d are gyro digits times microseconds.  We need
00045         // to convert those to the units of turnAngle, where 2^29 units
00046         // represents 45 degrees.  The conversion from gyro digits to
00047         // degrees per second (dps) is determined by the sensitivity of
00048         // the gyro: 0.07 degrees per second per digit.
00049         //
00050         // (0.07 dps/digit) * (1/1000000 s/us) * (2^29/45 unit/degree)
00051         // = 14680064/17578125 unit/(digit*us)
00052         
00053         if (rate > 0)
00054         {
00055           // Counter-clockwise scale factors from
00056           // log_190730_2.csv and log_190801_4.csv.
00057           const double scale = 1.012394298 * 0.998207092;
00058           angleUnsigned += (int64_t)d * (int32_t)(14680064 * scale) / 17578125;
00059         }
00060         else
00061         {
00062           // Clockwise scale factor calculated from
00063           // log_190730_3.csv and then log_190801_3.csv.
00064           const double scale = 0.992376154 * 0.999892525;
00065           angleUnsigned += (int64_t)d * (int32_t)(14680064 * scale) / 17578125;
00066         }
00067     }
00068 }