main.cpp

00001 #include "ADXL345_I2C.h"
00002 
00003  ADXL345_I2C accelerometer(I2C_SDA, I2C_SCL);
00004  Serial pc(USBTX, USBRX);
00005  
00006  class RangeMapper
00007  {
00008      /*
00009      RangeMapper convert data from input range to output range
00010      Math formular is y_output = ratio * x_input + offset;
00011      */
00012      protected: 
00013      double _ratio;
00014      double _offset;
00015      double _outrange;
00016      public:
00017      RangeMapper(double x_min, double x_max, double y_min, double y_max)
00018       {
00019           _ratio = (y_max - y_min) / (x_max - x_min);
00020           _offset = y_min - x_min * _ratio;
00021           _outrange = y_max - y_min; 
00022       };
00023      ~RangeMapper(){};
00024      double getOutput(double x_input)
00025         {return x_input * _ratio + _offset;};
00026      void zoom(float ratio)
00027         {_ratio *=  ratio;};
00028      void shift(float ratio)
00029         {_offset +=  _outrange * ratio ; } ;
00030       
00031  };
00032 
00033  
00034  class DataProcess  
00035  {
00036      protected: 
00037      // Statistics
00038      int64_t _summary; 
00039      double _squardsum; // summary (xi^2)
00040      int64_t _numbers;
00041      int64_t _max;
00042      int64_t _min;
00043      // integration, assume time interval is same 
00044      int64_t _itg1; //1 order integration
00045      int64_t _itg2; //2 order integration
00046      int64_t _itg3; //3 order integration
00047      // differentiation, assume time interval is same
00048      int64_t _preData;  
00049      int64_t _dif1; //1 order differentiation
00050      int64_t _dif2; //2 order differentiation
00051      public:
00052      DataProcess(): 
00053         _summary(0), _squardsum(0), _numbers(0), _max(0), _min(0),  
00054         _itg1(0), _itg2(0), _itg3(0), _preData(0), _dif1(0), _dif2(0) {};
00055      ~DataProcess() {}; 
00056      void putData(int64_t x) {
00057          int64_t temp;
00058          if (x > _max) _max = x;
00059          if (x < _min) _min = x;
00060          _summary += x;
00061          _squardsum += x*x;
00062          _numbers ++;
00063          _itg1 += x - _summary / _numbers ;
00064          _itg2 += _itg1;
00065          _itg3 += _itg2;
00066          temp = _dif1;
00067          _dif1 = x - _preData;
00068          _preData = x;
00069          _dif2 = _dif1 - temp; 
00070          };
00071      int64_t getSum(void) {return _summary;};
00072      double getMean(void) {return (double)_summary / (double)_numbers ;};
00073      double getStdDiv(void) {return sqrt((_squardsum - (double)_summary*_summary / (double)_numbers ) / (double)_numbers ); };
00074      int64_t getMax(void) {return _max; };
00075      int64_t getMin(void) {return _min; };
00076      int64_t getCount(void) {return _numbers;} ;
00077      int64_t getO1integration(void) {return _itg1;};
00078      int64_t getO2integration(void) {return _itg2;};
00079      int64_t getO3integration(void) {return _itg3;};
00080      int64_t GetO1differ(void) {return _dif1;};
00081      int64_t GetO2differ(void) {return _dif2;};
00082  };
00083  
00084  int main() {
00085      char getc =0;
00086      int channel = 0, datamode =0;
00087      bool output = true, mapout = true;
00088      pc.baud(115200);
00089      int readings[3] = {0, 0, 0};
00090      DataProcess dp1, dp2, dp3;
00091      DataProcess dp[3] = {dp1, dp2, dp3};
00092  
00093       
00094    
00095      pc.printf("Starting ADXL345 test...\r\n");
00096      wait(.001);
00097      pc.printf("Device ID is: 0x%02x\r\n", accelerometer.getDeviceID());
00098     wait(.001);
00099     
00100     restart:
00101     
00102      // These are here to test whether any of the initialization fails. It will print the failure
00103     if (accelerometer.setPowerControl(0x00)){
00104          pc.printf("didn't intitialize power control\r\n"); 
00105          return 0;  }
00106      wait(.001);
00107      
00108      //Full resolution, +/-16g, 4mg/LSB.
00109      if(accelerometer.setDataFormatControl(0x0B)){
00110      //Full resolution, +/-2g, 4mg/LSB.
00111      //if(accelerometer.setDataFormatControl(0x08)){
00112         pc.printf("didn't set data format\r\n");
00113         return 0;  }
00114      wait(.001);
00115      
00116      //3.2kHz data rate.
00117      if(accelerometer.setDataRate(ADXL345_3200HZ)){
00118         pc.printf("didn't set data rate\r\n");
00119         return 0;    }
00120      wait(.001);
00121      
00122      //Measurement mode.
00123      
00124      if(accelerometer.setPowerControl(MeasurementMode)) {
00125         pc.printf("didn't set the power control to measurement\r\n"); 
00126         return 0;   } 
00127 
00128      // pc.printf("x-axis, y-axis, z-axis\r\n"); 
00129      pc.printf("Data Commands: \r\n"); 
00130      pc.printf("  x -> x channdel \r\n"); 
00131      pc.printf("  y -> y channdel \r\n"); 
00132      pc.printf("  z -> z channdel \r\n"); 
00133      pc.printf("  o,0 -> data output on/off \r\n"); 
00134      pc.printf("  i,1 -> mapper on/off \r\n"); 
00135      pc.printf("  a -> raw data \r\n");
00136      pc.printf("  b -> mean \r\n");
00137      pc.printf("  c -> summary \r\n");
00138      pc.printf("  d -> variance \r\n");
00139      pc.printf("  e -> 1st order Integration \r\n");
00140      pc.printf("  f -> 2nd order Integration \r\n");
00141      pc.printf("  g -> 3rd order Integration \r\n");
00142      pc.printf("  h -> 1st order Differentiation \r\n");
00143      pc.printf("  k -> 2nd order Differentiation \r\n");
00144      pc.printf("  +,= -> Output Zoom In 2X \r\n");
00145      pc.printf("  - -> Output Zoom Out 2Z \r\n");
00146      pc.printf("  ^,6 -> shift up 1% \r\n");
00147      pc.printf("  v -> shift down 1% \r\n");
00148      pc.printf("Press any key to start. \r\n");
00149      getc = pc.getc();  
00150      
00151     int error_count=0;
00152     RangeMapper DAC_Mapper1( 0 - 1<<13, 1<<13, 0, 1);
00153     RangeMapper DAC_Mapper2( 0 - 1<<13, 1<<13, 0, 1);
00154  
00155      while (1) {
00156          if (pc.readable())
00157             {
00158                 getc = pc.getc();
00159                 switch( getc)
00160                     {
00161                         case 'x' : channel = 0 ; pc.printf("Data procesing output switch to X channdel \r\n"); break;
00162                         case 'y' : channel = 1 ; pc.printf("Data procesing output switch to Y channdel \r\n"); break;
00163                         case 'z' : channel = 2 ; pc.printf("Data procesing output switch to Z channdel \r\n"); break;
00164                         case '0' : 
00165                         case 'o' : output = !output ; pc.printf("Turn %s data output.\r\n",  (output ? "On" : "Off") ); break;
00166                         case '1' : 
00167                         case 'i' : mapout = !mapout ; pc.printf("Turn %s mapping output.\r\n",  (mapout ? "On" : "Off") ); break;
00168                         case 'a' : datamode = 0 ; pc.printf("Set to raw data output.\r\n"); break;
00169                         case 'b' : datamode = 1 ; pc.printf("Set to mean output.\r\n"); break;
00170                         case 'c' : datamode = 2 ; pc.printf("Set to summary output.\r\n"); break;
00171                         case 'd' : datamode = 3 ; pc.printf("Set to variance output.\r\n"); break;
00172                         case 'e' : datamode = 4 ; pc.printf("Set to 1st order Integration output.\r\n"); break;
00173                         case 'f' : datamode = 5 ; pc.printf("Set to 2nd order Integration output.\r\n"); break;
00174                         case 'g' : datamode = 6 ; pc.printf("Set to 3rd order Integration output.\r\n"); break;
00175                         case 'h' : datamode = 7 ; pc.printf("Set to 1st order Differentiation output.\r\n"); break;
00176                         case 'k' : datamode = 8 ; pc.printf("Set to 2nd order Differentiation output.\r\n"); break;
00177                         case '=' :
00178                         case '+' : DAC_Mapper2.zoom(2) ; pc.printf("Mapout zoom in.\r\n"); break;
00179                         case '-' : DAC_Mapper2.zoom(0.5) ; pc.printf("Mapout zoom out.\r\n"); break;
00180                         case '6' :
00181                         case '^' : DAC_Mapper2.shift(0.01) ; pc.printf("Mapout shift up.\r\n"); break;
00182                         case 'v' : DAC_Mapper2.shift(-0.01) ; pc.printf("Mapout shift down.\r\n"); break;
00183                         default: break;
00184                     }
00185             }
00186          accelerometer.getOutput(readings);
00187          if (( 17601 == readings[0] ) ||  ( 17601 == readings[1] ) || ( 17601 == readings[2] ))
00188             {
00189             error_count++;
00190             if (error_count>10)
00191                 {
00192                 accelerometer.setPowerControl(0);
00193                 pc.printf("Sensor Halt!\r\n"); 
00194                 goto restart;
00195                 }
00196             }
00197         else
00198             {
00199             error_count = 0; 
00200             int i;
00201             for (i = 0; i<3; i++)
00202                 dp[i].putData(readings[i] = (int16_t) readings[i] );
00203                 
00204             if (output)
00205              {
00206                switch (datamode)
00207                {
00208                 case 0: pc.printf("RAW: %i, %i, %i\r\n", (int16_t)readings[0], (int16_t)readings[1], (int16_t)readings[2]); break;
00209                 //case 0: pc.printf("RAW: %i, %i, %i\r\n", readings[0], readings[1], readings[2]); break;
00210                 case 1: pc.printf("MEAN: %f, %f, %f\r\n", dp[0].getMean(), dp[1].getMean(), dp[2].getMean() ); break;
00211                 case 2: pc.printf("SUM: %jd, %jd, %jd\r\n", dp[0].getSum(), dp[1].getSum(), dp[2].getSum() ); break;                
00212                 case 3: pc.printf("STD: %f, %f, %f\r\n", dp[0].getStdDiv(), dp[1].getStdDiv(), dp[2].getStdDiv() ); break;
00213                 case 4: pc.printf("1ITG: %jd, %jd, %jd\r\n", dp[0].getO1integration(), dp[1].getO1integration(), dp[2].getO1integration() ); break;     
00214                 case 5: pc.printf("2ITG: %jd, %jd, %jd\r\n", dp[0].getO2integration(), dp[1].getO2integration(), dp[2].getO2integration() ); break;     
00215                 case 6: pc.printf("3ITG: %jd, %jd, %jd\r\n", dp[0].getO3integration(), dp[1].getO3integration(), dp[2].getO3integration() ); break;     
00216                 case 7: pc.printf("1DIF: %jd, %jd, %jd\r\n", dp[0].GetO1differ(), dp[1].GetO1differ(), dp[2].GetO1differ() ); break;     
00217                 case 8: pc.printf("2DIF: %jd, %jd, %jd\r\n", dp[0].GetO2differ(), dp[1].GetO2differ(), dp[2].GetO2differ() ); break;     
00218                 default: break;         
00219                }
00220              }
00221              // Set data the DAC
00222              DAC_Mapper1.getOutput(readings[channel]);
00223              if (mapout)
00224              {
00225                double mapdata;
00226                switch (datamode)
00227                {
00228                 case 0: mapdata = readings[channel];  break;
00229                 case 1: mapdata = dp[channel].getMean(); break;
00230                 case 2: mapdata = dp[channel].getSum();  break;                
00231                 case 3: mapdata = dp[channel].getStdDiv();  break;
00232                 case 4: mapdata = dp[channel].getO1integration(); break;     
00233                 case 5: mapdata = dp[channel].getO2integration();  break;     
00234                 case 6: mapdata = dp[channel].getO3integration();  break;     
00235                 case 7: mapdata = dp[channel].GetO1differ(); break;     
00236                 case 8: mapdata = dp[channel].GetO2differ(); break;     
00237                 default: mapdata = 0; break;         
00238                }
00239                pc.printf("MAPPING OUTPUT: %f\r\n", DAC_Mapper2.getOutput(mapdata) );
00240                
00241              }
00242              else
00243              {
00244                DAC_Mapper2.getOutput(0);
00245              }
00246 
00247              wait(1);
00248             }
00249 
00250      }
00251  
00252  }