File content as of revision 2:9189fa810ef8:

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

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

// 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)
AnalogIn   sensor_l   (P0_4);  // SENS-L: P0_4  (A3)
AnalogIn   sensor_r   (P0_5);  // SENS-R: P0_5  (A4)

// flag for safety
bool StopFlag = false;

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

// you must adjust center position
#define RL_ADJ (-120)

// RC servo
// rl : -1.0 ? 1.0
void servo (float rl)
{
    servo_pwm.pulsewidth_us(1500 + RL_ADJ - (int)(500.0 * rl));
}

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)
{
    if (Handler->charHandle == charStick.getValueAttribute().getHandle()) 
    {
        uint16_t bytesRead;
        ble.readCharacteristicValue(charStick.getValueAttribute().getHandle(), payload, &bytesRead);
        memcpy( &stickData.bytes[0], payload, sizeof(stickData));
        
        //pc.printf("DATA:%02X %02X\n\r",stickData.bytes[0],stickData.bytes[1]);
        
        float m = (float)stickData.value.fb / 128.0;
        motor(m);
//        if(!StopFlag) motor(m);
        float s = (float)stickData.value.lr / 128.0;
        servo(s);
    }
}

// Distance sensor averaging number
#define AVE_SIZE 8

// Distance constant (for Sharp GP2Y0E03)
// Vout[V] = 2.369V - 0.0369*d[cm]
// sample = Vout[V]/3.3V * 0xFFFF
// sample = 47190 - 733.66*d[cm]
// 10cm : 39853
// 20cm : 32516
// 30cm : 25180
// 40cm : 17843
// 50cm : 10507
// 60cm : 3170
// 64cm : 0
#define DISTANCE_STOP 17843

void TimerHandler(void)
{
    // Averaging of dinstance sensors' value
    static int cnt = 0;
    static uint16_t r_buff[AVE_SIZE] ={0,};
    static uint16_t l_buff[AVE_SIZE] ={0,};
    static uint32_t r_acc = 0;
    static uint32_t l_acc = 0;
    
    uint16_t r = sensor_r;
    r_acc -= (uint32_t)r_buff[cnt];
    r_acc += (uint32_t)r;
    r_buff[cnt] = r;
    
    uint16_t l = sensor_l;
    l_acc -= (uint32_t)l_buff[cnt];
    l_acc += (uint32_t)l;
    l_buff[cnt] = l;
    
    cnt++;
    if(cnt >= AVE_SIZE){
        cnt = 0;
        uint16_t r_ave = (uint16_t)(r_acc / AVE_SIZE);
        uint16_t l_ave = (uint16_t)(l_acc / AVE_SIZE);
        // Stop! Obstacle Ahead
        if( (r_ave >= DISTANCE_STOP) && (l_ave >= DISTANCE_STOP) )
        {
            motor(0);
            StopFlag = true;
        }else{
            StopFlag = false;
        }
    }
}

int main(void)
{
    // initialize servo & motor
    servo_pwm.period_ms(20);
    servo(0);
    motor(0);
    
    Ticker ticker;
    ticker.attach_us(TimerHandler, 20000); // 20msec interval
    
    // initialize BLE
    ble.init();
    ble.onDisconnection(disconnectionCallback);
    ble.onDataWritten(WrittenHandler);  
    
    //pc.baud(9600);
    //pc.printf("BLE initialized\r\n");
    
    // 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,
    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(); 
    }
}