Release for tas lab

Dependencies:   BLE_API mbed

Fork of BLE_RCBController by Junichi Katsu

main.cpp

Committer:
tasdevelop
Date:
2015-03-19
Revision:
4:06b0361c5975
Parent:
3:6a9f4ea2e7c6

File content as of revision 4:06b0361c5975:

#include "mbed.h"
#include "BLEDevice.h"
#include "RCBController.h"

#define DBG 1

BLEDevice  ble;
Serial  pc(USBTX, USBRX);
/* LEDs for indication: */
DigitalOut  ConnectStateLed(LED1);
PwmOut  servo1(LED2);


/* RCBController Service */
static const uint16_t RCBController_service_uuid = 0xFFF0;
static const uint16_t RCBController_Characteristic_uuid = 0xFFF1;
uint8_t RCBControllerPayload[10] = {0,};

GattCharacteristic  ControllerChar (RCBController_Characteristic_uuid,RCBControllerPayload,10, 10,
								GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | 
								GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
GattCharacteristic *ControllerChars[] = {&ControllerChar};
GattService         RCBControllerService(RCBController_service_uuid, ControllerChars, sizeof(ControllerChars) / sizeof(GattCharacteristic *));

RCBController controller;

void onConnected(uint16_t h)
{
    ConnectStateLed = 0;
#if DBG
	pc.printf("Connected\n\r");
#endif
}

void onDisconnected(uint16_t h)
{
    ble.startAdvertising();
	ConnectStateLed = 1;
#if DBG
	pc.printf("Disconnected\n\r");
#endif
}
const int deg0msec = 600; // msec.
const int deg180msec = 2200; // msec.

//In float degree -90 to +90
void SetServoDegree(float degree)
{
	float range = (float)(deg180msec - deg0msec) / 180;
    float pwitdh = (float)((degree+90)*range)+(float)deg0msec;
    servo1.pulsewidth(pwitdh/1000000);
}

// GattEvent
void onDataWritten(uint16_t charHandle)
{
	if (charHandle == ControllerChar.getHandle()) {
		uint16_t bytesRead;
	 	ble.readCharacteristicValue(ControllerChar.getHandle(),RCBControllerPayload, &bytesRead);
        memcpy( &controller.data[0], RCBControllerPayload, sizeof(controller));
#if DBG

		pc.printf("DATA:%02X %02X %d %d %d %d %d %d %d %02X\n\r",controller.data[0],controller.data[1],controller.data[2],controller.data[3],controller.data[4],
															   controller.data[5],controller.data[6],controller.data[7],controller.data[8],controller.data[9]);
#endif

        if (controller.data[0] == 1){
	        SetServoDegree(-45);
        }
        else if (controller.data[1] == 0x40){
	        SetServoDegree(45);
        }
        else if (controller.data[1] == 0 && controller.data[0] == 0){
	        SetServoDegree(0);
        }
    }
}

/**************************************************************************/
/*!
    @brief  Program entry point
*/
/**************************************************************************/
int main(void)
{
#if DBG
		ConnectStateLed = 1;
        pc.baud(115200);  
		pc.printf("Start\n\r");
#endif
	
    ble.init(); 
    ble.onConnection(onConnected);
    ble.onDisconnection(onDisconnected);
    ble.onDataWritten(onDataWritten);
	
    /* setup advertising */
    ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
    ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
    ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
                                    (const uint8_t *)"mbed HRM1017", sizeof("mbed HRM1017") - 1);
    ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,
                                    (const uint8_t *)RCBController_service_uuid, sizeof(RCBController_service_uuid));

    ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
    ble.startAdvertising();

    ble.addService(RCBControllerService);
    servo1.period_ms(20);
    SetServoDegree(0);

    while (true) {
        ble.waitForEvent();
    }
}