ARSLab / Mbed 2 deprecated Seeed_BlueBot_demo_test1

Bluetooth program for the Seeed bot

Dependencies:   BluetoothSerial SeeedShieldBot mbed

Fork of Seeed_BlueBot_demo by ST

main.cpp@3:68fe5b9e069a, 2014-10-16 (annotated)

Committer:
bcostm
Date:
Thu Oct 16 06:18:14 2014 +0000
Revision:
3:68fe5b9e069a
Parent:
2:7b09d04f029b
Child:
4:5dd60bfc3cdd
Stop the motors when a front sensor has detected something.

Who changed what in which revision?

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