Diff: source/main.cpp

Revision:

32:777976c4d733

Parent:

31:51f52ffa4b51

Child:

33:8a98f8b9d859

diff -r 51f52ffa4b51 -r 777976c4d733 source/main.cpp
--- a/source/main.cpp	Tue Mar 14 16:24:15 2017 +0000
+++ b/source/main.cpp	Thu Mar 16 12:52:41 2017 +0000
@@ -1,89 +1,156 @@
-//#include "mbed.h"
-#include <stdio.h>
+#include "mbed.h"
+#include "pathfinding.h"
 
 
 
-int state = 0;
+int state = 10;
+bool positioning_state = 0;
+bool mapping_state = 0;
 
-int main() {
-    
-    init();
+int main()
+{
 
-    while (timer() < 300) // 5min
-    {
+
+    while (timer() < 300) { // 5min
         /*********************************************************************************
         Non_state machine driven function
         This functions will be called every cycle, use for safety and sensor functipons
         **********************************************************************************/
-/*      safty() {
+
+        safty() {
 
         }
 
         scanning() {
-        
+
         }
-*/
+
+        if(positioning_state) {
+            positioning();
+        }
+        if(mapping_state) {
+            mapping();
+        }
 
-    /*********************************************************************************
-    state machine driven function
-    This functions will only be called when there state is active.
-    The state machine will be in the same state until this action is completed or
-    a safty function kicks in and stops the current function.
-    Every function will return the next active state upon its transition table.
-    State and Transition Table will always be noted on the top of every state sheet
-    **********************************************************************************/
-        switch (state)
-        {
-        case 0:
-            //positioning
+        /*********************************************************************************
+        state machine driven function
+        This functions will only be called when their state is active.
+        The state machine will be in the same state until this action is completed or
+        a safty function kicks in and stops the current function.
+        Every function will return the next active state upon its transition table.
+        State and Transition Table can be found in the State_Machine.xlsx
+        **********************************************************************************/
+        switch (state) {
+            case 0:
+                state = emergency_shutdown();
+                break;
+            case 1:
+                state = colision_detected();
+                break;
+            case 2:
+                state = current_to_high();
+                break;
+            case 3:
+                state = overheating();
+                break;
 
-            break;
-        case 20:
-            //mapping
 
-            break;
-        case 40:
-            //a-star
+            case 10:
+                state = idel();
+                break;
+            case 11:
+                state = idel2();
+                break;
+
 
-            break;
-        case 60:
-            //moving
-
-            break;
-        case 80:
-            //grabing
+            case 15:
+                state = initialisation();
+                break;
+            case 16:
+                state = engage_motors();
+                break;
+            case 17:
+                state = test_servos();
+                break;
+            case 18:
+                state = inital_positioning();
+                break;
 
-            break;
-        case 100:
-            //something
 
-            break;
+            case 25:
+                state = start_positioning();
+                break;
+            case 26:
+                state = start_mapping();
+                break;
+            case 27:
+                state = initial_turn();
+                break;
 
-        case 120:
-            //something
 
-            break;
-        default:
-
-            printf("Fatal Error, Unkonwn state!");
-            break;
-        }
+            case 35:
+                state = select_target();
+                break;
+            case 36:
+                state = pathfinding();
+                break;
+            case 37:
+                state = remove_target();
+                break;
 
 
 
+            case 40:
+                state = moving();
+                break;
+            case 41:
+                state = moving_forward_for_distance();
+                break;
+            case 42:
+                state = moving_backward_for_distance();
+                break;
+            case 43:
+                state = turn_left_for_deg();
+                break;
+            case 44:
+                state = turn_right_for_deg();
+                break;
+
+            case 46:
+                state = move_to_next_coord();
+                break;
+            case 47:
+                state = move_in_search_for_brick();
+                break;
 
 
-    }
-
-    end_all();
-}
-
-int timer() {
+            case 50:
+                state = grabbing();
+                break;
+            case 51:
+                state = arm_move_up();
+                break;
+            case 52:
+                state = arm_move_down();
+                break;
+            case 53:
+                state = arm_move_to_hold();
+                break;
 
-}
-
-void init() {
+            case 55:
+                state = oben_grabber();
+                break;
+            case 56:
+                state = close_grabber();
+                break;
+            case 57:
+                state = test_color();
 
-}
-void end_all() {
+
+            default:
+                printf("Fatal Error, Unkonwn state!");
+                state = 0;
+                break;
+        }
+    }
 }
\ No newline at end of file