The Bubbleworks / Mbed 2 deprecated TheBubbleWorks_MicroBorg

micro:bit and PiBorg i2c motor driver (PiBorg Reverse) example program for a Web Bluetooth demo that lets you drive around a 4WD buggy http://www.thebubbleworks.com/

Dependencies:   BLE_API NanoBit PicoBorgReverse mbed nRF51822

main.cpp@1:5e5183fed184, 2016-03-14 (annotated)

Committer:
waynek
Date:
Mon Mar 14 10:37:25 2016 +0000
Revision:
1:5e5183fed184
Parent:
0:9ea115d01a95 
Initial Public commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
waynek 0:9ea115d01a95 1 /*
waynek 1:5e5183fed184 2 * Copyright (c) 2016 https://www.thebubbleworks.com/
waynek 0:9ea115d01a95 3 *
waynek 0:9ea115d01a95 4 * Licensed under the Apache License, Version 2.0 (the "License");
waynek 0:9ea115d01a95 5 * you may not use this file except in compliance with the License.
waynek 0:9ea115d01a95 6 * You may obtain a copy of the License at
waynek 0:9ea115d01a95 7 *
waynek 0:9ea115d01a95 8 * http://www.apache.org/licenses/LICENSE-2.0
waynek 0:9ea115d01a95 9 *
waynek 0:9ea115d01a95 10 * Unless required by applicable law or agreed to in writing, software
waynek 0:9ea115d01a95 11 * distributed under the License is distributed on an "AS IS" BASIS,
waynek 0:9ea115d01a95 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
waynek 0:9ea115d01a95 13 * See the License for the specific language governing permissions and
waynek 0:9ea115d01a95 14 * limitations under the License.
waynek 0:9ea115d01a95 15 */
waynek 1:5e5183fed184 16
waynek 1:5e5183fed184 17
waynek 1:5e5183fed184 18 // For more information, such as the Web Bluetooth remote controller, please see https://www.thebubbleworks.com/
waynek 1:5e5183fed184 19 #define MY_DBG_ENABLED 1
waynek 1:5e5183fed184 20
waynek 0:9ea115d01a95 21 #include "MyDebug.h"
waynek 0:9ea115d01a95 22 #include "PicoBorgReverse.h"
waynek 0:9ea115d01a95 23
waynek 0:9ea115d01a95 24
waynek 0:9ea115d01a95 25 #include "mbed.h"
waynek 0:9ea115d01a95 26 #include "ble/BLE.h"
waynek 0:9ea115d01a95 27 #include "ble/services/UARTService.h"
waynek 0:9ea115d01a95 28
waynek 1:5e5183fed184 29 // Nano smaller than Micro, in this case, a cut down library...
waynek 0:9ea115d01a95 30 #include "NanoBitPins.h"
waynek 0:9ea115d01a95 31 #include "NanoBitDisplay.h"
waynek 0:9ea115d01a95 32
waynek 0:9ea115d01a95 33
waynek 0:9ea115d01a95 34 I2C i2cBus(MICROBIT_PIN_SDA, MICROBIT_PIN_SCL);
waynek 0:9ea115d01a95 35
waynek 0:9ea115d01a95 36 #ifdef MY_DBG_ENABLED
waynek 0:9ea115d01a95 37 Serial serial(MICROBIT_PIN_USBTX, MICROBIT_PIN_USBRX);
waynek 1:5e5183fed184 38
waynek 1:5e5183fed184 39 // Use 5 of the micro:bit on-board LED's for status information
waynek 1:5e5183fed184 40
waynek 1:5e5183fed184 41 #define TRACE_INIT() MicroBitDisplay display; display.clear();
waynek 1:5e5183fed184 42 #define TRACE_START() display.setPixel(0,0);
waynek 1:5e5183fed184 43 #define TRACE_BLE_INIT() display.setPixel(0,1);
waynek 1:5e5183fed184 44 #define TRACE_BLE_STARTED() display.setPixel(0,2);
waynek 1:5e5183fed184 45 #define TRACE_MOTOR_STATUS(x) display.setPixel(0,3, x);
waynek 1:5e5183fed184 46 #define TRACE_BLE_CONNECTED(x) display.setPixel(0,4, x);
waynek 1:5e5183fed184 47 #else
waynek 1:5e5183fed184 48 // or don't.
waynek 1:5e5183fed184 49 #define TRACE_INIT()
waynek 1:5e5183fed184 50 #define TRACE_START()
waynek 1:5e5183fed184 51 #define TRACE_BLE_INIT()
waynek 1:5e5183fed184 52 #define TRACE_BLE_STARTED()
waynek 1:5e5183fed184 53 #define TRACE_MOTOR_STATUS(x)
waynek 1:5e5183fed184 54 #define TRACE_BLE_CONNECTED(x)
waynek 0:9ea115d01a95 55 #endif
waynek 0:9ea115d01a95 56
waynek 0:9ea115d01a95 57
waynek 0:9ea115d01a95 58 const static char DEVICE_NAME[] = "MicroBorg";
waynek 0:9ea115d01a95 59 UARTService *uartService;
waynek 0:9ea115d01a95 60
waynek 0:9ea115d01a95 61 static volatile bool triggerSensorPolling = false;
waynek 0:9ea115d01a95 62
waynek 0:9ea115d01a95 63 PicoBorgReverse* pbr;
waynek 0:9ea115d01a95 64
waynek 0:9ea115d01a95 65 int motor1Speed=0, motor2Speed=0;
waynek 0:9ea115d01a95 66
waynek 0:9ea115d01a95 67
waynek 0:9ea115d01a95 68
waynek 0:9ea115d01a95 69 void onData(const GattWriteCallbackParams *params) {
waynek 0:9ea115d01a95 70 const uint8_t* bytes = params->data;
waynek 0:9ea115d01a95 71 const uint16_t len = params->len;
waynek 0:9ea115d01a95 72 if (len>=6) {
waynek 0:9ea115d01a95 73 //MY_DBG_PRINTF("BLE Recv: %d = %x\n", len, bytes[0]);
waynek 0:9ea115d01a95 74 if ( (0x00 == bytes[0]) && ( 0x01 == bytes[1]) ) {
waynek 0:9ea115d01a95 75 motor1Speed = (bytes[2] + (bytes[3] << 8))-255;
waynek 0:9ea115d01a95 76 motor2Speed = - ((bytes[4] + (bytes[5] << 8))-255);
waynek 0:9ea115d01a95 77 }
waynek 0:9ea115d01a95 78 }
waynek 0:9ea115d01a95 79 }
waynek 0:9ea115d01a95 80
waynek 0:9ea115d01a95 81 void sendData(const void *buffer, size_t length) {
waynek 0:9ea115d01a95 82 BLE& ble = BLE::Instance(BLE::DEFAULT_INSTANCE);
waynek 0:9ea115d01a95 83 if (length>20)
waynek 0:9ea115d01a95 84 length = 20;
waynek 0:9ea115d01a95 85
waynek 0:9ea115d01a95 86 if (length>0 && ble.getGapState().connected) {
waynek 0:9ea115d01a95 87 ble.gattServer().write(uartService->getRXCharacteristicHandle(), static_cast<const uint8_t *>(buffer), length);
waynek 0:9ea115d01a95 88 }
waynek 0:9ea115d01a95 89 }
waynek 0:9ea115d01a95 90
waynek 0:9ea115d01a95 91
waynek 0:9ea115d01a95 92 //----------------------------------------------------
waynek 0:9ea115d01a95 93
waynek 1:5e5183fed184 94 // Try to reduce transfer latency (optional, the central device may ignore this 'request')
waynek 1:5e5183fed184 95 // The opposite of low power :) https://developer.mbed.org/questions/54515/How-to-lower-power-consumption/
waynek 0:9ea115d01a95 96
waynek 0:9ea115d01a95 97 void connectionCallback(const Gap::ConnectionCallbackParams_t *params)
waynek 0:9ea115d01a95 98 {
waynek 0:9ea115d01a95 99 Gap::Handle_t gap_handle = params->handle;
waynek 0:9ea115d01a95 100 Gap::ConnectionParams_t new_params;
waynek 0:9ea115d01a95 101
waynek 0:9ea115d01a95 102 new_params.minConnectionInterval = 20; /* 20 msec */
waynek 0:9ea115d01a95 103 new_params.maxConnectionInterval = 20; /* 20 msec */
waynek 0:9ea115d01a95 104 new_params.connectionSupervisionTimeout = 5000; /* 5 seconds */
waynek 0:9ea115d01a95 105 new_params.slaveLatency = 1;
waynek 0:9ea115d01a95 106 BLE::Instance(BLE::DEFAULT_INSTANCE).gap().updateConnectionParams(gap_handle, &new_params);
waynek 0:9ea115d01a95 107 }
waynek 0:9ea115d01a95 108
waynek 0:9ea115d01a95 109 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
waynek 0:9ea115d01a95 110 {
waynek 0:9ea115d01a95 111 BLE::Instance(BLE::DEFAULT_INSTANCE).gap().startAdvertising(); // restart advertising
waynek 0:9ea115d01a95 112 }
waynek 0:9ea115d01a95 113
waynek 0:9ea115d01a95 114
waynek 0:9ea115d01a95 115 void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
waynek 0:9ea115d01a95 116 {
waynek 0:9ea115d01a95 117 BLE &ble = params->ble;
waynek 0:9ea115d01a95 118 ble_error_t error = params->error;
waynek 0:9ea115d01a95 119
waynek 0:9ea115d01a95 120 if (error != BLE_ERROR_NONE) {
waynek 0:9ea115d01a95 121 return;
waynek 0:9ea115d01a95 122 }
waynek 0:9ea115d01a95 123
waynek 0:9ea115d01a95 124 ble.gap().onConnection(connectionCallback);
waynek 0:9ea115d01a95 125 ble.gap().onDisconnection(disconnectionCallback);
waynek 0:9ea115d01a95 126
waynek 0:9ea115d01a95 127 /* Setup primary service. */
waynek 0:9ea115d01a95 128 uartService = new UARTService(ble);
waynek 0:9ea115d01a95 129 ble.onDataWritten(onData);
waynek 0:9ea115d01a95 130
waynek 0:9ea115d01a95 131
waynek 0:9ea115d01a95 132 /* Setup advertising. */
waynek 0:9ea115d01a95 133 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
waynek 0:9ea115d01a95 134 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS, UARTServiceUUID_reversed , sizeof(UARTServiceUUID_reversed));
waynek 0:9ea115d01a95 135 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
waynek 0:9ea115d01a95 136 ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
waynek 0:9ea115d01a95 137 ble.gap().setAdvertisingInterval(ble.gap().getMinAdvertisingInterval());
waynek 0:9ea115d01a95 138 ble.gap().startAdvertising();
waynek 0:9ea115d01a95 139
waynek 0:9ea115d01a95 140
waynek 0:9ea115d01a95 141 }
waynek 0:9ea115d01a95 142
waynek 0:9ea115d01a95 143
waynek 0:9ea115d01a95 144
waynek 0:9ea115d01a95 145
waynek 0:9ea115d01a95 146 //----------------------------------------------------
waynek 0:9ea115d01a95 147
waynek 0:9ea115d01a95 148
waynek 0:9ea115d01a95 149 void periodicCallback(void)
waynek 0:9ea115d01a95 150 {
waynek 0:9ea115d01a95 151 /* Note that the periodicCallback() executes in interrupt context, so it is safer to do
waynek 0:9ea115d01a95 152 * heavy-weight sensor polling from the main thread. */
waynek 0:9ea115d01a95 153 triggerSensorPolling = true;
waynek 0:9ea115d01a95 154 }
waynek 0:9ea115d01a95 155
waynek 0:9ea115d01a95 156 //----------------------------------------------------
waynek 1:5e5183fed184 157 // PiBorg Reverese motor controller functions
waynek 0:9ea115d01a95 158
waynek 0:9ea115d01a95 159 void motorReset() {
waynek 0:9ea115d01a95 160 pbr->resetEpo();
waynek 0:9ea115d01a95 161 pbr->setLed(pbr->getEpo());
waynek 0:9ea115d01a95 162 }
waynek 0:9ea115d01a95 163
waynek 0:9ea115d01a95 164 bool motorInit()
waynek 0:9ea115d01a95 165 {
waynek 0:9ea115d01a95 166 pbr = new PicoBorgReverse(&i2cBus);
waynek 0:9ea115d01a95 167 if (!pbr->checkId()) {
waynek 0:9ea115d01a95 168 MY_DBG_PRINT("Motor not found\n");
waynek 0:9ea115d01a95 169 return false;
waynek 0:9ea115d01a95 170 }
waynek 0:9ea115d01a95 171 else {
waynek 0:9ea115d01a95 172 MY_DBG_PRINT("Motors OK\n");
waynek 0:9ea115d01a95 173 }
waynek 0:9ea115d01a95 174 motorReset(); // Reset the EPO latch on the PicoBorg Reverse
waynek 0:9ea115d01a95 175
waynek 0:9ea115d01a95 176 // Ensure the communications failsafe is enabled on the PicoBorg Reverse
waynek 0:9ea115d01a95 177 while (!pbr->getCommsFailsafe()) {
waynek 0:9ea115d01a95 178 pbr->setCommsFailsafe(true);
waynek 0:9ea115d01a95 179 }
waynek 0:9ea115d01a95 180 return true;
waynek 0:9ea115d01a95 181 }
waynek 0:9ea115d01a95 182
waynek 0:9ea115d01a95 183 void setMotor1Speed(int speed)
waynek 0:9ea115d01a95 184 {
waynek 0:9ea115d01a95 185 pbr->setMotor1(speed);
waynek 0:9ea115d01a95 186 pbr->setLed(pbr->getEpo()); // Set the PicoBorg Reverse LED on if the EPO has been latched
waynek 0:9ea115d01a95 187 }
waynek 0:9ea115d01a95 188
waynek 0:9ea115d01a95 189 void setMotor2Speed(int speed)
waynek 0:9ea115d01a95 190 {
waynek 0:9ea115d01a95 191 pbr->setMotor2(speed);
waynek 0:9ea115d01a95 192 pbr->setLed(pbr->getEpo()); // Set the PicoBorg Reverse LED on if the EPO has been latched
waynek 0:9ea115d01a95 193 }
waynek 0:9ea115d01a95 194
waynek 0:9ea115d01a95 195
waynek 0:9ea115d01a95 196
waynek 0:9ea115d01a95 197 //----------------------------------------------------
waynek 0:9ea115d01a95 198
waynek 0:9ea115d01a95 199
waynek 0:9ea115d01a95 200 int main(void)
waynek 0:9ea115d01a95 201 {
waynek 0:9ea115d01a95 202 MY_DBG_PRINT("Started\n");
waynek 0:9ea115d01a95 203 TRACE_INIT();
waynek 0:9ea115d01a95 204 TRACE_START();
waynek 0:9ea115d01a95 205
waynek 0:9ea115d01a95 206 Ticker ticker;
waynek 0:9ea115d01a95 207 ticker.attach(periodicCallback, 1.0); // blink LED every second
waynek 0:9ea115d01a95 208
waynek 0:9ea115d01a95 209 // Setup Bluetooht LE
waynek 0:9ea115d01a95 210 BLE& ble = BLE::Instance(BLE::DEFAULT_INSTANCE);
waynek 0:9ea115d01a95 211 ble.init(bleInitComplete);
waynek 0:9ea115d01a95 212 TRACE_BLE_INIT();
waynek 0:9ea115d01a95 213 while (ble.hasInitialized() == false) { /* spin loop */ }
waynek 1:5e5183fed184 214 TRACE_BLE_STARTED();
waynek 0:9ea115d01a95 215
waynek 0:9ea115d01a95 216 // Setup Motors
waynek 0:9ea115d01a95 217 bool motorInitOK = motorInit();
waynek 1:5e5183fed184 218 TRACE_MOTOR_STATUS(motorInitOK);
waynek 0:9ea115d01a95 219
waynek 0:9ea115d01a95 220
waynek 0:9ea115d01a95 221 MY_DBG_PRINT("Go!\n");
waynek 0:9ea115d01a95 222 while (1) {
waynek 0:9ea115d01a95 223 bool isConnected = ble.getGapState().connected;
waynek 0:9ea115d01a95 224
waynek 0:9ea115d01a95 225 TRACE_BLE_CONNECTED(isConnected);
waynek 0:9ea115d01a95 226
waynek 0:9ea115d01a95 227 if (isConnected) {
waynek 0:9ea115d01a95 228 if (triggerSensorPolling) {
waynek 0:9ea115d01a95 229 triggerSensorPolling = false;
waynek 0:9ea115d01a95 230 MY_DBG_PRINTF("speeds = %d,%d\n", motor1Speed, motor2Speed);
waynek 0:9ea115d01a95 231 sendData("hello", 5);
waynek 0:9ea115d01a95 232 }
waynek 0:9ea115d01a95 233 } else {
waynek 0:9ea115d01a95 234 motor1Speed = 0;
waynek 0:9ea115d01a95 235 motor1Speed = 0;
waynek 0:9ea115d01a95 236
waynek 0:9ea115d01a95 237 //ble.waitForEvent(); // low power wait for event
waynek 0:9ea115d01a95 238 }
waynek 0:9ea115d01a95 239
waynek 0:9ea115d01a95 240 setMotor1Speed(motor1Speed);
waynek 0:9ea115d01a95 241 setMotor2Speed(motor2Speed);
waynek 0:9ea115d01a95 242 }
waynek 0:9ea115d01a95 243 }