Diff: main.cpp

Revision:

0:9ea115d01a95

Child:

1:5e5183fed184

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Mar 14 09:17:39 2016 +0000
@@ -0,0 +1,231 @@
+/* 
+ * Copyright (c) 2016 Wayne Keenan
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "MyDebug.h"
+#include "PicoBorgReverse.h"
+
+
+#include "mbed.h"
+#include "ble/BLE.h"
+#include "ble/services/UARTService.h"
+
+#include "NanoBitPins.h"
+#include "NanoBitDisplay.h"
+
+
+I2C i2cBus(MICROBIT_PIN_SDA, MICROBIT_PIN_SCL);
+
+#ifdef MY_DBG_ENABLED
+Serial serial(MICROBIT_PIN_USBTX, MICROBIT_PIN_USBRX);
+#endif
+
+
+const static char     DEVICE_NAME[]        = "MicroBorg";
+UARTService        *uartService;
+
+static volatile bool  triggerSensorPolling = false;
+
+PicoBorgReverse* pbr;
+
+int motor1Speed=0, motor2Speed=0;
+
+
+
+void onData(const GattWriteCallbackParams *params) {
+    const uint8_t* bytes  = params->data;
+    const uint16_t len    = params->len;
+    if (len>=6) {
+        //MY_DBG_PRINTF("BLE Recv: %d = %x\n", len, bytes[0]);    
+        if (   (0x00 == bytes[0]) && ( 0x01 == bytes[1]) ) {
+            motor1Speed = (bytes[2] + (bytes[3] << 8))-255;
+            motor2Speed = - ((bytes[4] + (bytes[5] << 8))-255);
+        }
+    }
+}
+
+void sendData(const void *buffer, size_t length) {
+    BLE& ble = BLE::Instance(BLE::DEFAULT_INSTANCE);
+    if (length>20) 
+        length = 20;
+        
+    if (length>0 && ble.getGapState().connected) {
+        ble.gattServer().write(uartService->getRXCharacteristicHandle(), static_cast<const uint8_t *>(buffer), length);
+    }
+}
+
+
+//----------------------------------------------------
+
+
+void connectionCallback(const Gap::ConnectionCallbackParams_t *params)
+{
+    Gap::Handle_t gap_handle = params->handle;
+    Gap::ConnectionParams_t new_params;
+ 
+    new_params.minConnectionInterval = 20; /* 20 msec */
+    new_params.maxConnectionInterval = 20; /* 20 msec */
+    new_params.connectionSupervisionTimeout = 5000; /* 5 seconds */
+    new_params.slaveLatency = 1;
+    BLE::Instance(BLE::DEFAULT_INSTANCE).gap().updateConnectionParams(gap_handle, &new_params);
+}
+
+void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
+{
+    BLE::Instance(BLE::DEFAULT_INSTANCE).gap().startAdvertising(); // restart advertising
+}
+
+    
+void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
+{
+    BLE &ble          = params->ble;
+    ble_error_t error = params->error;
+
+    if (error != BLE_ERROR_NONE) {
+        return;
+    }
+
+    ble.gap().onConnection(connectionCallback);
+    ble.gap().onDisconnection(disconnectionCallback);
+
+    /* Setup primary service. */
+    uartService = new UARTService(ble);
+    ble.onDataWritten(onData);
+    
+    
+    /* Setup advertising. */
+    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
+    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS, UARTServiceUUID_reversed , sizeof(UARTServiceUUID_reversed));
+    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
+    ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
+    ble.gap().setAdvertisingInterval(ble.gap().getMinAdvertisingInterval()); 
+    ble.gap().startAdvertising();
+
+
+}
+
+
+
+
+//----------------------------------------------------
+
+
+void periodicCallback(void)
+{
+    /* Note that the periodicCallback() executes in interrupt context, so it is safer to do
+     * heavy-weight sensor polling from the main thread. */
+    triggerSensorPolling = true;
+}
+
+
+
+// The opposite of low power :)  https://developer.mbed.org/questions/54515/How-to-lower-power-consumption/
+
+
+
+
+
+//----------------------------------------------------
+
+void motorReset() {
+    pbr->resetEpo(); 
+    pbr->setLed(pbr->getEpo()); 
+}
+
+bool motorInit()
+{
+    pbr = new PicoBorgReverse(&i2cBus);
+    if (!pbr->checkId()) {
+        MY_DBG_PRINT("Motor not found\n");
+        return false;
+    }
+    else {
+        MY_DBG_PRINT("Motors OK\n");
+    }
+    motorReset();              // Reset the EPO latch on the PicoBorg Reverse
+
+    // Ensure the communications failsafe is enabled on the PicoBorg Reverse
+    while (!pbr->getCommsFailsafe()) {
+        pbr->setCommsFailsafe(true);
+    }
+    return true;
+}
+
+void setMotor1Speed(int speed)
+{
+    pbr->setMotor1(speed);
+    pbr->setLed(pbr->getEpo());           // Set the PicoBorg Reverse LED on if the EPO has been latched
+}
+
+void setMotor2Speed(int speed)
+{
+    pbr->setMotor2(speed);
+    pbr->setLed(pbr->getEpo());           // Set the PicoBorg Reverse LED on if the EPO has been latched
+}
+
+
+
+//----------------------------------------------------
+
+#define TRACE_INIT()            MicroBitDisplay display; display.clear();
+#define TRACE_START()           display.setPixel(0,0);
+#define TRACE_BLE_INIT()        display.setPixel(0,1);
+#define TRACE_BLE_STARTED()     display.setPixel(0,2);
+#define TRACE_MOTOR_STATUS(x)   display.setPixel(0,3, x);
+#define TRACE_BLE_CONNECTED(x)  display.setPixel(0,4, x);
+
+int main(void)
+{
+    MY_DBG_PRINT("Started\n");
+    TRACE_INIT();
+    TRACE_START();
+    
+    Ticker ticker;
+    ticker.attach(periodicCallback, 1.0); // blink LED every second
+
+    //  Setup Bluetooht LE
+    BLE& ble = BLE::Instance(BLE::DEFAULT_INSTANCE);
+    ble.init(bleInitComplete);
+    TRACE_BLE_INIT();
+    while (ble.hasInitialized()  == false) { /* spin loop */ }
+    TRACE_BLE_STARTED()
+    
+    // Setup Motors
+    bool motorInitOK = motorInit();    
+    display.setPixel(0,3, motorInitOK);
+    
+
+    MY_DBG_PRINT("Go!\n");
+    while (1) {
+        bool isConnected = ble.getGapState().connected;
+        
+        TRACE_BLE_CONNECTED(isConnected);
+        
+        if (isConnected) {
+            if (triggerSensorPolling) {
+                triggerSensorPolling = false;
+                MY_DBG_PRINTF("speeds = %d,%d\n", motor1Speed, motor2Speed);
+                sendData("hello", 5);
+            }
+        } else {
+            motor1Speed = 0;
+            motor1Speed = 0;
+
+            //ble.waitForEvent(); // low power wait for event
+        }
+        
+        setMotor1Speed(motor1Speed);
+        setMotor2Speed(motor2Speed);
+    }
+}