Diff: source/Main.cpp

Revision:

71:ddf4eb5c3081

Parent:

69:1fdcef6a7577

Child:

72:4e8a151d804e

--- a/source/Main.cpp	Thu Apr 20 07:36:50 2017 +0000
+++ b/source/Main.cpp	Thu Apr 20 11:37:24 2017 +0000
@@ -5,14 +5,14 @@
 bool positioning_state = 0;
 bool mapping_state = 0;
 
+Timer game_timer;
+
 int IMU();
 
 int main()
 {
     printf("start...\r\n");
-
-    // while (timer() < 300) {
-    while (1) {
+    while (game_timer.read() < 300) {
         wait(0.01f);
         // 5min
         //*******************************************************************************
@@ -23,7 +23,7 @@
         safety();
 
         //scanning();
-        
+
 
 
         if(positioning_state) {
@@ -41,6 +41,7 @@
         //Every function will return the next active state upon its transition table.
         //State and Transition Table can be found in the State_Machine.xlsx
         //*******************************************************************************
+        printf("state: %d\r\n",state);
         switch (state) {
             case 0:
                 state = emergency_shutdown();
@@ -57,12 +58,12 @@
 
 
             case 10:
-                state = idel();
-                printf("idel1...\r\n");
+                state = idle();
+                printf("idle1...\r\n");
                 break;
             case 11:
-                state = idel2();
-                printf("idel2...\r\n");
+                state = idle2();
+                printf("idle2...\r\n");
                 break;
 
 
@@ -70,21 +71,25 @@
                 state = initialisation();
                 break;
             case 16:
-                //arm_position_release();
-                //open_grabber();
-                state = inital_positioning();
+                state = initial_positioning();
                 break;
-
+            case 17:
+                state = inital_arm_positioning();
+                break;
 
             case 25:
                 state = 26;
-                positioning_state = 1;
+                game_timer.start();
                 break;
             case 26:
                 state = 27;
+                positioning_state = 1;
+                break;
+            case 27:
+                state = 28;
                 mapping_state = 1;
                 break;
-            case 27:
+            case 28:
                 state = initial_turn();
                 break;
 
@@ -147,64 +152,6 @@
                 state = 53;
                 wait_ms(1000);
                 break;
-
-/*
-            case 99:
-                sate = init();
-                break;
-            case 100:
-                state = turn_left(30);
-                break;
-            case 101:
-                state = turn_right_for_brick;
-                break;
-            case 102:
-                state = move_to_brick;
-*/
-
-
-//test states
-            case 200:
-                state = IMU();
-                break;
-            default:
-                printf("Fatal Error, Unkonwn state!");
-                state = 0;
-                break;
         }
     }
-}
-
-int IMU()
-{
-    printf("%f\r\n", read_heading());
-    return 200;
-}
-
-int init()
-{
-    Robot_init_all();
-    return 100;
-}
-
-
-/*
-int turn_left(float deg)
-{
-    bool in_function = false;
-    if(in_function = false) {
-        float current_deg = read_heading();
-        set_speed(-50, 50);
-        in_function = true;
-    }
-    if(current_deg - deg < read_heading() {
-    set_speed(0, 0);
-        return 101;
-        in_function = false;
-    }
-    return 100;
-}
-
-int turn_right_for_brick {
-    return 0
-}*/
\ No newline at end of file
+}
\ No newline at end of file