Diff: source/Main.cpp

Revision:

135:644346924339

Parent:

133:0046d2224e39

Child:

136:b35f2d9b7402

diff -r 2e7875243597 -r 644346924339 source/Main.cpp
--- a/source/Main.cpp	Tue May 16 14:20:59 2017 +0000
+++ b/source/Main.cpp	Tue May 16 15:24:23 2017 +0000
@@ -23,7 +23,7 @@
         //This functions will be called every cycle, use for safety and sensor functipons
         //*******************************************************************************
 
-        state = safety(state);
+        //state = safety(state);
 
 
 
@@ -43,7 +43,7 @@
         //State and Transition Table can be found in the State_Machine.xlsx
         //*******************************************************************************
 
-        if(state != old_state){
+        if(state != old_state) {
             printf("state: %d\r\n",state);
             old_state = state;
         }
@@ -74,116 +74,116 @@
             case 16:
                 state = inital_arm_positioning();
                 break;
-            /*case 17:
-                state = initial_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 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 35:
+                                state = select_target();
+                                break;
+                            case 36:
+                                state = pathfinding();
+                                break;
+                            case 37:
+                                state = switch_target_red();
+                                break;
 
 
-            case 40:
-                state = moving();
-                break;
+                            case 40:
+                                state = moving();
+                                break;
 
 
-            case 45:
-                state = move_to_brick_by_coordlist();
-                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 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);
-
+                /*
+                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);
+                */
         }
     }
 }
\ No newline at end of file