Diff: AI.cpp

Revision:

5:210cd333f770

Child:

6:3fb9f96765f6

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/AI.cpp	Wed Apr 30 05:53:51 2014 +0000
@@ -0,0 +1,53 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "IMU_RPY.h"
+#include "GameCode.h"
+#include "AIPosition.h"
+#include "AIControl.h"
+
+InterruptIn sw(p30);
+
+void StopISR()
+{
+    rightMotorPWM = 0;
+    leftMotorPWM = 0;
+    exit(1);
+}
+
+int main()
+{
+    pc.printf("\n\rStartup...\r\n");
+
+    //Emergency stop mechanism
+    sw.rise(&StopISR);
+
+    //Reset xbee
+    pc.printf("Resetting XBee...\r\n");
+    rst1 = 0;
+    wait_us(1);
+    rst1 = 1;
+    wait_us(1);
+
+    //Initialize IMU and filter
+    pc.printf("Initializing IMU...\r\n");
+    imuFilter.reset();
+    rpy_init();
+
+    // start positioning system
+    pc.printf("Starting Positioning System...\n\r");
+    Thread position(PositionSystemMain, NULL, osPriorityRealtime, 256 * 4);
+    wait(3); // wait for the mouse sensor to boot up
+
+    // get away from user before starting game
+    pc.printf("Getting head start (straight)...\n\r");
+    wait(5);
+    // gyroDriveStraight(0.7, false, 10000);
+    // pc.printf("Getting head start (turn right)...\n\r");
+    // centerTurnRight(90);
+    
+    // bursts of 10 seconds for driving straight
+    pc.printf("Running!\r\n");
+    while(!gameOver) {
+       gyroDriveStraight(0.7, true, 10000);
+    }
+}
\ No newline at end of file