File content as of revision 33:8a98f8b9d859:

#include "mbed.h"
#include "pathfinding.h"



int state = 10;
bool positioning_state = 0;
bool mapping_state = 0;

int main()
{


    while (timer() < 300) {
        */// 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();


            default:
                printf("Fatal Error, Unkonwn state!");
                state = 0;
                break;
        }
    }
}