Eurobot2012_Primary
Dependencies: mbed Eurobot_2012_Primary
Diff: main.cpp
- Revision:
- 23:1901cb6d0d95
- Parent:
- 22:7ba09c0af0d0
- Child:
- 24:7a3906c2f5d5
diff -r 7ba09c0af0d0 -r 1901cb6d0d95 main.cpp --- a/main.cpp Thu May 03 14:20:04 2012 +0000 +++ b/main.cpp Fri May 04 02:50:07 2012 +0000 @@ -10,13 +10,18 @@ #include "motion.h" #include "ai.h" #include "ui.h" +#include "front_arms.h" //#include <iostream> //Interface declaration Serial pc(USBTX, USBRX); // tx, rx +bool Colour = 1; // 1 for red, 0 for blue +pos beaconpos[] = {{3000, 1000},{0,0}, {0,2000}}; //predefined red start + DigitalIn StartTrig(p12); +DigitalIn ColourToggle(p16); //high for red, low for blue(purple) Ticker StopTicker; Motors motors; @@ -32,17 +37,48 @@ //Main loop int main() { - pc.baud(115200); // no motor motions till we pull the trig ai.flag_motorStop = true; + + Colour = ColourToggle; + // re-defines beacon positions by the toggle switch + kalman.statelock.lock(); + if (Colour) { + beaconpos[0].x = 3000; + beaconpos[0].y = 1000; + beaconpos[1].x = 0; + beaconpos[1].y = 0; + beaconpos[2].x = 0; + beaconpos[2].y = 2000; + //beaconpos[] = {{3000, 1000},{0,0}, {0,2000}}; + } else { + beaconpos[0].x = 0; + beaconpos[0].y = 1000; + beaconpos[1].x = 3000; + beaconpos[1].y = 0; + beaconpos[2].x = 3000; + beaconpos[2].y = 2000; + //beaconpos[] = {{0, 1000},{3000,0}, {3000,2000}}; + } + kalman.statelock.unlock(); + + pc.baud(115200); + ArmsEnable(); + ArmsClose(); + + //Init kalman, this should be done in the mid of the arena before the game starts kalman.KalmanInit(); //Thread tMotorThread(vMotorThread,NULL,osPriorityNormal,256); - Thread tUpdateState(vPrintState,NULL,osPriorityNormal,1024); + //Thread tUpdateState(vPrintState,NULL,osPriorityNormal,1024); pc.printf("We got to main! ;D\r\n"); + if (Colour) + printf("I'm in Red \n\r"); + else + printf("I'm in Blue \n\r"); //REMEMBERT TO PUT PULL UP RESISTORS ON I2C!!!!!!!!!!!!!! while (1) { @@ -87,39 +123,56 @@ printf("Waiting for the trigger pull ....\r\n"); // wait for the start triger - while (StartTrig) { + while (!StartTrig) { Thread::wait(10); }; + + printf("GO! \r\n"); + kalman.KalmanReset(); + Thread::wait(100); + + - // attach a 90 seconds stop timer - StopTicker.attach(&vStop, 90); + // attach a 87 seconds stop timer + StopTicker.attach(&vStop, 87); // starts motors ai.flag_motorStop = false; -#ifdef STARTLOC_RED + + // no override + ai.flag_manOverride = false; + + +//if (Colour){ // strat 1 RED ================================== + ArmsOpen(); + Thread::wait(500); + // goto middle x - settarget(1500, 250, PI/2, true); + settarget(1500, 250, PI/2, true,Colour); Thread::signal_wait(0x01); Thread::wait(2000); + // to palm tree - settarget(1500, 1000, PI, true); + settarget(1500, 1050, PI, true,Colour); Thread::signal_wait(0x01); Thread::wait(2000); // run over totem - settarget(640,1000,PI, true); + settarget(640,1050,PI, true,Colour); Thread::signal_wait(0x01); Thread::wait(2000); + ArmsClose(); // back to ship - settarget(220,780,PI,true); + settarget(220,780,PI,true,Colour); Thread::signal_wait(0x01); Thread::wait(2000); +//} -#else +/*else{ // strat 1 BLUE ================================== // goto middle x settarget(3000-1500, 250, PI/2, true); @@ -139,18 +192,18 @@ // back to ship settarget(3000-220,780,0,true); Thread::signal_wait(0x01); - Thread::wait(2000); -#endif - + Thread::wait(2000); +} +*/ // going from ship to ship for the remaining secs while (true){ - // back to ship, RED - settarget(220,780,PI,true); + // back to home, RED + settarget(500,400,PI,true); Thread::signal_wait(0x01); Thread::wait(2000); // back to ship, BLUE - settarget(3000-220,780,0,true); + settarget(500,1600,0,true); Thread::signal_wait(0x01); Thread::wait(2000); } @@ -217,7 +270,7 @@ void vStop (void) { // while (true) { - motors.stop(); + motors.coastStop(); ai.flag_motorStop = true; // terminate thread, stopps motors permanently ai.flag_terminate = true;