Diff: main.cpp

Revision:

23:1901cb6d0d95

Parent:

22:7ba09c0af0d0

Child:

24:7a3906c2f5d5

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