Diff: main.cpp

Revision:

0:901651f381c9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Oct 12 11:33:19 2015 +0000
@@ -0,0 +1,161 @@
+#include "mbed.h"
+#include "BLE.h"
+#include "SampleService.h"
+#include "DeviceInformationService.h"
+#include "x_nucleo_idb0xa1_targets.h"
+#include "ZumoShield.h"
+
+// See also the file X_NUCLEO_IDB0XA1/x_nucleo_idb0xa1_targets.h to configure
+// the correct SPI clock (D3 or D13 pin).
+
+// Configuration for NUCLEO_F401RE/F030R8/F070RB/F072RB/F091RC/F303RE/F334R8/F411RE/F446RE/L152RE/L476RG
+#define PWM1 D9
+#define PWM2 D10
+// Configuration for NUCLEO_F103RB
+//#define PWM1 D9
+//#define PWM2 D2
+// Configuration for NUCLEO_F302R8
+//#define PWM1 D2
+//#define PWM2 D10
+// Configuration for NUCLEO_L053R8
+//#define PWM1 D9
+//#define PWM2 PA_15 // on Morpho connector
+
+#define PC_DEBUG(args...) printf(args)
+
+// Bluetooth Low Energy (BLE) device
+BLE ble;
+// BLE Primary Service
+SampleService *sampleService;
+
+ZumoShield zumo(
+//  Function      Nucleo board   Zumo shield
+    /* Motor 1 PWM */ PWM1,          // D9  !!! WARNING !!! see configuration above
+    /* Motor 1 DIR */ D6,            // D7  !!! WARNING !!!
+    /* Motor 2 PWM */ PWM2,          // D10 !!! WARNING !!! see configuration above
+    /* Motor 2 DIR */ D8             // D8
+);
+
+// Hardware device selection
+BusIn device_id(D15, D14);
+
+// Parameters for Bluetooth Low Energy
+uint8_t DEVICE_NAME[] = "Zumo_car_X"; // X is defined below with value of device_id
+Gap::Address_t BLE_address_BE; // defined below with value of device_id
+
+static const uint16_t uuid16_list[] = {SampleServiceShortUUID, GattService::UUID_DEVICE_INFORMATION_SERVICE};
+
+void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
+{
+    ble.startAdvertising(); // restart advertising
+}
+
+void remoteControlCallback(const uint8_t* data, uint16_t len)
+{
+    float speed = 0;
+
+    // Check if this is a remote control command (3-byte len and starting with a null byte)
+    if ( (data[0]) || (len != 3) ) return;
+
+    // data is a 3-byte sequence
+    // first byte is always null
+    // second byte is vertical slider position (speed)
+    // third byte is horizontal slider position (direction)
+    int s = data[1]; // speed (0-100)
+    int d = data[2]; // direction (0-100)
+
+    PC_DEBUG(">>> Speed=%d, Direction=%d\r\n", s, d);
+
+    if ((s == 50) && (d == 50)) {
+        zumo.stopAll();
+        return;
+    }
+
+    if (s > 50) {
+        speed = float(s)/100;
+        if (d == 50) {
+            zumo.forward(speed);
+            return;
+        }
+    } else if (s < 50) {
+        speed = (100-float(s))/100;
+        if (d == 50) {
+            zumo.backward(speed);
+            return;
+        }
+    }
+
+    if (d > 50) {
+        if (s > 50) {
+            zumo.turn_right(speed);
+        } else if (s < 50) {
+            zumo.turn_left(-speed);
+        } else { // s == 50
+            speed = float(d)/100;
+            zumo.right(speed);
+        }
+    } else if (d < 50) {
+        if (s > 50) {
+            zumo.turn_left(speed);
+        } else if (s < 50) {
+            zumo.turn_right(-speed);
+        } else { // s == 50
+            speed = (100-float(d))/100;
+            zumo.left(speed);
+        }
+    }
+
+}
+
+// Setup Bluetooth Low Energy device
+void setup_BLE(void)
+{
+    // Set BLE device name and address
+    BLE_address_BE[0] = 0xca;
+    BLE_address_BE[1] = 0xcc;
+    BLE_address_BE[2] = 0x61;
+    BLE_address_BE[3] = 0xab;
+    BLE_address_BE[4] = 0xef;
+    BLE_address_BE[5] = device_id;
+    DEVICE_NAME[9] = '0' + device_id;
+
+    // Set BT Address
+    ble.setAddress(Gap::ADDR_TYPE_PUBLIC, BLE_address_BE);
+
+    // Init the BLE dev
+    ble.init();
+    ble.onDisconnection(disconnectionCallback);
+
+    /* Setup primary service. */
+    sampleService = new SampleService(ble, remoteControlCallback);
+
+    /* Setup auxiliary service. */
+    DeviceInformationService deviceInfo(ble, "STM", "Model1", "SN1", "hw-rev1", "fw-rev1", "soft-rev1");
+
+    /* Setup advertising. */
+    ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
+    ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));
+    ble.accumulateAdvertisingPayload(GapAdvertisingData::UNKNOWN);
+    ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
+    ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
+    ble.setAdvertisingInterval(1000);
+    ble.startAdvertising();
+}
+
+int main()
+{
+    PC_DEBUG(">>> Bluetooth Low Energy setup...\r\n");
+    setup_BLE();
+    PC_DEBUG(">>> Setup done\r\n");
+
+    // Check motors
+    zumo.right(1.0);
+    wait(0.5);
+    zumo.left(1.0);
+    wait(0.5);
+    zumo.stopAll();
+
+    while (1) {
+        ble.waitForEvent(); // low power wait for event. Must be called in the loop or BLE won't work
+    }
+}