Diff: main.cpp

Revision:

3:6a7620e9abd9

Parent:

2:56eb726bdb0d

diff -r 56eb726bdb0d -r 6a7620e9abd9 main.cpp
--- a/main.cpp	Tue Apr 22 23:16:32 2014 +0000
+++ b/main.cpp	Fri Apr 25 23:06:47 2014 +0000
@@ -1,48 +1,41 @@
 #include "mbed.h"
 #include "RobotControl.h"
 #include "rtos.h"
-
+ 
 #define ENTERCALIB 0x43
 #define EXITCALIB 0x45
 
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+DigitalOut rst1(p12);
+
+Serial xbee1(p13, p14);
+ 
 InterruptIn sw(p30);
-
+ 
 void StopISR()
 {
     rightMotorPWM = 0;
     leftMotorPWM = 0;
     exit(1);
 }
-
-void InitCompass() {
-    //Init compass
-    compass.setReset();
-    compass.setOpMode(HMC6352_CONTINUOUS, 1, 20);
+ 
+int main() {
     
-    //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() {
     //Emergency stop mechanism
     sw.rise(&StopISR);
-
-    //Setup mode and perform calibration
-    InitCompass();
     
-    compassDriveStraight(0.7, false, 20000);
+    // start positioning system
+    Thread position(PositionSystemMain, NULL, osPriorityNormal, 256 * 4);
+    wait(3); // wait for the mouse sensor to boot up
+    
+    // calibrate 90 degree turns
+    turnCalibrate();
     
-    while(1) {
-        float sample = compass.sample() / 10;
-        pc.printf("%f\r\n", sample);
-    }
-}
+    // PTURN_RIGHT is the measured x value for a 90 degree turn
+    pc.printf("PTURN_RIGHT: %f\n\r", PTURN_RIGHT); 
+    wait(1);
+    
+    // try a 90 degree turn with the measured PTURN_RIGHT value
+    turnRight(90);
+}
\ No newline at end of file