Diff: functions.cpp

Revision:

11:b45798cc3c10

Parent:

10:cec68ef272cd

Child:

14:e639a42edf2e

--- a/functions.cpp	Sat May 23 17:48:01 2015 +0000
+++ b/functions.cpp	Sat May 23 19:31:37 2015 +0000
@@ -106,8 +106,8 @@
 void moverandom()
 {
     int counter;
-    int dir;
-    int direction;
+    int fwd_bck;
+    int fwd;
     float random_time;
     float random_speed;
 
@@ -118,28 +118,31 @@
     {
         random_time = rand() % 1000 + 1000;
 
-        dir = rand()/(RAND_MAX);
+        fwd_bck = rand()%2;
         random_speed = (float)rand()/(float)(RAND_MAX);
-        if (random_speed < 0.5) {
-            random_speed = 0.5;
-        } else if (random_speed > 0.8) {
-            random_speed = 0.8;
-        }
         
-        if (dir == 0) {
-            direction = rand()/(RAND_MAX);
-            if  (direction == 1) { 
-                charge(random_speed);
-                wait_ms(random_time);
+        if (random_speed < 0.6) {
+            random_speed = 0.5;
+        } else if (random_speed >= 0.9) {
+            random_speed = 0.9;
+        }
+        // If fwd_back == 1 move forward or backwards
+        if (fwd_bck == 1) {
+            printf("Moving forward\n");
+            printf("%d\n",fwd_bck);
+            fwd = rand()%2;
+            // If fwd == 1 move forward
+            if  (fwd == 1) { 
+                move_detect(random_speed, fwd_bck, random_time);
+            // If fwd == 0 move bacward
             } else {
-                reverse(random_speed);
-                wait_ms(random_time);
+                move_detect(-random_speed, fwd_bck, random_time);
             }   
+        // Turn
         } else {
-            turn(random_speed);
-            wait_ms(random_time);
+            printf("Turning\n");
+            move_detect(random_speed, fwd_bck, random_time);
         }
-        return;
     }
 }
 
@@ -160,7 +163,7 @@
     dist = 0;
     reverse = 0;
         
-    while (usensor_t.read_ms() < 15000) {
+    while (usensor_t.read_ms() < 5000) {
         // Start the ultrasonic sensor
         usensor.start();
         inner_t.start();
@@ -190,4 +193,32 @@
     usensor_t.stop();
     usensor_t.reset();
     return sense;
+}
+
+void move_detect(float speed, int fwd_bck, int time) {
+    Timer t;
+    t.start();
+    int detect = 0;
+    
+    while (t.read_ms() < time) {
+        if(fwd_bck == 1) {
+            charge(speed);
+        } else {
+            turn(speed);
+        }
+        detect = detect_line();
+        // If line is detected from front then reverse
+        if(detect == 1) {
+            reverse(speed);
+            wait(2);
+            stop();
+            break;
+            // If line is detected from back just keep on moving forward
+        } else if (detect == -1) {
+            wait(2);
+            stop();
+            break;
+        }
+    }
+    t.stop();
 }
\ No newline at end of file