Diff: main.cpp

Revision:

25:143b19c1fb05

Parent:

24:7a3906c2f5d5

--- a/main.cpp	Fri May 04 05:23:45 2012 +0000
+++ b/main.cpp	Wed Oct 17 22:22:28 2012 +0000
@@ -11,17 +11,21 @@
 #include "ai.h"
 #include "ui.h"
 #include "front_arms.h"
+#include "motion.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)
+DigitalIn ColourToggle(p22); //high for red, low for blue(purple)
 Ticker StopTicker;
 
 Motors motors;
@@ -36,14 +40,17 @@
 void vStop (void);
 
 //Main loop
-int main() {      
+int main() {
+    AnalogIn ObsAvoidPin(p20);
     // no motor motions till we pull the trig
     ai.flag_motorStop = true;
-    nopwait(1000);
-    Colour = ColourToggle;
+    Colour = !(ObsAvoidPin > 0.5);
+    OLED3 = Colour;
+    //nopwait(1000);
+    //Colour = ColourToggle;
     // re-defines beacon positions by the toggle switch
     kalman.statelock.lock();
-    if (Colour) {
+    if (true) {
         beaconpos[0].x = 3000;
         beaconpos[0].y = 1000;
         beaconpos[1].x = 0;
@@ -65,7 +72,7 @@
     pc.baud(115200);
     ArmsEnable();
     ArmsClose();
-    
+
 
     //Init kalman, this should be done in the mid of the arena before the game starts
     kalman.KalmanInit();
@@ -76,9 +83,9 @@
 
     pc.printf("We got to main! ;D\r\n");
     if (Colour)
-    printf("I'm in Red \n\r");
+        printf("I'm in Red \n\r");
     else
-    printf("I'm in Blue \n\r");
+        printf("I'm in Blue \n\r");
 
     //REMEMBERT TO PUT PULL UP RESISTORS ON I2C!!!!!!!!!!!!!!
     while (1) {
@@ -103,6 +110,8 @@
 }
 
 void AI::ai_thread () {
+
+    motors.accelerationRegister = 1;
     /*
     //printf("aithreadstart\r\n");
     Thread::signal_wait(0x01);
@@ -126,91 +135,120 @@
     while (!StartTrig) {
         Thread::wait(10);
     };
-    
+
     printf("GO! \r\n");
     kalman.KalmanReset();
     Thread::wait(100);
-    
-    
+
+
 
     // attach a 87 seconds stop timer
-    
-    
+
+
     //REPLACE TICKER!!!!
     StopTicker.attach(&vStop, 87);
 
 
     // starts motors
     ai.flag_motorStop = false;
-    
+
     // no override
     ai.flag_manOverride = false;
-      
+
 
 //if (Colour){
     // strat 1 RED ==================================
     ArmsOpen();
-    //Thread::wait(500);    
-        
+    //Thread::wait(500);
+
     // goto middle x
     settarget(1500, 250, PI/2, true,Colour, 35);
     Thread::signal_wait(0x01);
     Thread::wait(2000);
-        
+
     // to palm tree
     settarget(1500, 1000, PI, true,Colour, 35);
     Thread::signal_wait(0x01);
-    Thread::wait(2000);
+    Thread::wait(4000);
 
     // run over totem
-    settarget(800,1050,PI, true,Colour, 60);
+    settarget(840,1000,PI, true,Colour, 80);
+    motors.accelerationRegister = 0;
+    Thread::wait(5000);
+    //Thread::signal_wait(0x01);
+
+    while (hypot(kalman.X(0) - 1.1f, kalman.X(1) - 1.0f) < 0.10) {
+        // to palm tree
+        settarget(1500, 1000, PI, false,Colour, 35);
+        Thread::signal_wait(0x01);
+        Thread::wait(4000);
+
+        // run over totem
+        settarget(840,1000,PI, true,Colour, 80);
+        motors.accelerationRegister = 0;
+        Thread::wait(5000);
+    }
     Thread::signal_wait(0x01);
-    Thread::wait(2000);
 
+    motors.accelerationRegister = 1;
+   
+    // back to ship
+    settarget(220,1000,0,true,Colour, 50);
+    Thread::signal_wait(0x01);
+    
     ArmsClose();
-    // back to ship
-    settarget(220,780,PI,true,Colour, 35);   
+    
+    settarget(840,1000,PI, true,Colour, 40);
     Thread::signal_wait(0x01);
-    Thread::wait(2000);
+    
+     
+     
+     settarget(220,1000,PI,true,Colour, 40);
+    Thread::signal_wait(0x01);
+    
+     settarget(840,1000,PI,false,Colour, 40);
+    Thread::signal_wait(0x01);
+    
+   
 //}
 
-/*else{
-    // strat 1 BLUE ==================================
-    // goto middle x
-    settarget(3000-1500, 250, PI/2, true);
-    Thread::signal_wait(0x01);
-    Thread::wait(2000);
-
-    // to palm tree
-    settarget(3000-1500, 1000, 0, true);
-    Thread::signal_wait(0x01);
-    Thread::wait(2000);
-
-    // run over totem
-    settarget(3000-640,1000,0, true);
-    Thread::signal_wait(0x01);
-    Thread::wait(2000);
+    /*else{
+        // strat 1 BLUE ==================================
+        // goto middle x
+        settarget(3000-1500, 250, PI/2, true);
+        Thread::signal_wait(0x01);
+        Thread::wait(2000);
 
-    // back to ship
-    settarget(3000-220,780,0,true);
-    Thread::signal_wait(0x01);
-    Thread::wait(2000); 
-}
-*/
-/*
-// going from ship to ship for the remaining secs
-    while (true){
-    // back to home, RED
-    settarget(500,400,PI,true);
-    Thread::signal_wait(0x01);
-    Thread::wait(2000);
-    
-    // back to ship, BLUE
-    settarget(500,1600,0,true);
-    Thread::signal_wait(0x01);
-    Thread::wait(2000);
+        // to palm tree
+        settarget(3000-1500, 1000, 0, true);
+        Thread::signal_wait(0x01);
+        Thread::wait(2000);
+
+        // run over totem
+        settarget(3000-640,1000,0, true);
+        Thread::signal_wait(0x01);
+        Thread::wait(2000);
+
+        // back to ship
+        settarget(3000-220,780,0,true);
+        Thread::signal_wait(0x01);
+        Thread::wait(2000);
     }
     */
+    /*
+    // going from ship to ship for the remaining secs
+        while (true){
+        // back to home, RED
+        settarget(500,400,PI,true);
+        Thread::signal_wait(0x01);
+        Thread::wait(2000);
+
+        // back to ship, BLUE
+        settarget(500,1600,0,true);
+        Thread::signal_wait(0x01);
+        Thread::wait(2000);
+        }
+        */
 
     // terminate thread, stopps motors permanently
     ai.flag_terminate = true;
@@ -274,9 +312,9 @@
 
 void vStop (void) {
 //    while (true) {
-        motors.coastStop();   
-        ai.flag_motorStop = true;
-        // terminate thread, stopps motors permanently
-        ai.flag_terminate = true;
+    motors.coastStop();
+    ai.flag_motorStop = true;
+    // terminate thread, stopps motors permanently
+    ai.flag_terminate = true;
 //    };
 }
\ No newline at end of file