Diff: main.cpp

Revision:

22:7ba09c0af0d0

Parent:

21:15da49f18c63

Child:

23:1901cb6d0d95

--- a/main.cpp	Tue May 01 16:54:44 2012 +0000
+++ b/main.cpp	Thu May 03 14:20:04 2012 +0000
@@ -16,18 +16,19 @@
 //Interface declaration
 Serial pc(USBTX, USBRX); // tx, rx
 
+DigitalIn StartTrig(p12);
+Ticker StopTicker;
+
 Motors motors;
 UI ui;
 Kalman kalman(motors,ui,p23,p14,p14,p14,p15,p15,p15,p5,p6,p7,p8,p11);
 AI ai;
 Motion motion(motors, ai, kalman);
 
-//TODO mutex on kalman state, and on motor commands (i.e. on the i2c bus)
-
-
 void vMotorThread(void const *argument);
 void vPrintState(void const *argument);
 void motion_thread(void const *argument);
+void vStop (void);
 
 //Main loop
 int main() {
@@ -38,28 +39,28 @@
     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");
 
     //REMEMBERT TO PUT PULL UP RESISTORS ON I2C!!!!!!!!!!!!!!
     while (1) {
-    // we use main loop to estimate the cpu usage
-        
+        // we use main loop to estimate the cpu usage
+
         osThreadSetPriority (osThreadGetId(), osPriorityIdle);
-        
+
         Timer timer;
         ui.regid(10, 1);
-        
-        while(1) {
+
+        while (1) {
             timer.reset();
             timer.start();
             nopwait(1000);
-            
+
             ui.updateval(10, timer.read_us());
         }
-        
+
         // do nothing
         //Thread::wait(osWaitForever);
     }
@@ -82,44 +83,85 @@
 
     flag_terminate = true;
     */
-    
-    
-    //
-    // Put some code here so it's started by the pull trigger
-    // start a 90s timer here as well
-    //
-    //while (!Tiggered);
-    
+
+    printf("Waiting for the trigger pull ....\r\n");
+
+    // wait for the start triger
+    while (StartTrig) {
+        Thread::wait(10);
+    };
+
+    // attach a 90 seconds stop timer
+    StopTicker.attach(&vStop, 90);
+
+
     // starts motors
     ai.flag_motorStop = false;
-    // strat 1 ==================================
+#ifdef STARTLOC_RED
+    // strat 1 RED ==================================
     // goto middle x
     settarget(1500, 250, PI/2, true);
     Thread::signal_wait(0x01);
     Thread::wait(2000);
-    
+
     // to palm tree
     settarget(1500, 1000, PI, true);
     Thread::signal_wait(0x01);
     Thread::wait(2000);
-    
+
     // run over totem
     settarget(640,1000,PI, true);
     Thread::signal_wait(0x01);
     Thread::wait(2000);
-    
+
     // back to ship
     settarget(220,780,PI,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);
+
+    // 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);
+#endif
+
+// going from ship to ship for the remaining secs
+    while (true){
+    // back to ship, RED
+    settarget(220,780,PI,true);
+    Thread::signal_wait(0x01);
+    Thread::wait(2000);
     
+    // back to ship, BLUE
+    settarget(3000-220,780,0,true);
+    Thread::signal_wait(0x01);
+    Thread::wait(2000);
+    }
+
     // terminate thread, stopps motors permanently
     ai.flag_terminate = true;
-    while(true){
+    while (true) {
         Thread::wait(osWaitForever);
     }
-    
-    
+
+
     // end of strat 1 ===========================
 }
 
@@ -152,7 +194,7 @@
     float SonarMeasures[3];
     float IRMeasures[3];
 
-
+    Thread::wait(5000);
     while (1) {
         kalman.statelock.lock();
         state[0] = kalman.X(0);
@@ -166,9 +208,18 @@
         IRMeasures[2] = kalman.IRMeasures[2];
         kalman.statelock.unlock();
         pc.printf("\r\n");
-        pc.printf("current: %0.4f %0.4f %0.4f \r\n", state[0], state[1],state[2]);
-        pc.printf("Sonar: %0.4f %0.4f %0.4f \r\n",SonarMeasures[0],SonarMeasures[1],SonarMeasures[2]);
+        pc.printf("current: %0.4f %0.4f %0.4f \r\n", state[0]*1000, state[1]*1000,state[2]*180/PI);
+        pc.printf("Sonar: %0.4f %0.4f %0.4f \r\n",SonarMeasures[0]*1000,SonarMeasures[1]*1000,SonarMeasures[2]*1000);
         pc.printf("IR   : %0.4f %0.4f %0.4f \r\n",IRMeasures[0]*180/PI,IRMeasures[1]*180/PI,IRMeasures[2]*180/PI);
         Thread::wait(100);
     }
 }
+
+void vStop (void) {
+//    while (true) {
+        motors.stop();
+        ai.flag_motorStop = true;
+        // terminate thread, stopps motors permanently
+        ai.flag_terminate = true;
+//    };
+}
\ No newline at end of file