Gyroscope.cpp

00001 #include "Gyroscope.h"
00002 
00003 Gyroscope::Gyroscope(L3G4200D* gyroHardware, float sensitivity, float dataRate)
00004 {
00005     this->gyroHardware = gyroHardware;
00006     this->sensitivity = sensitivity;
00007     this->dataRate = dataRate;
00008     
00009     updateZeroRates();
00010 }
00011 
00012 Gyroscope::~Gyroscope(void)
00013 {
00014 }
00015 
00016 void Gyroscope::updateZeroRates()
00017 {
00018     update(100, 10, true);
00019     
00020     zeroRateX = rawX;
00021     zeroRateY = rawY;
00022     zeroRateZ = rawZ;
00023     
00024     resetAngles();
00025 }
00026 
00027 void Gyroscope::update()
00028 {    
00029     update(1, 1, false);
00030 }
00031 
00032 void Gyroscope::update(int samplesSize, int sampleDataRate, bool iswait)
00033 {
00034     int axes[3] = {0, 0, 0};
00035     
00036     rawX = 0;
00037     rawY = 0;
00038     rawZ = 0;
00039     
00040     for (int i = 0; i < samplesSize; i++)
00041     {
00042         gyroHardware->getAxes(axes);
00043         
00044        //O valor do ângulo real para o ângulo medido está com uma diferença de 2.4 vezes maior.
00045        rawX += ((gyroHardware->getX() * 2.55) / samplesSize);
00046        rawY += ((gyroHardware->getY() * 2.55) / samplesSize);
00047        rawZ += ((gyroHardware->getZ() * 2.55) / samplesSize);
00048         
00049         if(iswait)
00050             wait_ms(sampleDataRate);
00051     }
00052     
00053     angleX += getDegreesX() * dataRate;
00054     angleY += getDegreesY() * dataRate;
00055     angleZ += getDegreesZ() * dataRate;
00056 }
00057 
00058 void Gyroscope::resetAngles()
00059 {
00060     angleX = 0;
00061     angleY = 0;
00062     angleZ = 0;
00063 }
00064 
00065 float Gyroscope::getDegreesX()
00066 {
00067     return (rawX - zeroRateX) * sensitivity;
00068 }
00069 
00070 float Gyroscope::getDegreesY()
00071 {
00072     return (rawY - zeroRateY) * sensitivity;
00073 }
00074 
00075 float Gyroscope::getDegreesZ()
00076 {
00077     return (rawZ - zeroRateZ) * sensitivity;
00078 }
00079 
00080 float Gyroscope::getRadiansX()
00081 {
00082     return getDegreesX() / 180.0 * PI;
00083 }
00084 
00085 float Gyroscope::getRadiansY()
00086 {
00087     return getDegreesY() / 180.0 * PI;
00088 }
00089 
00090 float Gyroscope::getRadiansZ()
00091 {
00092     return getDegreesZ() / 180.0 * PI;
00093 }
00094 
00095 float Gyroscope::getAngleX()
00096 {
00097     return angleX;
00098 }
00099 
00100 float Gyroscope::getAngleY()
00101 {
00102     return angleY;
00103 }
00104 
00105 float Gyroscope::getAngleZ()
00106 {
00107     return angleZ;
00108 }