File content as of revision 32:777976c4d733:

#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;
        }
    }
}