StingerBot Project
Dependencies: mbed HMC6352 mbed-rtos
Diff: main.cpp
- Revision:
- 2:56eb726bdb0d
- Parent:
- 1:41cee26b35cc
--- a/main.cpp Fri Apr 11 17:54:21 2014 +0000 +++ b/main.cpp Tue Apr 22 23:16:32 2014 +0000 @@ -1,13 +1,48 @@ #include "mbed.h" #include "RobotControl.h" +#include "rtos.h" + +#define ENTERCALIB 0x43 +#define EXITCALIB 0x45 + +InterruptIn sw(p30); + +void StopISR() +{ + rightMotorPWM = 0; + leftMotorPWM = 0; + exit(1); +} + +void InitCompass() { + //Init compass + compass.setReset(); + compass.setOpMode(HMC6352_CONTINUOUS, 1, 20); + + //Calibration level - Garrus + //Rotate robot one full rotation during this section + pc.printf("Begin Calibration...\r\n"); + compass.setCalibrationMode(ENTERCALIB); + wait(10); + compass.setCalibrationMode(EXITCALIB); + + //Sample a few to clean out buffer + compass.sample(); + compass.sample(); + compass.sample(); +} int main() { - //Init compass - compass.setOpMode(HMC6352_CONTINUOUS, 1, 20); - wait(2); - //centerTurnLeft(25); - stop(); + //Emergency stop mechanism + sw.rise(&StopISR); + + //Setup mode and perform calibration + InitCompass(); + + compassDriveStraight(0.7, false, 20000); while(1) { + float sample = compass.sample() / 10; + pc.printf("%f\r\n", sample); } }