Diff: source/Movement.cpp

Revision:

134:2e7875243597

Parent:

133:0046d2224e39

Child:

135:644346924339

--- a/source/Movement.cpp	Tue May 16 13:36:36 2017 +0000
+++ b/source/Movement.cpp	Tue May 16 14:20:59 2017 +0000
@@ -23,6 +23,7 @@
 
 Timer t;
 Timer t8;                                                                       // timer used for waiting enough distance measurements
+Timer t9;
 
 int search_state = 0;
 int coord_move_state = 0;
@@ -305,7 +306,7 @@
             float x = (next_coord.x - current_coordinates.x) / 25.0f;
             float y = (next_coord.y - current_coordinates.y) / 25.0f;
             distance_to_next_coord = sqrt(x*x + y*y);
-            
+
             needed_heading = 90 + (atan(-y / x)/(float)M_PI * 180.0f)*-1.0f;
             if (x < 0) needed_heading += 180;
             printf("current heading %f\r\n", current_head);
@@ -315,7 +316,7 @@
             } else {
                 coord_move_state = 8;
             }
-            break;          
+            break;
 
         case 5:
             // turn init with new heading
@@ -478,7 +479,6 @@
             // error
             //if nothing found or something got wrong move random
             return 60;
-            break;
     }
     return 47;                                                                  //called until function is done
 }
@@ -486,7 +486,41 @@
 
 int move_random()
 {
-    return 47;  //move in search for brick
+    t9.start();
+    float sign = 1.0f;
+    float rad = 0.0f;
+    float dist = 0.0f;
+    float time = t9.read();
+    time *=100;
+    int time_sub = (int)time;
+    time -= (float)time_sub;
+    time /= 100;
+    srand(time);
+    switch (random_state) {
+        case 0:
+            if ((rand()%1 + 1.0f) < 0.5f) {
+                sign = -1.0f;
+            } else {
+                sign = 1.0f;
+            }
+            rad = (rand()%1 + 1.0f) * 0.7f + 0.3f;
+            rad *= sign;
+            dist = (rand()%1 + 1.0f) * 0.5f + 0.5f;
+            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 {
+                random_state = 0;
+                return 61; //swerve
+            }
+            break;
+    }
     return 60;  //recall
 }
 
@@ -495,13 +529,13 @@
 {
     int dist_left = 0;
     int dist_right = 0;
-    case(swerve_state) {
+    switch (swerve_state) {
         case 0:
             set_servo_position(2, 45);
             set_servo_position(4, -45);
-            wait(0.5f);        
+            wait(0.5f);
             dist_left = getDistanceIR(4);
-            dist_right = getDistanceIR(2);  
+            dist_right = getDistanceIR(2);
             if (dist_left < dist_right) {
                 swerve_state = 1;
             } else {
@@ -529,7 +563,9 @@
             if (move_for_distance(0) < 0) {
                 swerve_state = 0;
                 return 60;
-            break;    
+            }
+            break;
+    }
     return 61;  //recall
 }