Accelerometre.cpp

00001 #include "Accelerometre.h"
00002 
00003 Accelerometre::Accelerometre(void)
00004 {
00005     position =0;
00006     _acc =NULL; 
00007     for (i=0; i<3;i++){
00008         readings[i] = 0 ;
00009     }
00010 }
00011 
00012 
00013 Accelerometre::Accelerometre(ADXL345_I2C * acc)
00014 {
00015     position =0;
00016        for (i=0; i<3;i++){
00017         readings[i] = 0 ;
00018     }
00019     if (_acc) delete _acc; // destruction de l'objet accelerometre 
00020     _acc =acc;  
00021     
00022 }
00023 
00024 
00025 
00026 
00027 Accelerometre ::~Accelerometre(void){
00028    if (_acc) delete _acc; // destruction de l'objet accelerometre 
00029    if (readings) delete readings;
00030 
00031 }
00032 
00033 
00034 int Accelerometre::initAcc(void){
00035     if (_acc) {
00036             
00037             // These are here to test whether any of the initialization fails. It will print the failure
00038         if (_acc->setPowerControl(0x00)){
00039              //pc.printf("didn't intitialize power control\n\r"); 
00040              return 1;  }
00041          //Full resolution, +/-16g, 4mg/LSB.
00042          wait(.001);
00043          
00044          if(_acc->setDataFormatControl(0x0B)){
00045             //pc.printf("didn't set data format\n\r");
00046             return 2;  }
00047          wait(.001);
00048          
00049          //3.2kHz data rate.
00050          if(_acc->setDataRate(ADXL345_3200HZ)){
00051             //pc.printf("didn't set data rate\n\r");
00052             return 3;    }
00053          wait(.001);
00054          
00055          //Measurement mode.
00056         if(_acc->setPowerControl(MeasurementMode)) {
00057         //pc.printf("didn't set the power control to measurement\n\r"); 
00058         return 4;   }
00059          
00060          _acc->getOutput(readingPrec);
00061          
00062           for(i=0;i<3;i++){
00063                 if ((int16_t)readingPrec[i]<0){
00064                     readingPrecABS[i] = readingPrec[i]*(-1);
00065                 }
00066                 else{
00067                     
00068                     readingPrecABS[i] = readingPrec[i];
00069                 } 
00070                   
00071             }
00072          return 10;
00073         }
00074         return 0;      
00075 }
00076 
00077 void Accelerometre::calculeABS(){ 
00078     for(i=0;i<3;i++){
00079         if ((int16_t)readings[i]<0){
00080             readingsABS[i] = readings[i]*(-1);
00081         }
00082         else{ 
00083             readingsABS[i] = readings[i];
00084         } 
00085           
00086     }
00087 }
00088 
00089 
00090 
00091 
00092 
00093 char Accelerometre::getPosition(void){
00094         _acc->getOutput(readings);
00095         
00096         calculeABS();
00097         
00098        
00099             
00100             if(((int16_t)readings[0]>150)&&((int16_t)readings[1]<150)&&((int16_t)readings[1]>-150)){ //bas
00101                 position = 1;
00102             }   
00103             else {if(((int16_t)readings[0]<-100)&&((int16_t)readings[1]<150)&&((int16_t)readings[1]>-150)){//haut
00104                     position = 2;
00105                 }
00106                 else {if(((int16_t)readings[1]>150)&&((int16_t)readings[0]<150)&&((int16_t)readings[0]>-100)){//gauche
00107                         position = 3;
00108                     }
00109                     else {if(((int16_t)readings[1]<-150)&&((int16_t)readings[0]<150)&&((int16_t)readings[0]>-100)){//droite
00110                             position = 4;
00111                         }
00112                         else{position=0;} // attente
00113                     }
00114                 }
00115             }           
00116        
00117         
00118         for (i=0;i<3;i++){
00119             readingPrecABS[i] = readingsABS[i];
00120             readingPrec[i] = readings[i];
00121         }
00122         
00123         
00124         return position;
00125         
00126 }
00127 
00128 void Accelerometre::getXYZ(int* posXYZ){
00129         _acc->getOutput(readings);    
00130         posXYZ[0] = readings[0];
00131         posXYZ[1] = readings[1];
00132         posXYZ[2] = readings[2];
00133     }
00134