main.cpp

00001 #include "mbed.h"
00002 #include "motordriver.h"
00003 #include "HMC6352.h"
00004 
00005 // test program to verify compass integrity when run on motor and to determine filtering values. uncomment/comment motor sections to test.
00006 HMC6352 compass(p9, p10);
00007 Serial pc(USBTX, USBRX);
00008 //Motor left(p21, p22, p20, 1); // pwm, fwd, rev, has brake feature
00009 //Motor right(p24, p25, p27, 1);
00010 
00011 int main() {
00012     float avgdev, dev, maxdev, swing, prevval, curval, minval, maxval = 0.0; //variables to measure swing, maxdev, and avg dev of measurements
00013     long count = 0;
00014     //float speed = .1;
00015     pc.printf("Starting HMC6352 test...\n");
00016     //left.speed(speed);
00017     //right.speed(speed);
00018     //Continuous mode, periodic set/reset, 20Hz measurement rate.
00019     compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
00020     wait(1);
00021     //init math vals
00022     curval = compass.sample()/10.0;
00023     minval = curval;
00024     maxval = curval;
00025     while (1) {
00026         count++; // for avg
00027         prevval = curval;
00028         curval = compass.sample()/10.0;
00029         dev = abs(curval - prevval);
00030         if (curval > maxval) {maxval = curval;}
00031         if (curval < minval) {minval = curval;}
00032         if (dev > maxdev)    {maxdev = dev;}    //largest deviation between two successive readings.
00033         avgdev += dev; //divide avg dev in print statement
00034         swing = maxval - minval; //swing is largest reading change possible from remaining stationary.
00035         if (count % 10 == 1) {
00036         pc.printf("Heading is: %f, Max Dev: %f, Avg Dev: %f, Swing: %f\n", curval, maxdev, avgdev/count, swing);
00037             //if (speed < .8) {speed += .05;
00038             //left.speed(speed);
00039             //right.speed(speed);}
00040         }
00041         wait(0.05);
00042     }
00043 
00044 }