Diff: main.cpp

Revision:

2:56eb726bdb0d

Parent:

1:41cee26b35cc

Child:

3:6a7620e9abd9

--- 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);
     }
 }