Diff: functions.cpp

Revision:

19:67ea4e8be9e1

Parent:

17:47aa9ef2bec6

Child:

20:37a89edd1cde

diff -r 7bd638e3926d -r 67ea4e8be9e1 functions.cpp
--- a/functions.cpp	Sat May 30 16:18:31 2015 +0000
+++ b/functions.cpp	Sat May 30 19:08:26 2015 +0000
@@ -7,6 +7,11 @@
 #include "hcsr04.h"
 #include "functions.h"
 
+DigitalOut myled1(LED1);
+DigitalOut myled2(LED2);
+DigitalOut myled3(LED3);
+DigitalOut myled4(LED4);
+
 // Two sensors are used to detect a line, line_sens1 and line_sens2
 // if there is a difference between the readings of these sensors 
 // the robot has detected a line.
@@ -23,6 +28,43 @@
 // Example: usensor(Trigger, Echo)
 HCSR04 usensor(p25,p26);
 
+void turn_leds_on() {
+        myled1 = 1;
+        myled2 = 1;
+        myled3 = 1;
+        myled4 = 1;
+        wait(0.1);
+        myled1 = 0;
+        myled2 = 0;
+        myled3 = 0;
+        myled4 = 0;
+}
+
+void flash_leds() {
+    Timer t;
+    t.start();
+    while(t.read_ms() < 1000) {
+        myled1 = 0;
+        myled2 = 0;
+        myled3 = 0;
+        myled4 = 0;
+        wait(0.15);
+        myled1 = 1;
+        wait(0.15);
+        myled2 = 1;
+        wait(0.15);
+        myled3 = 1;
+        wait(0.15);
+        myled4 = 1;
+        wait(0.15);
+        myled1 = 0;
+        myled2 = 0;
+        myled3 = 0;
+        myled4 = 0;
+    }
+    t.stop();
+}
+
 // Returns value from the QRE1113
 // lower numbers mean more refleacive
 // more than 3000 means nothing was reflected.
@@ -98,7 +140,7 @@
     MotorRight.speed(-(speed-0.1));
 }
 
-void charge(float speed) {
+void move_forward(float speed) {
     MotorLeft.speed(speed);
     MotorRight.speed(speed);
 }
@@ -112,20 +154,23 @@
     float random_speed;
 
     printf("Moving randomly\n");
-    counter = rand() % 10;
+    counter = rand() % 5;
 
     for (int i = 0; i < counter; i++)
     {
         random_time = rand() % 1000 + 1000;
 
         fwd_bck = rand()%2;
-        random_speed = (float)rand()/(float)(RAND_MAX);
+        //random_speed = (float)rand()/(float)(RAND_MAX);
         
-        if (random_speed < 0.6) {
-            random_speed = 0.5;
-        } else if (random_speed >= 0.9) {
-            random_speed = 0.9;
-        }
+        //if (random_speed < 0.6) {
+            //random_speed = 0.5;
+        //} else if (random_speed >= 0.8) {
+            //random_speed = 0.8;
+        //}
+        
+        random_speed = 0.5;
+        
         // If fwd_back == 1 move forward or backwards
         if (fwd_bck == 1) {
             fwd = rand()%2;
@@ -172,6 +217,8 @@
         // If an object is detected based on out set range return 1
         if (dist <= range && dist >= 1) {
             sense = 1;
+            stop();
+            flash_leds();
             break;
         } else {
             sense = 0;
@@ -200,24 +247,31 @@
     t.start();
     int detect = 0;
     
+    if(fwd_bck == 1) {
+        move_forward(speed);
+        wait_ms(500);
+    } else {
+        turn(speed + 0.2);
+    }
+    
     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) {
-            move_detect(-speed,1,1500);
+            stop();
+            turn_leds_on();
+            move_detect(-0.5,1,1000);
             stop();
             break;
         // If line is detected from back just keep on moving forward
         } else if (detect == -1) {
-            move_detect(speed,1,1500);
+            stop();
+            turn_leds_on();
+            move_detect(0.5,1,1000);
             stop();
             break;
         }
     }
+    stop();
     t.stop();
 }
\ No newline at end of file