File content as of revision 0:9ea115d01a95:

/* 
 * 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);
    }
}