File content as of revision 5:88d466dea486:

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

#define DBG 0

BLEDevice  ble;
Serial  pc(USBTX, USBRX);
/* LEDs for indication: */
DigitalOut  ConnectStateLed(P0_18);
DigitalOut  testLED(P0_19);
DigitalOut  outlow(P0_20);
DigitalIn	sw1(P0_16);
DigitalIn	sw2(P0_17);

//PwmOut  ControllerStateLed(LED2);

TB6612 left(P0_29,P0_24,P0_23);
TB6612 right(P0_28,P0_30,P0_0);

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

int cmode = 0;

void onConnected(Gap::Handle_t handle, const Gap::ConnectionParams_t *params)
{
    ConnectStateLed = 0;
#if DBG
	pc.printf("Connected\n\r");
#endif
}

void onDisconnected(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
{
    ble.startAdvertising();
	ConnectStateLed = 1;
#if DBG
	pc.printf("Disconnected\n\r");
#endif
}


// GattEvent
void onDataWritten(const GattCharacteristicWriteCBParams *params)
{
	if (params->charHandle == ControllerChar.getValueAttribute().getHandle())
	{
		uint16_t bytesRead;
		memcpy( &controller.data[0], params->data , params->len );
	 //	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
	    float right_factor;
	    float left_factor;

		left_factor = ((float)((int)controller.status.LeftAnalogUD -128) / 128.0);
		right_factor = ((float)((int)controller.status.RightAnalogUD -128) / 128.0);
		
		if( (left_factor != 0.0)||(right_factor != 0.0) )
		{
			left = left_factor;
			right = right_factor;
		}
		else
		{
			float factor = ((float)((int)controller.status.AcceleX -128) / 128.0); 

		    float right_factor = ((factor <= 0.0) ? 1.0 : 1.0 - (factor*2));
		    float left_factor = ((factor >= 0.0) ? 1.0 : 1.0 - (-factor*2));

			if( controller.status.B == 1 )
			{
				left = left_factor;
				right = right_factor;
			}
			else if( controller.status.A == 1 )
			{
				left = -right_factor;
				right = -left_factor;
			}
			else if( controller.status.UP == 1 )
			{
				left = 1.0;
				right = 1.0;
			}
			else if( controller.status.DOWN == 1 )
			{
				left = -1.0;
				right = -1.0;
			}
			else if( controller.status.RIGHT == 1 )
			{
				left = 1.0;
				right = -1.0;
			}
			else if( controller.status.LEFT == 1 )
			{
				left = -1.0;
				right = 1.0;
			}
			else
			{
				left = 0.0;
				right = 0.0;	
			}
		}
		//ControllerStateLed = (float)controller.status.LeftAnalogLR / 255.0;		
	}
		 
}

/**************************************************************************/
/*!
    @brief  Program entry point
*/
/**************************************************************************/
int main(void)
{
	pc.baud(921600);
	
	sw1.mode(PullUp);
	sw2.mode(PullUp);
	
	ConnectStateLed = 1;
#if DBG
		pc.printf("Start\n\r");
#endif
	outlow = 0;
	
    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 WallbotBLE", sizeof("mbed WallbotBLE") - 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);
    
    testLED = cmode;
	int old_sw = 0;

    while (true) {
#if 0
        if(sw1 == 0 && old_sw == 1)
        {
        	cmode = !cmode;
        	testLED = cmode;
        	wait(0.1);
        }
        old_sw = sw1;
#endif
        ble.waitForEvent();
    }
}