Code to find max and avg deviation, max swing, or simple readings from a still HMC6352 compass. Can be used with motors to test error from magnetic interference.
Dependencies: HMC6352 Motordriver mbed
Fork of HMC6352_HelloWorld by
main.cpp@2:ca1d3bef09a6, 2012-12-14 (annotated)
- Committer:
- theschrade54
- Date:
- Fri Dec 14 05:05:35 2012 +0000
- Revision:
- 2:ca1d3bef09a6
- Parent:
- 0:f9a9be860001
Code to find max and avg deviation, max swing, or simple readings from a still HMC6352 compass. Can be used with motors to test error from magnetic interference.
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
theschrade54 | 2:ca1d3bef09a6 | 1 | #include "mbed.h" |
theschrade54 | 2:ca1d3bef09a6 | 2 | #include "motordriver.h" |
theschrade54 | 2:ca1d3bef09a6 | 3 | #include "HMC6352.h" |
theschrade54 | 2:ca1d3bef09a6 | 4 | |
theschrade54 | 2:ca1d3bef09a6 | 5 | // test program to verify compass integrity when run on motor and to determine filtering values. uncomment/comment motor sections to test. |
theschrade54 | 2:ca1d3bef09a6 | 6 | HMC6352 compass(p9, p10); |
theschrade54 | 2:ca1d3bef09a6 | 7 | Serial pc(USBTX, USBRX); |
theschrade54 | 2:ca1d3bef09a6 | 8 | //Motor left(p21, p22, p20, 1); // pwm, fwd, rev, has brake feature |
theschrade54 | 2:ca1d3bef09a6 | 9 | //Motor right(p24, p25, p27, 1); |
theschrade54 | 2:ca1d3bef09a6 | 10 | |
theschrade54 | 2:ca1d3bef09a6 | 11 | int main() { |
theschrade54 | 2:ca1d3bef09a6 | 12 | float avgdev, dev, maxdev, swing, prevval, curval, minval, maxval = 0.0; //variables to measure swing, maxdev, and avg dev of measurements |
theschrade54 | 2:ca1d3bef09a6 | 13 | long count = 0; |
theschrade54 | 2:ca1d3bef09a6 | 14 | //float speed = .1; |
theschrade54 | 2:ca1d3bef09a6 | 15 | pc.printf("Starting HMC6352 test...\n"); |
theschrade54 | 2:ca1d3bef09a6 | 16 | //left.speed(speed); |
theschrade54 | 2:ca1d3bef09a6 | 17 | //right.speed(speed); |
theschrade54 | 2:ca1d3bef09a6 | 18 | //Continuous mode, periodic set/reset, 20Hz measurement rate. |
theschrade54 | 2:ca1d3bef09a6 | 19 | compass.setOpMode(HMC6352_CONTINUOUS, 1, 20); |
theschrade54 | 2:ca1d3bef09a6 | 20 | wait(1); |
theschrade54 | 2:ca1d3bef09a6 | 21 | //init math vals |
theschrade54 | 2:ca1d3bef09a6 | 22 | curval = compass.sample()/10.0; |
theschrade54 | 2:ca1d3bef09a6 | 23 | minval = curval; |
theschrade54 | 2:ca1d3bef09a6 | 24 | maxval = curval; |
theschrade54 | 2:ca1d3bef09a6 | 25 | while (1) { |
theschrade54 | 2:ca1d3bef09a6 | 26 | count++; // for avg |
theschrade54 | 2:ca1d3bef09a6 | 27 | prevval = curval; |
theschrade54 | 2:ca1d3bef09a6 | 28 | curval = compass.sample()/10.0; |
theschrade54 | 2:ca1d3bef09a6 | 29 | dev = abs(curval - prevval); |
theschrade54 | 2:ca1d3bef09a6 | 30 | if (curval > maxval) {maxval = curval;} |
theschrade54 | 2:ca1d3bef09a6 | 31 | if (curval < minval) {minval = curval;} |
theschrade54 | 2:ca1d3bef09a6 | 32 | if (dev > maxdev) {maxdev = dev;} //largest deviation between two successive readings. |
theschrade54 | 2:ca1d3bef09a6 | 33 | avgdev += dev; //divide avg dev in print statement |
theschrade54 | 2:ca1d3bef09a6 | 34 | swing = maxval - minval; //swing is largest reading change possible from remaining stationary. |
theschrade54 | 2:ca1d3bef09a6 | 35 | if (count % 10 == 1) { |
theschrade54 | 2:ca1d3bef09a6 | 36 | pc.printf("Heading is: %f, Max Dev: %f, Avg Dev: %f, Swing: %f\n", curval, maxdev, avgdev/count, swing); |
theschrade54 | 2:ca1d3bef09a6 | 37 | //if (speed < .8) {speed += .05; |
theschrade54 | 2:ca1d3bef09a6 | 38 | //left.speed(speed); |
theschrade54 | 2:ca1d3bef09a6 | 39 | //right.speed(speed);} |
theschrade54 | 2:ca1d3bef09a6 | 40 | } |
theschrade54 | 2:ca1d3bef09a6 | 41 | wait(0.05); |
theschrade54 | 2:ca1d3bef09a6 | 42 | } |
theschrade54 | 2:ca1d3bef09a6 | 43 | |
theschrade54 | 2:ca1d3bef09a6 | 44 | } |