File content as of revision 35:554c922f2bb5:

#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_position_move();
                              break;
                          case 52:
                              state = arm_position_grabbing();
                              break;
                          case 53:
                              state = arm_position_release();
                              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
}*/