mikl reid / Mbed 2 deprecated Seeed_BlueBot_demo

v

Dependencies:   mbed SeeedShieldBot BluetoothSerial

main.cpp@0:4da4b81151c6, 2021-11-28 (annotated)

Committer:
m1kl
Date:
Sun Nov 28 17:59:43 2021 +0000
Revision:
0:4da4b81151c6
b

Who changed what in which revision?

UserRevisionLine numberNew contents of line
m1kl 0:4da4b81151c6 1 #include "mbed.h"
m1kl 0:4da4b81151c6 2 #include "BluetoothSerial.h"
m1kl 0:4da4b81151c6 3 #include "SeeedStudioShieldBot.h"
m1kl 0:4da4b81151c6 4
m1kl 0:4da4b81151c6 5 // The following configuration must be done on the NUCLEO board:
m1kl 0:4da4b81151c6 6 // - Close SB62/SB63 and open SB13/SB14 solder bridges to enable the D0/D1 pins
m1kl 0:4da4b81151c6 7 // - Open SB21 solder bridge to disconnect the LED
m1kl 0:4da4b81151c6 8
m1kl 0:4da4b81151c6 9 BluetoothSerial bluetooth(D1, D0); // TX, RX
m1kl 0:4da4b81151c6 10
m1kl 0:4da4b81151c6 11 #ifdef TARGET_NUCLEO_L053R8
m1kl 0:4da4b81151c6 12 #define PWM1 D6 // Connect D6 and D8
m1kl 0:4da4b81151c6 13 #define PWM2 D12
m1kl 0:4da4b81151c6 14 #else // NUCLEO_F072RB
m1kl 0:4da4b81151c6 15 #define PWM1 D8
m1kl 0:4da4b81151c6 16 #define PWM2 D12
m1kl 0:4da4b81151c6 17 #endif
m1kl 0:4da4b81151c6 18
m1kl 0:4da4b81151c6 19 SeeedStudioShieldBot bot(
m1kl 0:4da4b81151c6 20 PWM1, D9, D11, // Right motor pins (PwmOut, DigitalOut, DigitalOut) -> Motor 1
m1kl 0:4da4b81151c6 21 PWM2, D10, D13, // Left motor pins (PwmOut, DigitalOut, DigitalOut) -> Motor 2
m1kl 0:4da4b81151c6 22 A0, A1, A2, A3, A4 // Sensors pins (all DigitalIn)
m1kl 0:4da4b81151c6 23 );
m1kl 0:4da4b81151c6 24
m1kl 0:4da4b81151c6 25 // Disable the feature by wiring A5 to GND
m1kl 0:4da4b81151c6 26 DigitalIn ReadSensorsEnabled(A5, PullUp);
m1kl 0:4da4b81151c6 27
m1kl 0:4da4b81151c6 28 // Enable it for debugging on hyperterminal
m1kl 0:4da4b81151c6 29 #define DEBUG 0
m1kl 0:4da4b81151c6 30 #if DEBUG == 1
m1kl 0:4da4b81151c6 31 Serial pc(PC_10, PC_11); // Connect PC10 and CN3-RX
m1kl 0:4da4b81151c6 32 #define PC_DEBUG(args...) pc.printf(args)
m1kl 0:4da4b81151c6 33 #else
m1kl 0:4da4b81151c6 34 #define PC_DEBUG(args...)
m1kl 0:4da4b81151c6 35 #endif
m1kl 0:4da4b81151c6 36
m1kl 0:4da4b81151c6 37 Ticker tick;
m1kl 0:4da4b81151c6 38
m1kl 0:4da4b81151c6 39 float speed = 1.0; // Used to select the motors speed
m1kl 0:4da4b81151c6 40 int stop = 0; // Used to stop the motors when a sensor detects something
m1kl 0:4da4b81151c6 41
m1kl 0:4da4b81151c6 42 void ReadCommand(void)
m1kl 0:4da4b81151c6 43 {
m1kl 0:4da4b81151c6 44 int cmd = 0;
m1kl 0:4da4b81151c6 45 PC_DEBUG(">>> Read command...\n");
m1kl 0:4da4b81151c6 46
m1kl 0:4da4b81151c6 47 if (bluetooth.readable())
m1kl 0:4da4b81151c6 48 {
m1kl 0:4da4b81151c6 49 cmd = bluetooth.getc();
m1kl 0:4da4b81151c6 50 PC_DEBUG(">>> Bluetooth read [%c]\n", cmd);
m1kl 0:4da4b81151c6 51
m1kl 0:4da4b81151c6 52 // Ignore the receive command (excepted "Backward") if a sensor has detected something.
m1kl 0:4da4b81151c6 53 if ((stop) && (cmd != '2')) return;
m1kl 0:4da4b81151c6 54
m1kl 0:4da4b81151c6 55 switch (cmd)
m1kl 0:4da4b81151c6 56 {
m1kl 0:4da4b81151c6 57 case '1': // Forward
m1kl 0:4da4b81151c6 58 bot.forward(speed);
m1kl 0:4da4b81151c6 59 break;
m1kl 0:4da4b81151c6 60 case '2': // Backward
m1kl 0:4da4b81151c6 61 bot.backward(speed);
m1kl 0:4da4b81151c6 62 break;
m1kl 0:4da4b81151c6 63 case '3': // Left
m1kl 0:4da4b81151c6 64 bot.left(speed);
m1kl 0:4da4b81151c6 65 break;
m1kl 0:4da4b81151c6 66 case '4': // Right
m1kl 0:4da4b81151c6 67 bot.right(speed);
m1kl 0:4da4b81151c6 68 break;
m1kl 0:4da4b81151c6 69 case '5': // Turn left forward
m1kl 0:4da4b81151c6 70 bot.turn_right(speed);
m1kl 0:4da4b81151c6 71 break;
m1kl 0:4da4b81151c6 72 case '6': // Turn right forward
m1kl 0:4da4b81151c6 73 bot.turn_left(speed);
m1kl 0:4da4b81151c6 74 break;
m1kl 0:4da4b81151c6 75 case '7': // Turn left backward
m1kl 0:4da4b81151c6 76 bot.turn_right(-speed);
m1kl 0:4da4b81151c6 77 break;
m1kl 0:4da4b81151c6 78 case '8': // Turn right backward
m1kl 0:4da4b81151c6 79 bot.turn_left(-speed);
m1kl 0:4da4b81151c6 80 break;
m1kl 0:4da4b81151c6 81 case '9': // Slow
m1kl 0:4da4b81151c6 82 speed = 0.5;
m1kl 0:4da4b81151c6 83 break;
m1kl 0:4da4b81151c6 84 case 'A': // Fast
m1kl 0:4da4b81151c6 85 speed = 1.0;
m1kl 0:4da4b81151c6 86 break;
m1kl 0:4da4b81151c6 87 default: // Stop
m1kl 0:4da4b81151c6 88 bot.stopAll();
m1kl 0:4da4b81151c6 89 break;
m1kl 0:4da4b81151c6 90 }
m1kl 0:4da4b81151c6 91 }
m1kl 0:4da4b81151c6 92 }
m1kl 0:4da4b81151c6 93
m1kl 0:4da4b81151c6 94 int main()
m1kl 0:4da4b81151c6 95 {
m1kl 0:4da4b81151c6 96 PC_DEBUG("\n\nSeeed Bluetooth shield test started.\n");
m1kl 0:4da4b81151c6 97
m1kl 0:4da4b81151c6 98 // Enable motors
m1kl 0:4da4b81151c6 99 bot.enable_right_motor();
m1kl 0:4da4b81151c6 100 bot.enable_left_motor();
m1kl 0:4da4b81151c6 101
m1kl 0:4da4b81151c6 102 PC_DEBUG(">>> Bluetooth setup...");
m1kl 0:4da4b81151c6 103 bluetooth.setup();
m1kl 0:4da4b81151c6 104 PC_DEBUG("done\n");
m1kl 0:4da4b81151c6 105
m1kl 0:4da4b81151c6 106 PC_DEBUG(">>> Bluetooth in slave mode...");
m1kl 0:4da4b81151c6 107 bluetooth.slave("bt_seeed_1"); // default PIN code: 0000
m1kl 0:4da4b81151c6 108 PC_DEBUG("done\n");
m1kl 0:4da4b81151c6 109
m1kl 0:4da4b81151c6 110 wait(2);
m1kl 0:4da4b81151c6 111
m1kl 0:4da4b81151c6 112 PC_DEBUG(">>> Bluetooth connect...");
m1kl 0:4da4b81151c6 113 bluetooth.connect();
m1kl 0:4da4b81151c6 114 PC_DEBUG("done\n");
m1kl 0:4da4b81151c6 115
m1kl 0:4da4b81151c6 116 tick.attach(ReadCommand, 0.3); // Every 300 ms read Bluetooth command
m1kl 0:4da4b81151c6 117
m1kl 0:4da4b81151c6 118 // Check if motors are alive
m1kl 0:4da4b81151c6 119 bot.left(speed);
m1kl 0:4da4b81151c6 120 wait(0.2);
m1kl 0:4da4b81151c6 121 bot.right(speed);
m1kl 0:4da4b81151c6 122 wait(0.2);
m1kl 0:4da4b81151c6 123 bot.stopAll();
m1kl 0:4da4b81151c6 124
m1kl 0:4da4b81151c6 125 while (1) {
m1kl 0:4da4b81151c6 126 // Stop the motors if a sensor has detected something.
m1kl 0:4da4b81151c6 127 if ((!bot.rightSensor || !bot.inRightSensor || !bot.centreSensor || !bot.inLeftSensor || !bot.leftSensor) && ReadSensorsEnabled)
m1kl 0:4da4b81151c6 128 {
m1kl 0:4da4b81151c6 129 if (!stop) bot.stopAll();
m1kl 0:4da4b81151c6 130 stop = 1;
m1kl 0:4da4b81151c6 131 }
m1kl 0:4da4b81151c6 132 else
m1kl 0:4da4b81151c6 133 {
m1kl 0:4da4b81151c6 134 stop = 0;
m1kl 0:4da4b81151c6 135 }
m1kl 0:4da4b81151c6 136 }
m1kl 0:4da4b81151c6 137 }