Diff: source/Main.cpp

Revision:

34:40d8d29b44b8

Parent:

33:8a98f8b9d859

Child:

35:554c922f2bb5

Child:

36:b654afdf886e

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/source/Main.cpp	Wed Mar 29 12:10:32 2017 +0000
@@ -0,0 +1,206 @@
+#include "mbed.h"
+#include "Pathfinding.h"
+#include "Robot.h"
+
+
+int state = 10;
+bool positioning_state = 0;
+bool mapping_state = 0;
+
+int IMU();
+
+int main()
+{
+
+
+    // while (timer() < 300) {
+    while (1) {
+        // 5min
+        //*******************************************************************************
+        //Non_state machine driven function
+        //This functions will be called every cycle, use for safety and sensor functipons
+        //*******************************************************************************
+
+        /*     safty() {
+
+             }
+
+             scanning() {
+        }
+        */
+
+        if(positioning_state) {
+            // positioning();
+        }
+        if(mapping_state) {
+            // mapping();
+        }
+
+        //*******************************************************************************
+        //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;
+
+
+                          case 10:
+                              state = idel();
+                              break;
+                          case 11:
+                              state = idel2();
+                              break;
+
+
+                          case 15:
+                              state = initialisation();
+                              break;
+                          case 16:
+                              state = engage_motors();
+                              break;
+                          case 17:
+                              state = test_servos();
+                              break;
+                          case 18:
+                              state = inital_positioning();
+                              break;
+
+
+                          case 25:
+                              state = start_positioning();
+                              break;
+                          case 26:
+                              state = start_mapping();
+                              break;
+                          case 27:
+                              state = initial_turn();
+                              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;
+
+
+                          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;
+
+                          case 55:
+                              state = oben_grabber();
+                              break;
+                          case 56:
+                              state = close_grabber();
+                              break;
+                          case 57:
+                              state = test_color();
+
+                          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;
+
+            */          case 200:
+                state = IMU();
+                break;
+            default:
+                printf("Fatal Error, Unkonwn state!");
+                state = 0;
+                break;
+        }
+    }
+}
+
+int IMU()
+{
+    printf("%f", 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