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