AI.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "IMU_RPY.h"
00004 #include "GameCode.h"
00005 #include "AIPosition.h"
00006 #include "AIControl.h"
00007 
00008 InterruptIn sw(p30);
00009 
00010 void StopISR()
00011 {
00012     rightMotorPWM = 0;
00013     leftMotorPWM = 0;
00014     exit(1);
00015 }
00016 
00017 int main()
00018 {
00019     pc.printf("\n\rStartup...\r\n");
00020 
00021     //Emergency stop mechanism
00022     sw.rise(&StopISR);
00023 
00024     //Reset xbee
00025     pc.printf("Resetting XBee...\r\n");
00026     rst1 = 0;
00027     wait_us(1);
00028     rst1 = 1;
00029     wait_us(1);
00030 
00031     //Initialize IMU and filter
00032     pc.printf("Initializing IMU...\r\n");
00033     imuFilter.reset();
00034     rpy_init();
00035 
00036     // start positioning system
00037     pc.printf("Starting Positioning System...\n\r");
00038     Thread position(PositionSystemMain, NULL, osPriorityRealtime, 256 * 4);
00039     wait(3); // wait for the mouse sensor to boot up
00040 
00041     // get away from user before starting game
00042     pc.printf("Getting head start (straight)...\n\r");
00043     gyroDriveStraight(0.8, false, 5000);
00044     //wait(5);
00045     
00046     // gyroDriveStraight(0.7, false, 10000);
00047     // pc.printf("Getting head start (turn right)...\n\r");
00048     // centerTurnRight(90);
00049     
00050     // bursts of 10 seconds for driving straight
00051     pc.printf("Running!\r\n");
00052     while(!gameOver) {
00053        gyroDriveStraight(0.7, true, 10000);
00054     }
00055 }