Bluetooth program for the Seeed bot
Dependencies: BluetoothSerial SeeedShieldBot mbed
Fork of Seeed_BlueBot_demo by
Diff: main.cpp
- Revision:
- 3:68fe5b9e069a
- Parent:
- 2:7b09d04f029b
- Child:
- 4:5dd60bfc3cdd
diff -r 7b09d04f029b -r 68fe5b9e069a main.cpp --- a/main.cpp Mon Sep 08 13:00:23 2014 +0000 +++ b/main.cpp Thu Oct 16 06:18:14 2014 +0000 @@ -11,7 +11,7 @@ SeeedStudioShieldBot bot( D8, D9, D11, // Right motor pins (PwmOut, DigitalOut, DigitalOut) -> Motor 1 D12, D10, D13, // Left motor pins (PwmOut, DigitalOut, DigitalOut) -> Motor 2 - A0, A1, A2, A3, D4 // Sensors pins (all DigitalIn) + A0, A1, A2, A3, A4 // Sensors pins (all DigitalIn) ); // Enable it for debugging on hyperterminal @@ -25,45 +25,51 @@ Ticker tick; -float speed = 1.0; // Motors speed - +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); - switch(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 - bot.turn_right(speed); - break; - case '6': // Turn right - bot.turn_left(speed); - break; - case '7': // Slow - speed = 0.4; - break; - case '8': // Fast - speed = 1.0; - break; - default: // Stop - bot.stopAll(); - break; + 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 + bot.turn_right(speed); + break; + case '6': // Turn right + bot.turn_left(speed); + break; + case '7': // Slow + speed = 0.4; + break; + case '8': // Fast + speed = 1.0; + break; + default: // Stop + bot.stopAll(); + break; } } } @@ -75,13 +81,6 @@ // Enable motors bot.enable_right_motor(); bot.enable_left_motor(); - - // Check if they are alive - bot.left(speed); - wait(0.2); - bot.right(speed); - wait(0.2); - bot.stopAll(); PC_DEBUG(">>> Bluetooth setup..."); bluetooth.setup(); @@ -97,15 +96,25 @@ bluetooth.connect(); PC_DEBUG("done\n"); - tick.attach_us(ReadCommand, 500000); // Every 500ms read Bluetooth command + 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(); - bot.forward(speed); - wait(0.4); - bot.backward(speed); - wait(0.4); - bot.stopAll(); - while (1) { - wait(1); - } + // Stop the motors if a sensor has detected something. + if (!bot.rightSensor || !bot.inRightSensor || !bot.centreSensor || !bot.inLeftSensor || !bot.leftSensor) + { + if (!stop) bot.stopAll(); + stop = 1; + } + else + { + stop = 0; + } + } }