jack zen / Mbed 2 deprecated ADXL345Test_for_motor_test

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed

Fork of ADXL345Test_for_motortest by jack zen

main.cpp@7:592373200317, 2017-09-01 (annotated)

Committer:
Frederick_H
Date:
Fri Sep 01 10:53:50 2017 +0000
Revision:
7:592373200317
Parent:
6:88787a48e96e 
Add zoom and shift function on Mapper

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Frederick_H 3:e4783c57bcc0 1 #include "ADXL345_I2C.h"
screamer 0:51be560629b8 2
Frederick_H 3:e4783c57bcc0 3 ADXL345_I2C accelerometer(I2C_SDA, I2C_SCL);
Frederick_H 3:e4783c57bcc0 4 Serial pc(USBTX, USBRX);
Frederick_H 4:f5a78245f2d0 5
Frederick_H 4:f5a78245f2d0 6 class RangeMapper
Frederick_H 4:f5a78245f2d0 7 {
Frederick_H 4:f5a78245f2d0 8 /*
Frederick_H 4:f5a78245f2d0 9 RangeMapper convert data from input range to output range
Frederick_H 4:f5a78245f2d0 10 Math formular is y_output = ratio * x_input + offset;
Frederick_H 4:f5a78245f2d0 11 */
Frederick_H 4:f5a78245f2d0 12 protected:
Frederick_H 4:f5a78245f2d0 13 double _ratio;
Frederick_H 4:f5a78245f2d0 14 double _offset;
Frederick_H 7:592373200317 15 double _outrange;
Frederick_H 4:f5a78245f2d0 16 public:
Frederick_H 4:f5a78245f2d0 17 RangeMapper(double x_min, double x_max, double y_min, double y_max)
Frederick_H 4:f5a78245f2d0 18 {
Frederick_H 4:f5a78245f2d0 19 _ratio = (y_max - y_min) / (x_max - x_min);
Frederick_H 7:592373200317 20 _offset = y_min - x_min * _ratio;
Frederick_H 7:592373200317 21 _outrange = y_max - y_min;
Frederick_H 4:f5a78245f2d0 22 };
Frederick_H 4:f5a78245f2d0 23 ~RangeMapper(){};
Frederick_H 4:f5a78245f2d0 24 double getOutput(double x_input)
Frederick_H 4:f5a78245f2d0 25 {return x_input * _ratio + _offset;};
Frederick_H 7:592373200317 26 void zoom(float ratio)
Frederick_H 7:592373200317 27 {_ratio *= ratio;};
Frederick_H 7:592373200317 28 void shift(float ratio)
Frederick_H 7:592373200317 29 {_offset += _outrange * ratio ; } ;
Frederick_H 7:592373200317 30
Frederick_H 4:f5a78245f2d0 31 };
screamer 0:51be560629b8 32
Frederick_H 4:f5a78245f2d0 33
Frederick_H 4:f5a78245f2d0 34 class DataProcess
Frederick_H 4:f5a78245f2d0 35 {
Frederick_H 4:f5a78245f2d0 36 protected:
Frederick_H 4:f5a78245f2d0 37 // Statistics
Frederick_H 4:f5a78245f2d0 38 int64_t _summary;
Frederick_H 4:f5a78245f2d0 39 double _squardsum; // summary (xi^2)
Frederick_H 4:f5a78245f2d0 40 int64_t _numbers;
Frederick_H 4:f5a78245f2d0 41 int64_t _max;
Frederick_H 4:f5a78245f2d0 42 int64_t _min;
Frederick_H 4:f5a78245f2d0 43 // integration, assume time interval is same
Frederick_H 4:f5a78245f2d0 44 int64_t _itg1; //1 order integration
Frederick_H 4:f5a78245f2d0 45 int64_t _itg2; //2 order integration
Frederick_H 4:f5a78245f2d0 46 int64_t _itg3; //3 order integration
Frederick_H 4:f5a78245f2d0 47 // differentiation, assume time interval is same
Frederick_H 4:f5a78245f2d0 48 int64_t _preData;
Frederick_H 4:f5a78245f2d0 49 int64_t _dif1; //1 order differentiation
Frederick_H 4:f5a78245f2d0 50 int64_t _dif2; //2 order differentiation
Frederick_H 4:f5a78245f2d0 51 public:
Frederick_H 4:f5a78245f2d0 52 DataProcess():
Frederick_H 4:f5a78245f2d0 53 _summary(0), _squardsum(0), _numbers(0), _max(0), _min(0),
Frederick_H 4:f5a78245f2d0 54 _itg1(0), _itg2(0), _itg3(0), _preData(0), _dif1(0), _dif2(0) {};
Frederick_H 4:f5a78245f2d0 55 ~DataProcess() {};
Frederick_H 4:f5a78245f2d0 56 void putData(int64_t x) {
Frederick_H 4:f5a78245f2d0 57 int64_t temp;
Frederick_H 4:f5a78245f2d0 58 if (x > _max) _max = x;
Frederick_H 4:f5a78245f2d0 59 if (x < _min) _min = x;
Frederick_H 4:f5a78245f2d0 60 _summary += x;
Frederick_H 4:f5a78245f2d0 61 _squardsum += x*x;
Frederick_H 4:f5a78245f2d0 62 _numbers ++;
Frederick_H 6:88787a48e96e 63 _itg1 += x - _summary / _numbers ;
Frederick_H 4:f5a78245f2d0 64 _itg2 += _itg1;
Frederick_H 4:f5a78245f2d0 65 _itg3 += _itg2;
Frederick_H 4:f5a78245f2d0 66 temp = _dif1;
Frederick_H 4:f5a78245f2d0 67 _dif1 = x - _preData;
Frederick_H 4:f5a78245f2d0 68 _preData = x;
Frederick_H 4:f5a78245f2d0 69 _dif2 = _dif1 - temp;
Frederick_H 4:f5a78245f2d0 70 };
Frederick_H 4:f5a78245f2d0 71 int64_t getSum(void) {return _summary;};
Frederick_H 4:f5a78245f2d0 72 double getMean(void) {return (double)_summary / (double)_numbers ;};
Frederick_H 4:f5a78245f2d0 73 double getStdDiv(void) {return sqrt((_squardsum - (double)_summary*_summary / (double)_numbers ) / (double)_numbers ); };
Frederick_H 4:f5a78245f2d0 74 int64_t getMax(void) {return _max; };
Frederick_H 4:f5a78245f2d0 75 int64_t getMin(void) {return _min; };
Frederick_H 4:f5a78245f2d0 76 int64_t getCount(void) {return _numbers;} ;
Frederick_H 4:f5a78245f2d0 77 int64_t getO1integration(void) {return _itg1;};
Frederick_H 4:f5a78245f2d0 78 int64_t getO2integration(void) {return _itg2;};
Frederick_H 4:f5a78245f2d0 79 int64_t getO3integration(void) {return _itg3;};
Frederick_H 4:f5a78245f2d0 80 int64_t GetO1differ(void) {return _dif1;};
Frederick_H 4:f5a78245f2d0 81 int64_t GetO2differ(void) {return _dif2;};
Frederick_H 4:f5a78245f2d0 82 };
Frederick_H 4:f5a78245f2d0 83
Frederick_H 3:e4783c57bcc0 84 int main() {
Frederick_H 4:f5a78245f2d0 85 char getc =0;
Frederick_H 4:f5a78245f2d0 86 int channel = 0, datamode =0;
Frederick_H 6:88787a48e96e 87 bool output = true, mapout = true;
Frederick_H 3:e4783c57bcc0 88 pc.baud(115200);
Frederick_H 3:e4783c57bcc0 89 int readings[3] = {0, 0, 0};
Frederick_H 4:f5a78245f2d0 90 DataProcess dp1, dp2, dp3;
Frederick_H 4:f5a78245f2d0 91 DataProcess dp[3] = {dp1, dp2, dp3};
Frederick_H 4:f5a78245f2d0 92
Frederick_H 4:f5a78245f2d0 93
Frederick_H 4:f5a78245f2d0 94
Frederick_H 3:e4783c57bcc0 95 pc.printf("Starting ADXL345 test...\r\n");
Frederick_H 3:e4783c57bcc0 96 wait(.001);
Frederick_H 3:e4783c57bcc0 97 pc.printf("Device ID is: 0x%02x\r\n", accelerometer.getDeviceID());
Frederick_H 3:e4783c57bcc0 98 wait(.001);
Frederick_H 3:e4783c57bcc0 99
Frederick_H 3:e4783c57bcc0 100 restart:
Frederick_H 3:e4783c57bcc0 101
Frederick_H 3:e4783c57bcc0 102 // These are here to test whether any of the initialization fails. It will print the failure
Frederick_H 3:e4783c57bcc0 103 if (accelerometer.setPowerControl(0x00)){
Frederick_H 3:e4783c57bcc0 104 pc.printf("didn't intitialize power control\r\n");
Frederick_H 3:e4783c57bcc0 105 return 0; }
Frederick_H 3:e4783c57bcc0 106 wait(.001);
Frederick_H 3:e4783c57bcc0 107
Frederick_H 3:e4783c57bcc0 108 //Full resolution, +/-16g, 4mg/LSB.
Frederick_H 3:e4783c57bcc0 109 if(accelerometer.setDataFormatControl(0x0B)){
Frederick_H 3:e4783c57bcc0 110 //Full resolution, +/-2g, 4mg/LSB.
Frederick_H 3:e4783c57bcc0 111 //if(accelerometer.setDataFormatControl(0x08)){
Frederick_H 3:e4783c57bcc0 112 pc.printf("didn't set data format\r\n");
Frederick_H 3:e4783c57bcc0 113 return 0; }
Frederick_H 3:e4783c57bcc0 114 wait(.001);
Frederick_H 3:e4783c57bcc0 115
Frederick_H 3:e4783c57bcc0 116 //3.2kHz data rate.
Frederick_H 3:e4783c57bcc0 117 if(accelerometer.setDataRate(ADXL345_3200HZ)){
Frederick_H 3:e4783c57bcc0 118 pc.printf("didn't set data rate\r\n");
Frederick_H 3:e4783c57bcc0 119 return 0; }
Frederick_H 3:e4783c57bcc0 120 wait(.001);
Frederick_H 3:e4783c57bcc0 121
Frederick_H 3:e4783c57bcc0 122 //Measurement mode.
Frederick_H 3:e4783c57bcc0 123
Frederick_H 3:e4783c57bcc0 124 if(accelerometer.setPowerControl(MeasurementMode)) {
Frederick_H 3:e4783c57bcc0 125 pc.printf("didn't set the power control to measurement\r\n");
Frederick_H 3:e4783c57bcc0 126 return 0; }
Frederick_H 3:e4783c57bcc0 127
Frederick_H 4:f5a78245f2d0 128 // pc.printf("x-axis, y-axis, z-axis\r\n");
Frederick_H 4:f5a78245f2d0 129 pc.printf("Data Commands: \r\n");
Frederick_H 4:f5a78245f2d0 130 pc.printf(" x -> x channdel \r\n");
Frederick_H 4:f5a78245f2d0 131 pc.printf(" y -> y channdel \r\n");
Frederick_H 4:f5a78245f2d0 132 pc.printf(" z -> z channdel \r\n");
Frederick_H 4:f5a78245f2d0 133 pc.printf(" o,0 -> data output on/off \r\n");
Frederick_H 4:f5a78245f2d0 134 pc.printf(" i,1 -> mapper on/off \r\n");
Frederick_H 4:f5a78245f2d0 135 pc.printf(" a -> raw data \r\n");
Frederick_H 4:f5a78245f2d0 136 pc.printf(" b -> mean \r\n");
Frederick_H 4:f5a78245f2d0 137 pc.printf(" c -> summary \r\n");
Frederick_H 4:f5a78245f2d0 138 pc.printf(" d -> variance \r\n");
Frederick_H 4:f5a78245f2d0 139 pc.printf(" e -> 1st order Integration \r\n");
Frederick_H 4:f5a78245f2d0 140 pc.printf(" f -> 2nd order Integration \r\n");
Frederick_H 4:f5a78245f2d0 141 pc.printf(" g -> 3rd order Integration \r\n");
Frederick_H 4:f5a78245f2d0 142 pc.printf(" h -> 1st order Differentiation \r\n");
Frederick_H 4:f5a78245f2d0 143 pc.printf(" k -> 2nd order Differentiation \r\n");
Frederick_H 7:592373200317 144 pc.printf(" + -> Output Zoom In 2X \r\n");
Frederick_H 7:592373200317 145 pc.printf(" - -> Output Zoom Out 2Z \r\n");
Frederick_H 7:592373200317 146 pc.printf(" ^ -> shift up 1% \r\n");
Frederick_H 7:592373200317 147 pc.printf(" v -> shift down 1% \r\n");
Frederick_H 4:f5a78245f2d0 148 pc.printf("Press any key to start. \r\n");
Frederick_H 4:f5a78245f2d0 149 getc = pc.getc();
Frederick_H 4:f5a78245f2d0 150
Frederick_H 4:f5a78245f2d0 151
Frederick_H 3:e4783c57bcc0 152
Frederick_H 3:e4783c57bcc0 153 while (1) {
Frederick_H 3:e4783c57bcc0 154 int error_count=0;
Frederick_H 7:592373200317 155 RangeMapper DAC_Mapper1( 0 - 1<<13, 1<<13, 0, 1);
Frederick_H 7:592373200317 156 RangeMapper DAC_Mapper2( 0 - 1<<13, 1<<13, 0, 1);
Frederick_H 4:f5a78245f2d0 157 if (pc.readable())
Frederick_H 4:f5a78245f2d0 158 {
Frederick_H 4:f5a78245f2d0 159 getc = pc.getc();
Frederick_H 4:f5a78245f2d0 160 switch( getc)
Frederick_H 4:f5a78245f2d0 161 {
Frederick_H 4:f5a78245f2d0 162 case 'x' : channel = 0 ; pc.printf("Data procesing output switch to X channdel \r\n"); break;
Frederick_H 4:f5a78245f2d0 163 case 'y' : channel = 1 ; pc.printf("Data procesing output switch to Y channdel \r\n"); break;
Frederick_H 4:f5a78245f2d0 164 case 'z' : channel = 2 ; pc.printf("Data procesing output switch to Z channdel \r\n"); break;
Frederick_H 4:f5a78245f2d0 165 case '0' :
Frederick_H 4:f5a78245f2d0 166 case 'o' : output = !output ; pc.printf("Turn %s data output.\r\n", (output ? "On" : "Off") ); break;
Frederick_H 4:f5a78245f2d0 167 case '1' :
Frederick_H 4:f5a78245f2d0 168 case 'i' : mapout = !mapout ; pc.printf("Turn %s mapping output.\r\n", (mapout ? "On" : "Off") ); break;
Frederick_H 4:f5a78245f2d0 169 case 'a' : datamode = 0 ; pc.printf("Set to raw data output.\r\n"); break;
Frederick_H 4:f5a78245f2d0 170 case 'b' : datamode = 1 ; pc.printf("Set to mean output.\r\n"); break;
Frederick_H 4:f5a78245f2d0 171 case 'c' : datamode = 2 ; pc.printf("Set to summary output.\r\n"); break;
Frederick_H 4:f5a78245f2d0 172 case 'd' : datamode = 3 ; pc.printf("Set to variance output.\r\n"); break;
Frederick_H 4:f5a78245f2d0 173 case 'e' : datamode = 4 ; pc.printf("Set to 1st order Integration output.\r\n"); break;
Frederick_H 4:f5a78245f2d0 174 case 'f' : datamode = 5 ; pc.printf("Set to 2nd order Integration output.\r\n"); break;
Frederick_H 4:f5a78245f2d0 175 case 'g' : datamode = 6 ; pc.printf("Set to 3rd order Integration output.\r\n"); break;
Frederick_H 4:f5a78245f2d0 176 case 'h' : datamode = 7 ; pc.printf("Set to 1st order Differentiation output.\r\n"); break;
Frederick_H 4:f5a78245f2d0 177 case 'k' : datamode = 8 ; pc.printf("Set to 2nd order Differentiation output.\r\n"); break;
Frederick_H 7:592373200317 178 case '+' : DAC_Mapper2.zoom(2) ; pc.printf("Mapout zoom in.\r\n"); break;
Frederick_H 7:592373200317 179 case '-' : DAC_Mapper2.zoom(0.5) ; pc.printf("Mapout zoom out.\r\n"); break;
Frederick_H 7:592373200317 180 case '^' : DAC_Mapper2.shift(0.01) ; pc.printf("Mapout zoom out.\r\n"); break;
Frederick_H 7:592373200317 181 case 'v' : DAC_Mapper2.shift(-0.01) ; pc.printf("Mapout zoom out.\r\n"); break;
Frederick_H 4:f5a78245f2d0 182 default: break;
Frederick_H 4:f5a78245f2d0 183 }
Frederick_H 4:f5a78245f2d0 184 }
Frederick_H 3:e4783c57bcc0 185 accelerometer.getOutput(readings);
Frederick_H 3:e4783c57bcc0 186 if (( 17601 == readings[0] ) || ( 17601 == readings[1] ) || ( 17601 == readings[2] ))
Frederick_H 3:e4783c57bcc0 187 {
Frederick_H 3:e4783c57bcc0 188 error_count++;
Frederick_H 3:e4783c57bcc0 189 if (error_count>10)
Frederick_H 3:e4783c57bcc0 190 {
Frederick_H 3:e4783c57bcc0 191 accelerometer.setPowerControl(0);
Frederick_H 3:e4783c57bcc0 192 pc.printf("Sensor Halt!\r\n");
Frederick_H 3:e4783c57bcc0 193 goto restart;
Frederick_H 3:e4783c57bcc0 194 }
Frederick_H 3:e4783c57bcc0 195 }
Frederick_H 3:e4783c57bcc0 196 else
Frederick_H 3:e4783c57bcc0 197 {
Frederick_H 4:f5a78245f2d0 198 error_count = 0;
Frederick_H 6:88787a48e96e 199 int i;
Frederick_H 6:88787a48e96e 200 for (i = 0; i<3; i++)
Frederick_H 6:88787a48e96e 201 dp[i].putData(readings[i] = (int16_t) readings[i] );
Frederick_H 6:88787a48e96e 202
Frederick_H 4:f5a78245f2d0 203 if (output)
Frederick_H 4:f5a78245f2d0 204 {
Frederick_H 4:f5a78245f2d0 205 switch (datamode)
Frederick_H 4:f5a78245f2d0 206 {
Frederick_H 6:88787a48e96e 207 case 0: pc.printf("RAW: %i, %i, %i\r\n", (int16_t)readings[0], (int16_t)readings[1], (int16_t)readings[2]); break;
Frederick_H 6:88787a48e96e 208 //case 0: pc.printf("RAW: %i, %i, %i\r\n", readings[0], readings[1], readings[2]); break;
Frederick_H 6:88787a48e96e 209 case 1: pc.printf("MEAN: %f, %f, %f\r\n", dp[0].getMean(), dp[1].getMean(), dp[2].getMean() ); break;
Frederick_H 6:88787a48e96e 210 case 2: pc.printf("SUM: %jd, %jd, %jd\r\n", dp[0].getSum(), dp[1].getSum(), dp[2].getSum() ); break;
Frederick_H 6:88787a48e96e 211 case 3: pc.printf("STD: %f, %f, %f\r\n", dp[0].getStdDiv(), dp[1].getStdDiv(), dp[2].getStdDiv() ); break;
Frederick_H 6:88787a48e96e 212 case 4: pc.printf("1ITG: %jd, %jd, %jd\r\n", dp[0].getO1integration(), dp[1].getO1integration(), dp[2].getO1integration() ); break;
Frederick_H 6:88787a48e96e 213 case 5: pc.printf("2ITG: %jd, %jd, %jd\r\n", dp[0].getO2integration(), dp[1].getO2integration(), dp[2].getO2integration() ); break;
Frederick_H 6:88787a48e96e 214 case 6: pc.printf("3ITG: %jd, %jd, %jd\r\n", dp[0].getO3integration(), dp[1].getO3integration(), dp[2].getO3integration() ); break;
Frederick_H 6:88787a48e96e 215 case 7: pc.printf("1DIF: %jd, %jd, %jd\r\n", dp[0].GetO1differ(), dp[1].GetO1differ(), dp[2].GetO1differ() ); break;
Frederick_H 6:88787a48e96e 216 case 8: pc.printf("2DIF: %jd, %jd, %jd\r\n", dp[0].GetO2differ(), dp[1].GetO2differ(), dp[2].GetO2differ() ); break;
Frederick_H 4:f5a78245f2d0 217 default: break;
Frederick_H 4:f5a78245f2d0 218 }
Frederick_H 4:f5a78245f2d0 219 }
Frederick_H 7:592373200317 220 // Set data the DAC
Frederick_H 7:592373200317 221 DAC_Mapper1.getOutput(readings[channel]);
Frederick_H 7:592373200317 222 if (mapout)
Frederick_H 4:f5a78245f2d0 223 {
Frederick_H 6:88787a48e96e 224 double mapdata;
Frederick_H 6:88787a48e96e 225 switch (datamode)
Frederick_H 6:88787a48e96e 226 {
Frederick_H 6:88787a48e96e 227 case 0: mapdata = readings[channel]; break;
Frederick_H 6:88787a48e96e 228 case 1: mapdata = dp[channel].getMean(); break;
Frederick_H 6:88787a48e96e 229 case 2: mapdata = dp[channel].getSum(); break;
Frederick_H 6:88787a48e96e 230 case 3: mapdata = dp[channel].getStdDiv(); break;
Frederick_H 6:88787a48e96e 231 case 4: mapdata = dp[channel].getO1integration(); break;
Frederick_H 6:88787a48e96e 232 case 5: mapdata = dp[channel].getO2integration(); break;
Frederick_H 6:88787a48e96e 233 case 6: mapdata = dp[channel].getO3integration(); break;
Frederick_H 6:88787a48e96e 234 case 7: mapdata = dp[channel].GetO1differ(); break;
Frederick_H 6:88787a48e96e 235 case 8: mapdata = dp[channel].GetO2differ(); break;
Frederick_H 6:88787a48e96e 236 default: mapdata = 0; break;
Frederick_H 6:88787a48e96e 237 }
Frederick_H 7:592373200317 238 pc.printf("MAPPING OUTPUT: %jd\r\n", DAC_Mapper2.getOutput(mapdata) );
Frederick_H 7:592373200317 239
Frederick_H 4:f5a78245f2d0 240 }
Frederick_H 7:592373200317 241 else
Frederick_H 7:592373200317 242 {
Frederick_H 7:592373200317 243 DAC_Mapper2.getOutput(0);
Frederick_H 7:592373200317 244 }
Frederick_H 7:592373200317 245
Frederick_H 4:f5a78245f2d0 246 wait(1);
Frederick_H 3:e4783c57bcc0 247 }
Frederick_H 4:f5a78245f2d0 248
Frederick_H 3:e4783c57bcc0 249 }
Frederick_H 3:e4783c57bcc0 250
Frederick_H 3:e4783c57bcc0 251 }