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@0:9ea115d01a95, 2016-03-14 (annotated)

Committer:
waynek
Date:
Mon Mar 14 09:17:39 2016 +0000
Revision:
0:9ea115d01a95
Child:
1:5e5183fed184
initial public version

Who changed what in which revision?

UserRevisionLine numberNew contents of line
waynek 0:9ea115d01a95 1 /*
waynek 0:9ea115d01a95 2 * Copyright (c) 2016 Wayne Keenan
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 0:9ea115d01a95 16 #include "MyDebug.h"
waynek 0:9ea115d01a95 17 #include "PicoBorgReverse.h"
waynek 0:9ea115d01a95 18
waynek 0:9ea115d01a95 19
waynek 0:9ea115d01a95 20 #include "mbed.h"
waynek 0:9ea115d01a95 21 #include "ble/BLE.h"
waynek 0:9ea115d01a95 22 #include "ble/services/UARTService.h"
waynek 0:9ea115d01a95 23
waynek 0:9ea115d01a95 24 #include "NanoBitPins.h"
waynek 0:9ea115d01a95 25 #include "NanoBitDisplay.h"
waynek 0:9ea115d01a95 26
waynek 0:9ea115d01a95 27
waynek 0:9ea115d01a95 28 I2C i2cBus(MICROBIT_PIN_SDA, MICROBIT_PIN_SCL);
waynek 0:9ea115d01a95 29
waynek 0:9ea115d01a95 30 #ifdef MY_DBG_ENABLED
waynek 0:9ea115d01a95 31 Serial serial(MICROBIT_PIN_USBTX, MICROBIT_PIN_USBRX);
waynek 0:9ea115d01a95 32 #endif
waynek 0:9ea115d01a95 33
waynek 0:9ea115d01a95 34
waynek 0:9ea115d01a95 35 const static char DEVICE_NAME[] = "MicroBorg";
waynek 0:9ea115d01a95 36 UARTService *uartService;
waynek 0:9ea115d01a95 37
waynek 0:9ea115d01a95 38 static volatile bool triggerSensorPolling = false;
waynek 0:9ea115d01a95 39
waynek 0:9ea115d01a95 40 PicoBorgReverse* pbr;
waynek 0:9ea115d01a95 41
waynek 0:9ea115d01a95 42 int motor1Speed=0, motor2Speed=0;
waynek 0:9ea115d01a95 43
waynek 0:9ea115d01a95 44
waynek 0:9ea115d01a95 45
waynek 0:9ea115d01a95 46 void onData(const GattWriteCallbackParams *params) {
waynek 0:9ea115d01a95 47 const uint8_t* bytes = params->data;
waynek 0:9ea115d01a95 48 const uint16_t len = params->len;
waynek 0:9ea115d01a95 49 if (len>=6) {
waynek 0:9ea115d01a95 50 //MY_DBG_PRINTF("BLE Recv: %d = %x\n", len, bytes[0]);
waynek 0:9ea115d01a95 51 if ( (0x00 == bytes[0]) && ( 0x01 == bytes[1]) ) {
waynek 0:9ea115d01a95 52 motor1Speed = (bytes[2] + (bytes[3] << 8))-255;
waynek 0:9ea115d01a95 53 motor2Speed = - ((bytes[4] + (bytes[5] << 8))-255);
waynek 0:9ea115d01a95 54 }
waynek 0:9ea115d01a95 55 }
waynek 0:9ea115d01a95 56 }
waynek 0:9ea115d01a95 57
waynek 0:9ea115d01a95 58 void sendData(const void *buffer, size_t length) {
waynek 0:9ea115d01a95 59 BLE& ble = BLE::Instance(BLE::DEFAULT_INSTANCE);
waynek 0:9ea115d01a95 60 if (length>20)
waynek 0:9ea115d01a95 61 length = 20;
waynek 0:9ea115d01a95 62
waynek 0:9ea115d01a95 63 if (length>0 && ble.getGapState().connected) {
waynek 0:9ea115d01a95 64 ble.gattServer().write(uartService->getRXCharacteristicHandle(), static_cast<const uint8_t *>(buffer), length);
waynek 0:9ea115d01a95 65 }
waynek 0:9ea115d01a95 66 }
waynek 0:9ea115d01a95 67
waynek 0:9ea115d01a95 68
waynek 0:9ea115d01a95 69 //----------------------------------------------------
waynek 0:9ea115d01a95 70
waynek 0:9ea115d01a95 71
waynek 0:9ea115d01a95 72 void connectionCallback(const Gap::ConnectionCallbackParams_t *params)
waynek 0:9ea115d01a95 73 {
waynek 0:9ea115d01a95 74 Gap::Handle_t gap_handle = params->handle;
waynek 0:9ea115d01a95 75 Gap::ConnectionParams_t new_params;
waynek 0:9ea115d01a95 76
waynek 0:9ea115d01a95 77 new_params.minConnectionInterval = 20; /* 20 msec */
waynek 0:9ea115d01a95 78 new_params.maxConnectionInterval = 20; /* 20 msec */
waynek 0:9ea115d01a95 79 new_params.connectionSupervisionTimeout = 5000; /* 5 seconds */
waynek 0:9ea115d01a95 80 new_params.slaveLatency = 1;
waynek 0:9ea115d01a95 81 BLE::Instance(BLE::DEFAULT_INSTANCE).gap().updateConnectionParams(gap_handle, &new_params);
waynek 0:9ea115d01a95 82 }
waynek 0:9ea115d01a95 83
waynek 0:9ea115d01a95 84 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
waynek 0:9ea115d01a95 85 {
waynek 0:9ea115d01a95 86 BLE::Instance(BLE::DEFAULT_INSTANCE).gap().startAdvertising(); // restart advertising
waynek 0:9ea115d01a95 87 }
waynek 0:9ea115d01a95 88
waynek 0:9ea115d01a95 89
waynek 0:9ea115d01a95 90 void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
waynek 0:9ea115d01a95 91 {
waynek 0:9ea115d01a95 92 BLE &ble = params->ble;
waynek 0:9ea115d01a95 93 ble_error_t error = params->error;
waynek 0:9ea115d01a95 94
waynek 0:9ea115d01a95 95 if (error != BLE_ERROR_NONE) {
waynek 0:9ea115d01a95 96 return;
waynek 0:9ea115d01a95 97 }
waynek 0:9ea115d01a95 98
waynek 0:9ea115d01a95 99 ble.gap().onConnection(connectionCallback);
waynek 0:9ea115d01a95 100 ble.gap().onDisconnection(disconnectionCallback);
waynek 0:9ea115d01a95 101
waynek 0:9ea115d01a95 102 /* Setup primary service. */
waynek 0:9ea115d01a95 103 uartService = new UARTService(ble);
waynek 0:9ea115d01a95 104 ble.onDataWritten(onData);
waynek 0:9ea115d01a95 105
waynek 0:9ea115d01a95 106
waynek 0:9ea115d01a95 107 /* Setup advertising. */
waynek 0:9ea115d01a95 108 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
waynek 0:9ea115d01a95 109 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS, UARTServiceUUID_reversed , sizeof(UARTServiceUUID_reversed));
waynek 0:9ea115d01a95 110 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
waynek 0:9ea115d01a95 111 ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
waynek 0:9ea115d01a95 112 ble.gap().setAdvertisingInterval(ble.gap().getMinAdvertisingInterval());
waynek 0:9ea115d01a95 113 ble.gap().startAdvertising();
waynek 0:9ea115d01a95 114
waynek 0:9ea115d01a95 115
waynek 0:9ea115d01a95 116 }
waynek 0:9ea115d01a95 117
waynek 0:9ea115d01a95 118
waynek 0:9ea115d01a95 119
waynek 0:9ea115d01a95 120
waynek 0:9ea115d01a95 121 //----------------------------------------------------
waynek 0:9ea115d01a95 122
waynek 0:9ea115d01a95 123
waynek 0:9ea115d01a95 124 void periodicCallback(void)
waynek 0:9ea115d01a95 125 {
waynek 0:9ea115d01a95 126 /* Note that the periodicCallback() executes in interrupt context, so it is safer to do
waynek 0:9ea115d01a95 127 * heavy-weight sensor polling from the main thread. */
waynek 0:9ea115d01a95 128 triggerSensorPolling = true;
waynek 0:9ea115d01a95 129 }
waynek 0:9ea115d01a95 130
waynek 0:9ea115d01a95 131
waynek 0:9ea115d01a95 132
waynek 0:9ea115d01a95 133 // The opposite of low power :) https://developer.mbed.org/questions/54515/How-to-lower-power-consumption/
waynek 0:9ea115d01a95 134
waynek 0:9ea115d01a95 135
waynek 0:9ea115d01a95 136
waynek 0:9ea115d01a95 137
waynek 0:9ea115d01a95 138
waynek 0:9ea115d01a95 139 //----------------------------------------------------
waynek 0:9ea115d01a95 140
waynek 0:9ea115d01a95 141 void motorReset() {
waynek 0:9ea115d01a95 142 pbr->resetEpo();
waynek 0:9ea115d01a95 143 pbr->setLed(pbr->getEpo());
waynek 0:9ea115d01a95 144 }
waynek 0:9ea115d01a95 145
waynek 0:9ea115d01a95 146 bool motorInit()
waynek 0:9ea115d01a95 147 {
waynek 0:9ea115d01a95 148 pbr = new PicoBorgReverse(&i2cBus);
waynek 0:9ea115d01a95 149 if (!pbr->checkId()) {
waynek 0:9ea115d01a95 150 MY_DBG_PRINT("Motor not found\n");
waynek 0:9ea115d01a95 151 return false;
waynek 0:9ea115d01a95 152 }
waynek 0:9ea115d01a95 153 else {
waynek 0:9ea115d01a95 154 MY_DBG_PRINT("Motors OK\n");
waynek 0:9ea115d01a95 155 }
waynek 0:9ea115d01a95 156 motorReset(); // Reset the EPO latch on the PicoBorg Reverse
waynek 0:9ea115d01a95 157
waynek 0:9ea115d01a95 158 // Ensure the communications failsafe is enabled on the PicoBorg Reverse
waynek 0:9ea115d01a95 159 while (!pbr->getCommsFailsafe()) {
waynek 0:9ea115d01a95 160 pbr->setCommsFailsafe(true);
waynek 0:9ea115d01a95 161 }
waynek 0:9ea115d01a95 162 return true;
waynek 0:9ea115d01a95 163 }
waynek 0:9ea115d01a95 164
waynek 0:9ea115d01a95 165 void setMotor1Speed(int speed)
waynek 0:9ea115d01a95 166 {
waynek 0:9ea115d01a95 167 pbr->setMotor1(speed);
waynek 0:9ea115d01a95 168 pbr->setLed(pbr->getEpo()); // Set the PicoBorg Reverse LED on if the EPO has been latched
waynek 0:9ea115d01a95 169 }
waynek 0:9ea115d01a95 170
waynek 0:9ea115d01a95 171 void setMotor2Speed(int speed)
waynek 0:9ea115d01a95 172 {
waynek 0:9ea115d01a95 173 pbr->setMotor2(speed);
waynek 0:9ea115d01a95 174 pbr->setLed(pbr->getEpo()); // Set the PicoBorg Reverse LED on if the EPO has been latched
waynek 0:9ea115d01a95 175 }
waynek 0:9ea115d01a95 176
waynek 0:9ea115d01a95 177
waynek 0:9ea115d01a95 178
waynek 0:9ea115d01a95 179 //----------------------------------------------------
waynek 0:9ea115d01a95 180
waynek 0:9ea115d01a95 181 #define TRACE_INIT() MicroBitDisplay display; display.clear();
waynek 0:9ea115d01a95 182 #define TRACE_START() display.setPixel(0,0);
waynek 0:9ea115d01a95 183 #define TRACE_BLE_INIT() display.setPixel(0,1);
waynek 0:9ea115d01a95 184 #define TRACE_BLE_STARTED() display.setPixel(0,2);
waynek 0:9ea115d01a95 185 #define TRACE_MOTOR_STATUS(x) display.setPixel(0,3, x);
waynek 0:9ea115d01a95 186 #define TRACE_BLE_CONNECTED(x) display.setPixel(0,4, x);
waynek 0:9ea115d01a95 187
waynek 0:9ea115d01a95 188 int main(void)
waynek 0:9ea115d01a95 189 {
waynek 0:9ea115d01a95 190 MY_DBG_PRINT("Started\n");
waynek 0:9ea115d01a95 191 TRACE_INIT();
waynek 0:9ea115d01a95 192 TRACE_START();
waynek 0:9ea115d01a95 193
waynek 0:9ea115d01a95 194 Ticker ticker;
waynek 0:9ea115d01a95 195 ticker.attach(periodicCallback, 1.0); // blink LED every second
waynek 0:9ea115d01a95 196
waynek 0:9ea115d01a95 197 // Setup Bluetooht LE
waynek 0:9ea115d01a95 198 BLE& ble = BLE::Instance(BLE::DEFAULT_INSTANCE);
waynek 0:9ea115d01a95 199 ble.init(bleInitComplete);
waynek 0:9ea115d01a95 200 TRACE_BLE_INIT();
waynek 0:9ea115d01a95 201 while (ble.hasInitialized() == false) { /* spin loop */ }
waynek 0:9ea115d01a95 202 TRACE_BLE_STARTED()
waynek 0:9ea115d01a95 203
waynek 0:9ea115d01a95 204 // Setup Motors
waynek 0:9ea115d01a95 205 bool motorInitOK = motorInit();
waynek 0:9ea115d01a95 206 display.setPixel(0,3, motorInitOK);
waynek 0:9ea115d01a95 207
waynek 0:9ea115d01a95 208
waynek 0:9ea115d01a95 209 MY_DBG_PRINT("Go!\n");
waynek 0:9ea115d01a95 210 while (1) {
waynek 0:9ea115d01a95 211 bool isConnected = ble.getGapState().connected;
waynek 0:9ea115d01a95 212
waynek 0:9ea115d01a95 213 TRACE_BLE_CONNECTED(isConnected);
waynek 0:9ea115d01a95 214
waynek 0:9ea115d01a95 215 if (isConnected) {
waynek 0:9ea115d01a95 216 if (triggerSensorPolling) {
waynek 0:9ea115d01a95 217 triggerSensorPolling = false;
waynek 0:9ea115d01a95 218 MY_DBG_PRINTF("speeds = %d,%d\n", motor1Speed, motor2Speed);
waynek 0:9ea115d01a95 219 sendData("hello", 5);
waynek 0:9ea115d01a95 220 }
waynek 0:9ea115d01a95 221 } else {
waynek 0:9ea115d01a95 222 motor1Speed = 0;
waynek 0:9ea115d01a95 223 motor1Speed = 0;
waynek 0:9ea115d01a95 224
waynek 0:9ea115d01a95 225 //ble.waitForEvent(); // low power wait for event
waynek 0:9ea115d01a95 226 }
waynek 0:9ea115d01a95 227
waynek 0:9ea115d01a95 228 setMotor1Speed(motor1Speed);
waynek 0:9ea115d01a95 229 setMotor2Speed(motor2Speed);
waynek 0:9ea115d01a95 230 }
waynek 0:9ea115d01a95 231 }