Revision 3:68fe5b9e069a, committed 2014-10-16

Comitter:

bcostm

Date:

Thu Oct 16 06:18:14 2014 +0000

Parent:

2:7b09d04f029b

Child:

4:5dd60bfc3cdd

Commit message:

Stop the motors when a front sensor has detected something.

Changed in this revision

--- a/main.cpp	Mon Sep 08 13:00:23 2014 +0000
+++ b/main.cpp	Thu Oct 16 06:18:14 2014 +0000
@@ -11,7 +11,7 @@
 SeeedStudioShieldBot bot(
     D8, D9, D11,       // Right motor pins (PwmOut, DigitalOut, DigitalOut) -> Motor 1
     D12, D10, D13,     // Left motor pins (PwmOut, DigitalOut, DigitalOut) -> Motor 2
-    A0, A1, A2, A3, D4 // Sensors pins (all DigitalIn)
+    A0, A1, A2, A3, A4 // Sensors pins (all DigitalIn)
 );
 
 // Enable it for debugging on hyperterminal
@@ -25,45 +25,51 @@
 
 Ticker tick;
 
-float speed = 1.0; // Motors speed
-    
+float speed = 1.0; // Used to select the motors speed
+int stop = 0; // Used to stop the motors when a sensor detects something
+
 void ReadCommand(void)
 {
     int cmd = 0;
     PC_DEBUG(">>> Read command...\n");
+
     if (bluetooth.readable())
     {
         cmd = bluetooth.getc();
         PC_DEBUG(">>> Bluetooth read [%c]\n", cmd);
-        switch(cmd)
+      
+        // Ignore the receive command (excepted "Backward") if a sensor has detected something.
+        if ((stop) && (cmd != '2')) return;
+
+        switch (cmd)
         {
-          case '1': // Forward
-            bot.forward(speed);
-            break;
-          case '2': // Backward
-            bot.backward(speed);
-            break;              
-          case '3': // Left
-            bot.left(speed);
-            break;
-          case '4': // Right
-            bot.right(speed);
-            break;              
-          case '5': // Turn left
-            bot.turn_right(speed);
-            break;
-          case '6': // Turn right
-            bot.turn_left(speed);
-            break; 
-          case '7': // Slow
-            speed = 0.4;
-            break;
-          case '8': // Fast
-            speed = 1.0;
-            break;
-          default: // Stop
-            bot.stopAll();
-            break;
+            case '1': // Forward
+                bot.forward(speed);
+                break;
+            case '2': // Backward
+                bot.backward(speed);
+                break;              
+            case '3': // Left
+                bot.left(speed);
+                break;
+            case '4': // Right
+                bot.right(speed);
+                break;              
+            case '5': // Turn left
+                bot.turn_right(speed);
+                break;
+            case '6': // Turn right
+                bot.turn_left(speed);
+                break; 
+            case '7': // Slow
+                speed = 0.4;
+                break;
+            case '8': // Fast
+                speed = 1.0;
+                break;
+            default: // Stop
+                bot.stopAll();
+                break;
          }
     }    
 }
@@ -75,13 +81,6 @@
     // Enable motors
     bot.enable_right_motor();
     bot.enable_left_motor();
-    
-    // Check if they are alive
-    bot.left(speed);
-    wait(0.2);
-    bot.right(speed);
-    wait(0.2);
-    bot.stopAll();
   
     PC_DEBUG(">>> Bluetooth setup...");
     bluetooth.setup();
@@ -97,15 +96,25 @@
     bluetooth.connect();
     PC_DEBUG("done\n");
     
-    tick.attach_us(ReadCommand, 500000); // Every 500ms read Bluetooth command
+    tick.attach(ReadCommand, 0.3); // Every 300 ms read Bluetooth command
+
+    // Check if motors are alive
+    bot.left(speed);
+    wait(0.2);
+    bot.right(speed);
+    wait(0.2);
+    bot.stopAll();
 
-    bot.forward(speed);
-    wait(0.4);
-    bot.backward(speed);
-    wait(0.4);
-    bot.stopAll();
-      
     while (1) {
-        wait(1);
-    } 
+        // Stop the motors if a sensor has detected something.
+        if (!bot.rightSensor || !bot.inRightSensor || !bot.centreSensor || !bot.inLeftSensor || !bot.leftSensor)
+        {
+            if (!stop) bot.stopAll();
+            stop = 1;
+        }
+        else
+        { 
+            stop = 0;
+        }
+    }
 }

--- a/mbed-src.lib	Mon Sep 08 13:00:23 2014 +0000
+++ b/mbed-src.lib	Thu Oct 16 06:18:14 2014 +0000
@@ -1,1 +1,1 @@
-http://mbed.org/users/mbed_official/code/mbed-src/#8f72041da6ba
+http://mbed.org/users/mbed_official/code/mbed-src/#1b8acea4af63