Xavier GERONIMI
/
ADXL335_3axe_accel_etallonage
acceleration et etallonage du zero
Fork of ADXL335_3axis_Accel by
main.cpp@4:ee0a25a1c3dc, 2018-04-11 (annotated)
- Committer:
- Zlatan10
- Date:
- Wed Apr 11 19:28:43 2018 +0000
- Revision:
- 4:ee0a25a1c3dc
- Parent:
- 3:7cef8e5abfe5
Acceleration et etallonage du zero
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Zlatan10 | 4:ee0a25a1c3dc | 1 | /***************************************** |
Zlatan10 | 4:ee0a25a1c3dc | 2 | ****** 3-axis Accelerometer ADXL335 ***** |
Zlatan10 | 4:ee0a25a1c3dc | 3 | *****************************************/ |
shivamtripathi | 3:7cef8e5abfe5 | 4 | |
bcostm | 0:c2d1ad5059da | 5 | #include "mbed.h" |
shivamtripathi | 3:7cef8e5abfe5 | 6 | |
Zlatan10 | 4:ee0a25a1c3dc | 7 | #define ETALONNAGE 100 |
Zlatan10 | 4:ee0a25a1c3dc | 8 | #define VERIFICATION 100 |
Zlatan10 | 4:ee0a25a1c3dc | 9 | #define MESURE 500 |
Zlatan10 | 4:ee0a25a1c3dc | 10 | //#define LIMITE 65535 * 1 / 1000 |
Zlatan10 | 4:ee0a25a1c3dc | 11 | #define LIMITE 4095 * 1 / 1000 |
Zlatan10 | 4:ee0a25a1c3dc | 12 | #define LSB 14.365 |
Zlatan10 | 4:ee0a25a1c3dc | 13 | |
Zlatan10 | 4:ee0a25a1c3dc | 14 | Timer timer; |
Zlatan10 | 4:ee0a25a1c3dc | 15 | |
Zlatan10 | 4:ee0a25a1c3dc | 16 | Serial pc(SERIAL_TX, SERIAL_RX); |
Zlatan10 | 4:ee0a25a1c3dc | 17 | |
Zlatan10 | 4:ee0a25a1c3dc | 18 | AnalogIn acc_x(A0); // Output of X-axis |
Zlatan10 | 4:ee0a25a1c3dc | 19 | AnalogIn acc_y(A1); // Output of y-axis |
Zlatan10 | 4:ee0a25a1c3dc | 20 | AnalogIn acc_z(A2); // Output of z-axis |
Zlatan10 | 4:ee0a25a1c3dc | 21 | DigitalOut led(LED1); // output of LED |
Zlatan10 | 4:ee0a25a1c3dc | 22 | |
Zlatan10 | 4:ee0a25a1c3dc | 23 | void mesure_zero_acceleration(int *zero_x, int *zero_y, int *zero_z) |
Zlatan10 | 4:ee0a25a1c3dc | 24 | { |
Zlatan10 | 4:ee0a25a1c3dc | 25 | float fx, fy, fz; |
Zlatan10 | 4:ee0a25a1c3dc | 26 | float f_etalonnage = (float)ETALONNAGE; |
Zlatan10 | 4:ee0a25a1c3dc | 27 | int x, y, z; |
Zlatan10 | 4:ee0a25a1c3dc | 28 | int c, k; |
Zlatan10 | 4:ee0a25a1c3dc | 29 | |
Zlatan10 | 4:ee0a25a1c3dc | 30 | for (c = 0; c < ETALONNAGE; c++) |
Zlatan10 | 4:ee0a25a1c3dc | 31 | { |
Zlatan10 | 4:ee0a25a1c3dc | 32 | x = 0; |
Zlatan10 | 4:ee0a25a1c3dc | 33 | y = 0; |
Zlatan10 | 4:ee0a25a1c3dc | 34 | z = 0; |
Zlatan10 | 4:ee0a25a1c3dc | 35 | for (k = 0; k < MESURE; k++) |
Zlatan10 | 4:ee0a25a1c3dc | 36 | { |
Zlatan10 | 4:ee0a25a1c3dc | 37 | x += acc_x.read_u16() >> 4; |
Zlatan10 | 4:ee0a25a1c3dc | 38 | y += acc_y.read_u16() >> 4; |
Zlatan10 | 4:ee0a25a1c3dc | 39 | z += acc_z.read_u16() >> 4; |
Zlatan10 | 4:ee0a25a1c3dc | 40 | } |
Zlatan10 | 4:ee0a25a1c3dc | 41 | *zero_x += x; |
Zlatan10 | 4:ee0a25a1c3dc | 42 | *zero_y += y; |
Zlatan10 | 4:ee0a25a1c3dc | 43 | *zero_z += z; |
Zlatan10 | 4:ee0a25a1c3dc | 44 | } |
Zlatan10 | 4:ee0a25a1c3dc | 45 | fx = *zero_x / f_etalonnage; |
Zlatan10 | 4:ee0a25a1c3dc | 46 | fy = *zero_y / f_etalonnage; |
Zlatan10 | 4:ee0a25a1c3dc | 47 | fz = *zero_z / f_etalonnage; |
Zlatan10 | 4:ee0a25a1c3dc | 48 | *zero_x /= ETALONNAGE; |
Zlatan10 | 4:ee0a25a1c3dc | 49 | *zero_y /= ETALONNAGE; |
Zlatan10 | 4:ee0a25a1c3dc | 50 | *zero_z /= ETALONNAGE; |
Zlatan10 | 4:ee0a25a1c3dc | 51 | |
Zlatan10 | 4:ee0a25a1c3dc | 52 | if ((fx - *zero_x) >= 0.5) *zero_x ++; |
Zlatan10 | 4:ee0a25a1c3dc | 53 | if ((fy - *zero_y) >= 0.5) *zero_y ++; |
Zlatan10 | 4:ee0a25a1c3dc | 54 | if ((fz - *zero_z) >= 0.5) *zero_z ++; |
Zlatan10 | 4:ee0a25a1c3dc | 55 | |
Zlatan10 | 4:ee0a25a1c3dc | 56 | pc.printf("AX_0;AY_0;AZ_0\n"); |
Zlatan10 | 4:ee0a25a1c3dc | 57 | pc.printf("%d;%d;%d\n", *zero_x, *zero_y, *zero_z); |
Zlatan10 | 4:ee0a25a1c3dc | 58 | } |
shivamtripathi | 3:7cef8e5abfe5 | 59 | |
Zlatan10 | 4:ee0a25a1c3dc | 60 | void verif_zero_acceleration(int zero_x, int zero_y, int zero_z) |
Zlatan10 | 4:ee0a25a1c3dc | 61 | { |
Zlatan10 | 4:ee0a25a1c3dc | 62 | int x, y, z; |
Zlatan10 | 4:ee0a25a1c3dc | 63 | float fx, fy, fz; |
Zlatan10 | 4:ee0a25a1c3dc | 64 | float f_mesure = (float)MESURE; |
Zlatan10 | 4:ee0a25a1c3dc | 65 | int c, k; |
Zlatan10 | 4:ee0a25a1c3dc | 66 | |
Zlatan10 | 4:ee0a25a1c3dc | 67 | for (c = 0; c < VERIFICATION; c++) |
Zlatan10 | 4:ee0a25a1c3dc | 68 | { |
Zlatan10 | 4:ee0a25a1c3dc | 69 | x = 0; y = 0; z = 0; |
Zlatan10 | 4:ee0a25a1c3dc | 70 | |
Zlatan10 | 4:ee0a25a1c3dc | 71 | for (k = 0; k < MESURE; k++) |
Zlatan10 | 4:ee0a25a1c3dc | 72 | { |
Zlatan10 | 4:ee0a25a1c3dc | 73 | x += ( acc_x.read_u16() >> 4 ); |
Zlatan10 | 4:ee0a25a1c3dc | 74 | y += ( acc_y.read_u16() >> 4 ); |
Zlatan10 | 4:ee0a25a1c3dc | 75 | z += ( acc_z.read_u16() >> 4 ); |
Zlatan10 | 4:ee0a25a1c3dc | 76 | } |
Zlatan10 | 4:ee0a25a1c3dc | 77 | |
Zlatan10 | 4:ee0a25a1c3dc | 78 | x -= zero_x; |
Zlatan10 | 4:ee0a25a1c3dc | 79 | y -= zero_y; |
Zlatan10 | 4:ee0a25a1c3dc | 80 | z -= zero_z; |
Zlatan10 | 4:ee0a25a1c3dc | 81 | |
Zlatan10 | 4:ee0a25a1c3dc | 82 | fx = x / f_mesure; |
Zlatan10 | 4:ee0a25a1c3dc | 83 | fy = y / f_mesure; |
Zlatan10 | 4:ee0a25a1c3dc | 84 | fz = z / f_mesure; |
Zlatan10 | 4:ee0a25a1c3dc | 85 | x /= MESURE; |
Zlatan10 | 4:ee0a25a1c3dc | 86 | y /= MESURE; |
Zlatan10 | 4:ee0a25a1c3dc | 87 | z /= MESURE; |
Zlatan10 | 4:ee0a25a1c3dc | 88 | |
Zlatan10 | 4:ee0a25a1c3dc | 89 | if (fx < 0) |
Zlatan10 | 4:ee0a25a1c3dc | 90 | { |
Zlatan10 | 4:ee0a25a1c3dc | 91 | if ( (fx - x) <= - 0.5 ) x--; |
Zlatan10 | 4:ee0a25a1c3dc | 92 | }else |
Zlatan10 | 4:ee0a25a1c3dc | 93 | if ( (fx - x) >= 0.5 ) x++; |
Zlatan10 | 4:ee0a25a1c3dc | 94 | if (fy < 0) |
Zlatan10 | 4:ee0a25a1c3dc | 95 | { |
Zlatan10 | 4:ee0a25a1c3dc | 96 | if ( (fy - y) <= - 0.5 ) y--; |
Zlatan10 | 4:ee0a25a1c3dc | 97 | }else |
Zlatan10 | 4:ee0a25a1c3dc | 98 | if ( (fy - y) >= 0.5 ) y++; |
Zlatan10 | 4:ee0a25a1c3dc | 99 | if (fz < 0) |
Zlatan10 | 4:ee0a25a1c3dc | 100 | { |
Zlatan10 | 4:ee0a25a1c3dc | 101 | if ( (fz - z) <= - 0.5 ) z--; |
Zlatan10 | 4:ee0a25a1c3dc | 102 | }else |
Zlatan10 | 4:ee0a25a1c3dc | 103 | if ( (fz - z) >= 0.5 ) z++; |
Zlatan10 | 4:ee0a25a1c3dc | 104 | |
Zlatan10 | 4:ee0a25a1c3dc | 105 | if( abs (x) > LIMITE ) |
Zlatan10 | 4:ee0a25a1c3dc | 106 | { |
Zlatan10 | 4:ee0a25a1c3dc | 107 | pc.printf("Etalonnage en X manque! Ecart = %d\n", abs(x) - LIMITE); |
Zlatan10 | 4:ee0a25a1c3dc | 108 | exit(EXIT_FAILURE); |
Zlatan10 | 4:ee0a25a1c3dc | 109 | } |
Zlatan10 | 4:ee0a25a1c3dc | 110 | if( abs (y) > LIMITE > LIMITE ) |
Zlatan10 | 4:ee0a25a1c3dc | 111 | { |
Zlatan10 | 4:ee0a25a1c3dc | 112 | pc.printf("Etalonnage en Y manque! Ecart = %d\n", abs(y) - LIMITE); |
Zlatan10 | 4:ee0a25a1c3dc | 113 | exit(EXIT_FAILURE); |
Zlatan10 | 4:ee0a25a1c3dc | 114 | } |
Zlatan10 | 4:ee0a25a1c3dc | 115 | if( abs (z) > LIMITE ) |
Zlatan10 | 4:ee0a25a1c3dc | 116 | { |
Zlatan10 | 4:ee0a25a1c3dc | 117 | pc.printf("Etalonnage en Z manque! Ecart = %d\n", abs(z) - LIMITE); |
Zlatan10 | 4:ee0a25a1c3dc | 118 | exit(EXIT_FAILURE); |
Zlatan10 | 4:ee0a25a1c3dc | 119 | } |
Zlatan10 | 4:ee0a25a1c3dc | 120 | } |
Zlatan10 | 4:ee0a25a1c3dc | 121 | } |
bcostm | 1:0490a15c76e4 | 122 | |
bcostm | 0:c2d1ad5059da | 123 | int main() { |
Zlatan10 | 4:ee0a25a1c3dc | 124 | int ax, ay, az; // Store the acceleration |
Zlatan10 | 4:ee0a25a1c3dc | 125 | /* |
Zlatan10 | 4:ee0a25a1c3dc | 126 | float vx = 0, vy = 0, vz = 0; // Store the speed |
Zlatan10 | 4:ee0a25a1c3dc | 127 | float x = 0, y = 0, z = 0; // Store the position |
Zlatan10 | 4:ee0a25a1c3dc | 128 | */ |
Zlatan10 | 4:ee0a25a1c3dc | 129 | float fx, fy, fz; |
Zlatan10 | 4:ee0a25a1c3dc | 130 | float f_mesure = (float)MESURE; |
Zlatan10 | 4:ee0a25a1c3dc | 131 | int zero_x = 0, zero_y = 0, zero_z = 0; // 0 acceleration |
Zlatan10 | 4:ee0a25a1c3dc | 132 | float t; // Measurement Time |
Zlatan10 | 4:ee0a25a1c3dc | 133 | int i, cpt = 0; // Counters |
bcostm | 1:0490a15c76e4 | 134 | |
Zlatan10 | 4:ee0a25a1c3dc | 135 | wait(2); |
Zlatan10 | 4:ee0a25a1c3dc | 136 | |
Zlatan10 | 4:ee0a25a1c3dc | 137 | timer.start(); |
Zlatan10 | 4:ee0a25a1c3dc | 138 | |
Zlatan10 | 4:ee0a25a1c3dc | 139 | pc.baud(9600); |
Zlatan10 | 4:ee0a25a1c3dc | 140 | |
Zlatan10 | 4:ee0a25a1c3dc | 141 | mesure_zero_acceleration(&zero_x, &zero_y, &zero_z); |
Zlatan10 | 4:ee0a25a1c3dc | 142 | //verif_zero_acceleration(zero_x, zero_y, zero_z); |
Zlatan10 | 4:ee0a25a1c3dc | 143 | |
Zlatan10 | 4:ee0a25a1c3dc | 144 | led = 1; |
Zlatan10 | 4:ee0a25a1c3dc | 145 | |
Zlatan10 | 4:ee0a25a1c3dc | 146 | pc.printf("Temps d'acquisition;AX;AY;AZ;Numero d'echantillon\n"); |
Zlatan10 | 4:ee0a25a1c3dc | 147 | //pc.printf("AX;AY;AZ;VX;VY;VZ;X;Y;Z\n"); |
bcostm | 1:0490a15c76e4 | 148 | |
bcostm | 1:0490a15c76e4 | 149 | while(1) { |
Zlatan10 | 4:ee0a25a1c3dc | 150 | cpt ++; |
Zlatan10 | 4:ee0a25a1c3dc | 151 | ax = 0; ay = 0; az = 0; |
Zlatan10 | 4:ee0a25a1c3dc | 152 | //timer.start(); |
Zlatan10 | 4:ee0a25a1c3dc | 153 | for (i = 0; i < MESURE; i++) |
Zlatan10 | 4:ee0a25a1c3dc | 154 | { |
Zlatan10 | 4:ee0a25a1c3dc | 155 | ax += ( acc_x.read_u16() >> 4 ); // Reads X-axis value (on 16 bits converted to 12) |
Zlatan10 | 4:ee0a25a1c3dc | 156 | ay += ( acc_y.read_u16() >> 4 ); // Reads Y-axis value |
Zlatan10 | 4:ee0a25a1c3dc | 157 | az += ( acc_z.read_u16() >> 4 ); // Reads Z-axis value |
Zlatan10 | 4:ee0a25a1c3dc | 158 | } |
Zlatan10 | 4:ee0a25a1c3dc | 159 | //timer.stop(); |
Zlatan10 | 4:ee0a25a1c3dc | 160 | t = timer.read(); |
Zlatan10 | 4:ee0a25a1c3dc | 161 | //timer.reset(); |
Zlatan10 | 4:ee0a25a1c3dc | 162 | |
Zlatan10 | 4:ee0a25a1c3dc | 163 | ax -= zero_x; |
Zlatan10 | 4:ee0a25a1c3dc | 164 | ay -= zero_y; |
Zlatan10 | 4:ee0a25a1c3dc | 165 | az -= zero_z; |
Zlatan10 | 4:ee0a25a1c3dc | 166 | |
Zlatan10 | 4:ee0a25a1c3dc | 167 | fx = ax / f_mesure; |
Zlatan10 | 4:ee0a25a1c3dc | 168 | fy = ay / f_mesure; |
Zlatan10 | 4:ee0a25a1c3dc | 169 | fz = az / f_mesure; |
Zlatan10 | 4:ee0a25a1c3dc | 170 | ax /= MESURE; |
Zlatan10 | 4:ee0a25a1c3dc | 171 | ay /= MESURE; |
Zlatan10 | 4:ee0a25a1c3dc | 172 | az /= MESURE; |
shivamtripathi | 3:7cef8e5abfe5 | 173 | |
Zlatan10 | 4:ee0a25a1c3dc | 174 | if (fx < 0) |
Zlatan10 | 4:ee0a25a1c3dc | 175 | { |
Zlatan10 | 4:ee0a25a1c3dc | 176 | if ( (fx - ax) <= - 0.5 ) ax--; |
Zlatan10 | 4:ee0a25a1c3dc | 177 | }else |
Zlatan10 | 4:ee0a25a1c3dc | 178 | if ( (fx - ax) >= 0.5 ) ax++; |
Zlatan10 | 4:ee0a25a1c3dc | 179 | if (fy < 0) |
Zlatan10 | 4:ee0a25a1c3dc | 180 | { |
Zlatan10 | 4:ee0a25a1c3dc | 181 | if ( (fy - ay) <= - 0.5 ) ay--; |
Zlatan10 | 4:ee0a25a1c3dc | 182 | }else |
Zlatan10 | 4:ee0a25a1c3dc | 183 | if ( (fy - ay) >= 0.5 ) ay++; |
Zlatan10 | 4:ee0a25a1c3dc | 184 | if (fz < 0) |
Zlatan10 | 4:ee0a25a1c3dc | 185 | { |
Zlatan10 | 4:ee0a25a1c3dc | 186 | if ( (fz - az) <= - 0.5 ) az--; |
Zlatan10 | 4:ee0a25a1c3dc | 187 | }else |
Zlatan10 | 4:ee0a25a1c3dc | 188 | if ( (fz - az) >= 0.5 ) az++; |
Zlatan10 | 4:ee0a25a1c3dc | 189 | |
Zlatan10 | 4:ee0a25a1c3dc | 190 | if(ax == -1 || ax == 1) ax = 0; |
Zlatan10 | 4:ee0a25a1c3dc | 191 | if(ay == -1 || ay == 1) ay = 0; |
Zlatan10 | 4:ee0a25a1c3dc | 192 | if(az == -1 || az == 1) az = 0; |
Zlatan10 | 4:ee0a25a1c3dc | 193 | |
Zlatan10 | 4:ee0a25a1c3dc | 194 | pc.printf("%f;%d;%d;%d;%d\n", t, ax, ay, az, cpt); |
Zlatan10 | 4:ee0a25a1c3dc | 195 | |
Zlatan10 | 4:ee0a25a1c3dc | 196 | /* |
Zlatan10 | 4:ee0a25a1c3dc | 197 | |
Zlatan10 | 4:ee0a25a1c3dc | 198 | vx += (ax * t * LSB); |
Zlatan10 | 4:ee0a25a1c3dc | 199 | vy += (ay * t * LSB); |
Zlatan10 | 4:ee0a25a1c3dc | 200 | vz += (az * t * LSB); |
Zlatan10 | 4:ee0a25a1c3dc | 201 | |
Zlatan10 | 4:ee0a25a1c3dc | 202 | x += (vx * t * LSB); |
Zlatan10 | 4:ee0a25a1c3dc | 203 | y += (vy * t * LSB); |
Zlatan10 | 4:ee0a25a1c3dc | 204 | z += (vz * t * LSB); |
Zlatan10 | 4:ee0a25a1c3dc | 205 | |
Zlatan10 | 4:ee0a25a1c3dc | 206 | if (cpt % 16 == 0) pc.printf("%d;%d;%d;%d;%d;%d;%d;%d;%d\n", ax, ay, az, vx, vy, vz, x, y, z); |
Zlatan10 | 4:ee0a25a1c3dc | 207 | |
Zlatan10 | 4:ee0a25a1c3dc | 208 | */ |
bcostm | 0:c2d1ad5059da | 209 | } |
Zlatan10 | 4:ee0a25a1c3dc | 210 | } |