Diff: source/Movement.cpp

Revision:

135:644346924339

Parent:

134:2e7875243597

Child:

136:b35f2d9b7402

--- a/source/Movement.cpp	Tue May 16 14:20:59 2017 +0000
+++ b/source/Movement.cpp	Tue May 16 15:24:23 2017 +0000
@@ -506,18 +506,31 @@
             rad = (rand()%1 + 1.0f) * 0.7f + 0.3f;
             rad *= sign;
             dist = (rand()%1 + 1.0f) * 0.5f + 0.5f;
+            printf("radius %f || distance %f\r\n", rad, dist);
             move_for_distance_with_radius(dist, rad);
             random_state = 1;
             break;
         case 1:
-            if (getDistanceIR(3) > 0.1f) {
-                if(move_for_distance_with_radius(0,0)) {
-                    random_state = 0;
-                    return 47;
-                }
-            } else {
+            float dist_ir = getDistanceIR(3);
+            printf("Distance ir3 %f\n\r",dist_ir);
+            if (dist_ir < 0.1f) {
+                /*move backwards*/
+                random_state = 2;
+            }
+
+            if(move_for_distance_with_radius(0,0) < 0 ) {
                 random_state = 0;
-                return 61; //swerve
+                return 47;
+            }
+            break;
+        case 2:
+            move_for_distance(-0.10f);
+            random_state = 3;
+            break;
+        case 3:
+            if(move_for_distance(0) < 0) {
+                random_state = 0;
+                return 61;
             }
             break;
     }
@@ -532,7 +545,7 @@
     switch (swerve_state) {
         case 0:
             set_servo_position(2, 45);
-            set_servo_position(4, -45);
+            set_servo_position(0, -45);
             wait(0.5f);
             dist_left = getDistanceIR(4);
             dist_right = getDistanceIR(2);