File content as of revision 0:c5082e68ff72:

/*
 * Copyright (C) 2015 Bizan Nishimura (@lipoyang)
 *
 * 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 "mbed.h"
#include "BLEDevice.h"
//#include "Servo.h"


//#define BLE_UUID_TXRX_SERVICE            0x0000 /**< The UUID of the Nordic UART Service. */
//#define BLE_UUID_TX_CHARACTERISTIC       0x0002 /**< The UUID of the TX Characteristic. */
//#define BLE_UUIDS_RX_CHARACTERISTIC      0x0003 /**< The UUID of the RX Characteristic. */

//#define TXRX_BUF_LEN                     20

//#define DIGITAL_OUT_PIN                  P0_9       //TXD
//#define DIGITAL_IN_PIN                   P0_10      //CTS
//#define PWM_PIN                          P0_11      //RXD
//#define SERVO_PIN                        P0_8       //RTS
//#define ANALOG_IN_PIN                    P0_4       //P04

BLEDevice  ble;

// BluePropo service UUID
//static const uint16_t UUID_BLUEPROPO = 0xFFF0;
static const uint8_t UUID_BLUEPROPO[] = 
{ 0xc4, 0x9d, 0xfd, 0x1b, 0x86, 0x04, 0x41, 0xd2, 0x89, 0x43, 0x13, 0x6f, 0x21, 0x4d, 0xd0, 0xbf };

// BluePropo::Stick characteristic UUID
//static const uint16_t UUID_BLUEPROPO_STICK = 0xFFF1;
static const uint8_t UUID_BLUEPROPO_STICK[] =
{ 0x74, 0x25, 0xfb, 0xa0, 0x72, 0x15, 0x41, 0x36, 0xaa, 0x3f, 0x07, 0x2a, 0xa0, 0x7d, 0x93, 0x54 };

// Device Name (for display)
#define DEVICE_NAME "MiniSteer BLE Nano"

// BluePropo::Stick data structure
union StickData
{
    struct  {
        // F(-128)<- 0 ->B(+127)
        signed char fb;
        // L(-128)<- 0 ->R(+127)
        signed char lr;
    }value;
    unsigned char bytes[2];
};
StickData stickData;

// buffer for BluePropo payload
uint8_t payload[10] = {0,};

// BluePropo::Stick characteristic
GattCharacteristic  charStick (UUID_BLUEPROPO_STICK, payload, 2, 2,
                                GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | 
                                GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
// BluePropo characteristics set
GattCharacteristic *chars[] = {&charStick};
// BluePropo service
GattService         serviceBluePropo(UUID_BLUEPROPO, chars, sizeof(chars) / sizeof(GattCharacteristic *));


//DigitalOut      LED_SET(DIGITAL_OUT_PIN);
//DigitalIn       BUTTON(DIGITAL_IN_PIN);
//PwmOut          PWM(PWM_PIN);
//AnalogIn        ANALOG(ANALOG_IN_PIN);
//Servo           MYSERVO(SERVO_PIN);

//Serial pc(USBTX, USBRX);

//static uint8_t analog_enabled = 0;
//static uint8_t old_state = 0;

// The Nordic UART Service
//static const uint8_t uart_base_uuid[] = {0x71, 0x3D, 0, 0, 0x50, 0x3E, 0x4C, 0x75, 0xBA, 0x94, 0x31, 0x48, 0xF1, 0x8D, 0x94, 0x1E};
//static const uint8_t uart_tx_uuid[]   = {0x71, 0x3D, 0, 3, 0x50, 0x3E, 0x4C, 0x75, 0xBA, 0x94, 0x31, 0x48, 0xF1, 0x8D, 0x94, 0x1E};
//static const uint8_t uart_rx_uuid[]   = {0x71, 0x3D, 0, 2, 0x50, 0x3E, 0x4C, 0x75, 0xBA, 0x94, 0x31, 0x48, 0xF1, 0x8D, 0x94, 0x1E};
//static const uint8_t uart_base_uuid_rev[] = {0x1E, 0x94, 0x8D, 0xF1, 0x48, 0x31, 0x94, 0xBA, 0x75, 0x4C, 0x3E, 0x50, 0, 0, 0x3D, 0x71};



//uint8_t txPayload[TXRX_BUF_LEN] = {0,};
//uint8_t rxPayload[TXRX_BUF_LEN] = {0,};

//static uint8_t rx_buf[TXRX_BUF_LEN];
//static uint8_t rx_len=0;

/*
GattCharacteristic  txCharacteristic (uart_tx_uuid, txPayload, 1, TXRX_BUF_LEN, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
                                      
GattCharacteristic  rxCharacteristic (uart_rx_uuid, rxPayload, 1, TXRX_BUF_LEN, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
                                      
GattCharacteristic *uartChars[] = {&txCharacteristic, &rxCharacteristic};

GattService         uartService(uart_base_uuid, uartChars, sizeof(uartChars) / sizeof(GattCharacteristic *));
*/

// pin asign
DigitalOut tb6612_ain1(P0_10); // AIN1:  P0_10 (D2)
DigitalOut tb6612_ain2(P0_9);  // AIN2:  P0_9  (D1)
PwmOut     tb6612_pwma(P0_11); // PWMA:  P0_11 (D0)
PwmOut     servo_pwm  (P0_8);  // SERVO: P0_8  (D3)

// SENS-L: P0_4 (A3)
// SENS-R: P0_5 (A4)

// DC motor driver (TB6612)
void motor (float speed)
{
    if (speed > 0) {
        // CW
        tb6612_pwma = speed;
        tb6612_ain1 = 1;
        tb6612_ain2 = 0;
    } else
    if (speed < 0) {
        // CCW
        tb6612_pwma = - speed;
        tb6612_ain1 = 0;
        tb6612_ain2 = 1;
    } else {
        // stop
        tb6612_pwma = 1;
        tb6612_ain1 = 0;
        tb6612_ain2 = 0;
//        // break
//        tb6612_pwma = 1;
//        tb6612_ain1 = 1;
//        tb6612_ain2 = 1;
    }
}

// RC servo
void servo (float deg)
{
    servo_pwm.pulsewidth_us(1500 + (int)(500.0 * deg));
}

void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
{
    //pc.printf("Disconnected \r\n");
    //pc.printf("Restart advertising \r\n");
    ble.startAdvertising();
}

void WrittenHandler(const GattCharacteristicWriteCBParams *Handler)
{   
//    uint8_t buf[TXRX_BUF_LEN];
//    uint16_t bytesRead;
    
    if (Handler->charHandle == charStick.getValueAttribute().getHandle()) 
    {
        uint16_t bytesRead;
        ble.readCharacteristicValue(charStick.getValueAttribute().getHandle(), payload, &bytesRead);
        memcpy( &stickData.bytes[0], payload, sizeof(stickData));
#if DBG

        pc.printf("DATA:%02X %02X\n\r",stickData.bytes[0],stickData.bytes[1]);
#endif
        float m = (float)stickData.value.fb / 128.0;
        motor(m);
        float s = (float)stickData.value.lr / 128.0;
        servo(s);

/*
        ble.readCharacteristicValue(txCharacteristic.getValueAttribute().getHandle(), buf, &bytesRead);
        memset(txPayload, 0, TXRX_BUF_LEN);
        memcpy(txPayload, buf, TXRX_BUF_LEN);       
        
        //for(index=0; index<bytesRead; index++)
            //pc.putc(buf[index]);
            
        if(buf[0] == 0x01)
        {
            if(buf[1] == 0x01)
                LED_SET = 1;
            else
                LED_SET = 0;    
        }
        else if(buf[0] == 0xA0)
        {
            if(buf[1] == 0x01)
                analog_enabled = 1;
            else
                analog_enabled = 0;
        }
        else if(buf[0] == 0x02)
        {
            float value = (float)buf[1]/255;
            PWM = value;
        }
        else if(buf[0] == 0x03)
        {
            MYSERVO.write(buf[1]);
        }
        else if(buf[0] == 0x04)
        {
            analog_enabled = 0;
            PWM = 0;
            MYSERVO.write(0);
            LED_SET = 0;
            old_state = 0;    
        }
*/
    }
}
/*
void uartCB(void)
{   
    while(pc.readable())    
    {
        rx_buf[rx_len++] = pc.getc();    
        if(rx_len>=20 || rx_buf[rx_len-1]=='\0' || rx_buf[rx_len-1]=='\n')
        {
            ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), rx_buf, rx_len); 
            pc.printf("RecHandler \r\n");
            pc.printf("Length: ");
            pc.putc(rx_len);
            pc.printf("\r\n");
            rx_len = 0;
            break;
        }
    }
}
*/
void m_status_check_handle(void)
{
/*
    uint8_t buf[3];
    if (analog_enabled)  // if analog reading enabled
    {
        // Read and send out
        float s = ANALOG;
        uint16_t value = s*1024; 
        buf[0] = (0x0B);
        buf[1] = (value >> 8);
        buf[2] = (value);
        ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), buf, 3); 
    }
    
    // If digital in changes, report the state
    if (BUTTON != old_state)
    {
        old_state = BUTTON;
        
        if (BUTTON == 1)
        {
            buf[0] = (0x0A);
            buf[1] = (0x01);
            buf[2] = (0x00);    
            ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), buf, 3); 
        }
        else
        {
            buf[0] = (0x0A);
            buf[1] = (0x00);
            buf[2] = (0x00);
           ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), buf, 3); 
        }
    }
*/
}


int main(void)
{
    // initialize servo & motor
    servo_pwm.period_ms(20);
    servo(0.5);
    motor(0);
    
    Ticker ticker;
    ticker.attach_us(m_status_check_handle, 200000);
    
    // initialize BLE
    ble.init();
    ble.onDisconnection(disconnectionCallback);
    ble.onDataWritten(WrittenHandler);  
    
    //pc.baud(9600);
    //pc.printf("SimpleChat Init \r\n");
    //pc.attach( uartCB , pc.RxIrq);
    
    // setup advertising 
    ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
    ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
    ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
                                    (const uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME) - 1);
//    ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
//                                    (const uint8_t *)uart_base_uuid_rev, sizeof(uart_base_uuid));
    ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,
                                    (const uint8_t *)UUID_BLUEPROPO, sizeof(UUID_BLUEPROPO));
    ble.setAdvertisingInterval(160); // 100ms; in multiples of 0.625ms. 
    ble.addService(serviceBluePropo);
    ble.startAdvertising(); 
    //pc.printf("Advertising Start \r\n");
    
    // main loop (wait for BLE event)
    while(true)
    {
        ble.waitForEvent(); 
    }
}