florian mordant / Mbed 2 deprecated Flatscan

h

Dependencies:   mbed SeeedShieldBot CANnucleo BluetoothSerial

main.cpp@0:d4d6dffec0cd, 2021-11-22 (annotated)

Committer:
b180088
Date:
Mon Nov 22 16:10:17 2021 +0000
Revision:
0:d4d6dffec0cd
hj

Who changed what in which revision?

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