Diff: main.cpp

Revision:

0:c8ce63279447

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Aug 12 14:34:13 2021 +0000
@@ -0,0 +1,137 @@
+#include "mbed.h"
+#include "BluetoothSerial.h"
+#include "SeeedStudioShieldBot.h"
+
+// The following configuration must be done on the NUCLEO board:
+// - Close SB62/SB63 and open SB13/SB14 solder bridges to enable the D0/D1 pins
+// - Open SB21 solder bridge to disconnect the LED
+
+BluetoothSerial bluetooth(D1, D0); // TX, RX
+
+#ifdef TARGET_NUCLEO_L053R8
+#define PWM1 D6 // Connect D6 and D8
+#define PWM2 D12
+#else // NUCLEO_F072RB
+#define PWM1 D8
+#define PWM2 D12
+#endif
+
+SeeedStudioShieldBot bot(
+    PWM1, D9, D11,     // Right motor pins (PwmOut, DigitalOut, DigitalOut) -> Motor 1
+    PWM2, D10, D13,    // Left motor pins (PwmOut, DigitalOut, DigitalOut) -> Motor 2
+    A0, A1, A2, A3, A4 // Sensors pins (all DigitalIn)
+);
+
+// Disable the feature by wiring A5 to GND
+DigitalIn ReadSensorsEnabled(A5, PullUp);
+
+// Enable it for debugging on hyperterminal
+#define DEBUG 0
+#if DEBUG == 1
+Serial pc(PC_10, PC_11); // Connect PC10 and CN3-RX
+#define PC_DEBUG(args...) pc.printf(args)
+#else
+#define PC_DEBUG(args...)
+#endif
+
+Ticker tick;
+
+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);
+      
+        // 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 forward
+                bot.turn_right(speed);
+                break;
+            case '6': // Turn right forward
+                bot.turn_left(speed);
+                break; 
+            case '7': // Turn left backward
+                bot.turn_right(-speed);
+                break;
+            case '8': // Turn right backward
+                bot.turn_left(-speed);
+                break; 
+            case '9': // Slow
+                speed = 0.5;
+                break;
+            case 'A': // Fast
+                speed = 1.0;
+                break;
+            default: // Stop
+                bot.stopAll();
+                break;
+         }
+    }    
+}
+    
+int main()
+{
+    PC_DEBUG("\n\nSeeed Bluetooth shield test started.\n");
+      
+    // Enable motors
+    bot.enable_right_motor();
+    bot.enable_left_motor();
+  
+    PC_DEBUG(">>> Bluetooth setup...");
+    bluetooth.setup();
+    PC_DEBUG("done\n");
+  
+    PC_DEBUG(">>> Bluetooth in slave mode...");
+    bluetooth.slave("bt_seeed_1");  // default PIN code: 0000
+    PC_DEBUG("done\n");
+    
+    wait(2);
+    
+    PC_DEBUG(">>> Bluetooth connect...");
+    bluetooth.connect();
+    PC_DEBUG("done\n");
+    
+    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();
+    
+    while (1) {
+        // Stop the motors if a sensor has detected something.
+        if ((!bot.rightSensor || !bot.inRightSensor || !bot.centreSensor || !bot.inLeftSensor || !bot.leftSensor) && ReadSensorsEnabled)
+        {
+            if (!stop) bot.stopAll();
+            stop = 1;
+        }
+        else
+        { 
+            stop = 0;
+        }
+    }
+}