Diff: source/Main.cpp

Revision:

113:c8ea0cd9b20c

Parent:

112:c7afe49752b9

Child:

119:eb212467afb0

--- a/source/Main.cpp	Tue May 09 13:58:57 2017 +0000
+++ b/source/Main.cpp	Tue May 09 15:21:26 2017 +0000
@@ -39,8 +39,8 @@
         //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);
         switch (state) {
             case 0:
@@ -148,7 +148,7 @@
                 state = 101;
                 break;
             case 102:
-                move_for_distance_with_radius(1,-0.19);
+                move_for_distance_with_radius(0.5f,-0.4);
                 state = 103;
                 break;
             case 103:
@@ -156,9 +156,24 @@
                 if(distance < 0) {
                     //printf("remaining deg %f\r\n", deg);
                     stop_turn();
-                    state = 11;
+                    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);
 
         }
     }