Robot code for searching an object and charging at it.

Dependencies:   HCSR04 Motor mbed

Revision:
37:a08d2e37b7e6
Parent:
36:0a69dc7bfcf9
Child:
38:decff231d886
--- a/functions.cpp	Tue Jun 09 10:28:16 2015 +0000
+++ b/functions.cpp	Wed Oct 12 11:13:45 2016 +0000
@@ -28,6 +28,27 @@
 // Example: usensor(Trigger, Echo)
 HCSR04 usensor(p25,p26);
 
+void turn_led_on(int LED) {
+        switch(LED) {
+            case 1:
+                myled1 = 1;
+                wait(0.1);
+                myled1 = 0;
+            case 2:
+                myled2 = 1;
+                wait(0.1);
+                myled2 = 0;
+            case 3:
+                myled3 = 1;
+                wait(0.1);
+                myled3 = 0;
+            case 4:
+                myled4 = 1;
+                wait(0.1);
+                myled4 = 0;
+        }
+}
+
 void turn_leds_on() {
         myled1 = 1;
         myled2 = 1;
@@ -146,19 +167,25 @@
     MotorRight.speed(-(speed));
 }
 
-void turn(float speed) {
-    //printf("Turning\n");
+void turn_left(float speed) {
+    //printf("Turning left\n");
     MotorLeft.speed(speed);
     MotorRight.speed(-(speed));
 }
 
+void turn_right(float speed) {
+    //printf("Turning right\n");
+    MotorLeft.speed(-(speed));
+    MotorRight.speed(speed);
+}
+
 void reverseandturn(float speed) {
     //printf("Reverse and turn\n");
     MotorLeft.speed((speed-0.3));
     MotorRight.speed(-(speed-0.1));
 }
 
-void move_forward(float speed, float adj) {
+void forwards(float speed, float adj) {
     MotorLeft.speed(speed+adj);
     MotorRight.speed(speed);
 }
@@ -232,7 +259,7 @@
             break;
         } else {
             sense = 0;
-            turn(speed);
+            turn_left(speed);
         }
         
         if (inner_t.read_ms() >=100) {
@@ -258,10 +285,10 @@
     int detect = 0;
     
     if(fwd_bck == 1) {
-        move_forward(speed);
+        forwards(speed);
         wait_ms(wait);
     } else {
-        turn(speed + 0.2);
+        turn_left(speed + 0.2);
     }
     
     while (t.read_ms() < time) {