Sine and Cosine in C++

[mikets]

We used the accelerometer to determine the distance travelled by doing a double integration on acceleration. We wrote a generic accelerometer module that will give you accel/vel/distance on all three axes. Then we defined a macro called MAGNITUDE that will take the x and y vectors and gives you distance magnitude and also the macro DIR_DEGREE to calculate the heading.

#define MAGNITUDE(x,y)          sqrt(pow(x, 2) + pow(y, 2))
#define RADIANS_TO_DEGREES(n)   ((n)*180.0/PI)
// Forward is 0-radian
#define DIR_RADIANS(x,y)        (((x == 0.0) && (y == 0.0))? 0.0: atan2(x, y))
#define DIR_DEGREES(x,y)        RADIANS_TO_DEGREES(DIR_RADIANS(x, y))

Our accelerometer code can be found at
http://proj.titanrobotics.net/hg/Frc...inc/TrcAccel.h

But be careful when using the double integrator code. The accelerometer has noise, so even if the robot sits still, you may end up with an increasing velocity if you sit long enough. Our code doesn't enable the accelerometer integration until right before we need it. And it clears the integrator right before we enable it so it clears the cumulated error. It also does zero-G calibration at the beginning initialization phase to eliminate the DC offset of the accelerometer. Also be mindful on situation that if your robot gets bumped or accelerates/decelerates too fast, it could saturate the accelerometer output so to give you error on your integraton. With all those limitations in mind, it will give you decent results for a short period of time.