File content as of revision 136:b35f2d9b7402:

#include "Main.h"


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



Timer game_timer;


int main()
{
    printf("start...\r\n");
    disable_servos();
    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
        //*******************************************************************************

        if (safety() && state != 50) { //arm down
            //irgendwie rückwärts fahren
            //state = 47
            state = 61;
        }



        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
        //*******************************************************************************

        if(state != old_state) {
            printf("state: %d\r\n",state);
            old_state = 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();
                break;
            case 11:
                state = idle2();
                break;

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

            case 25:
                state = 28;
                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 45:
                                state = move_to_brick_by_coordlist();
                                break;
                                */
            case 47:
                state = move_in_search_for_brick();
                break;

                /*
                            case 48:
                                state = generate_fake_target();
                                break;
                */

            case 50:
                state = grabbing();
                break;

            case 60:
                state = move_random();
                break;
            case 61:
                state = swerve_go();
                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 oben: %f \r\n", getDistanceIR(2));
                    printf("sensor unten: %f \r\n\n", getDistanceIR(3));
                //               printf("sensor 4: %f \r\n", getDistanceIR(4));
                //               printf("sensor 5: %f \r\n\n", getDistanceIR(5));
                    wait(0.1f);
                    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;
                    break;
                case 102:
                    move_for_distance_with_radius(0.5f,-0.4);
                    state = 103;
                    break;
                case 103:
                    float distance = move_for_distance_with_radius(0,0);
                    if(distance < 0) {
                        //printf("remaining deg %f\r\n", deg);
                        stop_turn();
                        state = 104;
                    }
                    break;
                case 104:
                    move_for_distance_with_radius(0.5f,0.4);
                    state = 105;
                    break;
                case 105:
                    float distance2 = move_for_distance_with_radius(0,0);
                    if(distance2 < 0) {
                        //printf("remaining deg %f\r\n", deg);
                        stop_turn();
                        state = 106;
                    }
                    break;
                case 106:
                    coordinates co = get_current_coord();
                    printf("current_coord: %f || %f\r\n", co.x, co.y);
                */
        }
    }
}