File content as of revision 90:7f9d6e641a01:

#include "Main.h"


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

Timer game_timer;

int main()
{
    printf("start...\r\n");
    while (game_timer.read() < 300) {
        wait(0.01f);
        // 5min
        //*******************************************************************************
        //Non_state machine driven function
        //This functions will be called every cycle, use for safety and sensor functipons
        //*******************************************************************************

        safety();

        //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
        //*******************************************************************************
        //printf("state: %d\r\n",state);
        printf("State - %d\r\n",state);
        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 = idle();
                printf("idle1...\r\n");
                break;
            case 11:
                state = idle2();
                printf("idle2...\r\n");
                break;


            case 15:
                state = initialisation();   //init servo is included
                state = 103;
                break;
            case 16:
                state = inital_arm_positioning();

                break;
            case 17:
                state = initial_positioning();
                break;

            case 25:
                state = 26;
                game_timer.start();
                break;
            case 26:
                state = 27;
                positioning_state = 1;
                break;
            case 27:
                state = 28;
                mapping_state = 1;
                break;
            case 28:
                state = initial_turn();
                break;


            case 35:
                state = select_target();
                break;
            case 36:
                state = pathfinding();
                break;
            case 37:
                state = switch_target_red();
                break;


            case 40:
                state = moving();
                break;


            case 46:
                state = move_to_next_coord();
                break;
            case 47:
                state = move_in_search_for_brick();
                break;
            case 48:
                state = generate_fake_target();
                break;


            case 50:
                state = grabbing();
                break;
            case 51:
                state = arm_position_move();
                break;
            case 52:
                arm_position_grabbing();
                state = 56;
                wait_ms(1000);
                break;
            case 53:
                state = arm_position_release();
                state = 55;
                wait_ms(1000);
                break;

            case 55:
                state = open_grabber();
                state = 52;
                wait_ms(1000);
                break;
            case 56:
                state = close_grabber();
                state = 53;
                wait_ms(1000);
                break;

                /******************************************************************************/
//Testtools

            case 100:   //Testtool for IR sensors
                printf("sensor 0: %f \r\n", getDistanceIR(0));
                printf("sensor 1: %f \r\n", getDistanceIR(1));
                printf("sensor 2: %f \r\n", getDistanceIR(2));
                printf("sensor 3: %f \r\n", getDistanceIR(3));
                printf("sensor 4: %f \r\n", getDistanceIR(4));
                printf("sensor 5: %f \r\n\n", getDistanceIR(5));
                state = 100;
                break;
            case 101:   //Testtool for get_color
                char str1[] = "GREEN";
                char str2[] = "RED";
                printf("Color: %s\r\n",get_color()? str1 : str2);
                wait_ms(500);
                state = 101;

            case 102:   //Testtool for grabbing functions
                arm_position_release();
                wait_ms(500);
                open_grabber();
                wait_ms(500);
                arm_position_grabbing();
                wait_ms(1000);
                //move_forward_for_distance(5.0);
                close_grabber();
                wait_ms(500);
                if (get_color() == 0) {
                    open_grabber();
                }
                wait_ms(500);
                state = 102;
                break;

            case 103:
                move_for_distance(-1.00f);
                state = 104;
                break;
            case 104:
                if(move_for_distance(0) < 0.0f) {
                    stop_move();
                    state = 0;
                }
                break;
        }
    }
}