Oskar Weigl
Calibrates the accelorometer and then spits out data
main.cpp@0:2b2172edbc90, 2011-12-05 (annotated)
- Committer:
- madcowswe
- Date:
- Mon Dec 05 18:08:44 2011 +0000
- Revision:
- 0:2b2172edbc90
- Child:
- 1:cc13fddeedc0
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| madcowswe | 0:2b2172edbc90 | 1 | #include "mbed.h" |
| madcowswe | 0:2b2172edbc90 | 2 | |
| madcowswe | 0:2b2172edbc90 | 3 | DigitalOut myled(LED1); |
| madcowswe | 0:2b2172edbc90 | 4 | AnalogIn ainAccX(p20); |
| madcowswe | 0:2b2172edbc90 | 5 | AnalogIn ainAccY(p19); |
| madcowswe | 0:2b2172edbc90 | 6 | AnalogIn ainAccZ(p18); |
| madcowswe | 0:2b2172edbc90 | 7 | Serial pc(USBTX, USBRX); |
| madcowswe | 0:2b2172edbc90 | 8 | Ticker ticker; |
| madcowswe | 0:2b2172edbc90 | 9 | |
| madcowswe | 0:2b2172edbc90 | 10 | int calibacc3ax[3] = {0,0,0}; |
| madcowswe | 0:2b2172edbc90 | 11 | float grav; |
| madcowswe | 0:2b2172edbc90 | 12 | |
| madcowswe | 0:2b2172edbc90 | 13 | float a = 0; |
| madcowswe | 0:2b2172edbc90 | 14 | double v = 0; |
| madcowswe | 0:2b2172edbc90 | 15 | double x = 0; |
| madcowswe | 0:2b2172edbc90 | 16 | |
| madcowswe | 0:2b2172edbc90 | 17 | unsigned int normalacc() { |
| madcowswe | 0:2b2172edbc90 | 18 | int x = ainAccX.read_u16() - calibacc3ax[0]; |
| madcowswe | 0:2b2172edbc90 | 19 | int y = ainAccY.read_u16() - calibacc3ax[1]; |
| madcowswe | 0:2b2172edbc90 | 20 | int z = ainAccZ.read_u16() - calibacc3ax[2]; |
| madcowswe | 0:2b2172edbc90 | 21 | return (sqrt(float(x*x + y*y + z*z))); |
| madcowswe | 0:2b2172edbc90 | 22 | } |
| madcowswe | 0:2b2172edbc90 | 23 | |
| madcowswe | 0:2b2172edbc90 | 24 | void tick(){ |
| madcowswe | 0:2b2172edbc90 | 25 | a = normalacc() * 3.3 * 32.27 * 0.001 * (1.0 / 0x10000); |
| madcowswe | 0:2b2172edbc90 | 26 | v += a; |
| madcowswe | 0:2b2172edbc90 | 27 | x += v * 0.001; |
| madcowswe | 0:2b2172edbc90 | 28 | } |
| madcowswe | 0:2b2172edbc90 | 29 | |
| madcowswe | 0:2b2172edbc90 | 30 | int main() { |
| madcowswe | 0:2b2172edbc90 | 31 | pc.baud(115200); |
| madcowswe | 0:2b2172edbc90 | 32 | |
| madcowswe | 0:2b2172edbc90 | 33 | for (int i = 0; i < 10000; i++) { |
| madcowswe | 0:2b2172edbc90 | 34 | calibacc3ax[0] += ainAccX.read_u16(); |
| madcowswe | 0:2b2172edbc90 | 35 | calibacc3ax[1] += ainAccY.read_u16(); |
| madcowswe | 0:2b2172edbc90 | 36 | calibacc3ax[2] += ainAccZ.read_u16(); |
| madcowswe | 0:2b2172edbc90 | 37 | |
| madcowswe | 0:2b2172edbc90 | 38 | wait_us(1000); |
| madcowswe | 0:2b2172edbc90 | 39 | } |
| madcowswe | 0:2b2172edbc90 | 40 | |
| madcowswe | 0:2b2172edbc90 | 41 | calibacc3ax[0] /= 10000; |
| madcowswe | 0:2b2172edbc90 | 42 | calibacc3ax[1] /= 10000; |
| madcowswe | 0:2b2172edbc90 | 43 | calibacc3ax[2] /= 10000; |
| madcowswe | 0:2b2172edbc90 | 44 | |
| madcowswe | 0:2b2172edbc90 | 45 | grav = //TODO: Make calibration for both +z and -z |
| madcowswe | 0:2b2172edbc90 | 46 | |
| madcowswe | 0:2b2172edbc90 | 47 | ticker.attach_us(&tick, 1000); |
| madcowswe | 0:2b2172edbc90 | 48 | |
| madcowswe | 0:2b2172edbc90 | 49 | while (1) { |
| madcowswe | 0:2b2172edbc90 | 50 | pc.printf("%f %f %f\r\n", a * 1000, v, x); |
| madcowswe | 0:2b2172edbc90 | 51 | wait(0.2); |
| madcowswe | 0:2b2172edbc90 | 52 | } |
| madcowswe | 0:2b2172edbc90 | 53 | } |