File content as of revision 1:c227f3064399:

/* mbed Microcontroller Library
 * Copyright (c) 2006-2015 ARM Limited
 *
 * 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 "ble/BLE.h"
#include "ble/DiscoveredCharacteristic.h"
#include "ble/DiscoveredService.h"

BLE        ble;
DigitalOut led(P0_19,1);
DigitalOut myled(P0_8);
DigitalOut neo(P0_6,1);
DigitalOut pin4(P0_4);
DigitalOut pin5(P0_5);
DigitalOut pin15(P0_15);
DigitalOut pin29(P0_29);

static const unsigned NUM_ROBOTS = 8;
static const unsigned ADDR_LEN = 6;

uint16_t customServiceUUID  = 0xA000;
uint16_t readwriteCharUUID  = 0xA001;
static const uint16_t secure_code[] = {0xABCD};

uint8_t info[NUM_ROBOTS]= {0};

ReadWriteArrayGattCharacteristic<uint8_t, NUM_ROBOTS> readwriteChar(readwriteCharUUID, info, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE);

//GattCharacteristic  readwriteChar (readwriteCharUUID, info, NUM_ROBOTS, NUM_ROBOTS, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE);

GattCharacteristic *characteristics[] = {&readwriteChar};

GattService        customService(customServiceUUID, characteristics, sizeof(characteristics) / sizeof(GattCharacteristic *));

bool triggerLedCharacteristic = false;
DiscoveredCharacteristic characteristics_discovered;

int8_t temp;
uint8_t pt_addr = 0;
uint8_t list_addr[NUM_ROBOTS][ADDR_LEN+NUM_ROBOTS]= {0,};
int8_t list_rssi[NUM_ROBOTS];
uint8_t initial_list_addr[NUM_ROBOTS][ADDR_LEN + 1]= {0,};
// 1st Device, 2nd device has '10(2)' information
uint8_t info_t[NUM_ROBOTS]= {0,};
uint8_t point;

void RGB_Show(uint8_t r, uint8_t g, uint8_t b)
{
    uint8_t rgb[3] = {g, r, b};
    uint8_t *p = rgb;
    uint8_t *end = p + 3;
    while (p < end) {
        uint8_t pix = *p++;
        for (uint8_t mask = 0x80; mask; mask >>= 1) {
            if (pix & mask) {
                // T1H 760ns
                NRF_GPIO->OUTSET = (1UL << 8);
                NRF_GPIO->OUTSET = (1UL << 8);
                NRF_GPIO->OUTSET = (1UL << 8);
                // T1L 660ns
                NRF_GPIO->OUTCLR = (1UL << 8);
            } else {
                // T0H 380ns
                NRF_GPIO->OUTSET = (1UL << 8);
                // T0L 840ns
                NRF_GPIO->OUTCLR = (1UL << 8);
                NRF_GPIO->OUTCLR = (1UL << 8);
                NRF_GPIO->OUTCLR = (1UL << 8);
                NRF_GPIO->OUTCLR = (1UL << 8);
            }
        }
    }
    NRF_GPIO->OUTCLR = (1UL << 8);
    wait_us(100);
}

void periodicCallback(void)
{
    led=!led;
    point = 0;
    for(int i=0; i<NUM_ROBOTS; i++) {
        point+=info[i];
    }
    if(point==0) {
        RGB_Show(0x00,0x00,0xff);
    } else if(point==1) {
        RGB_Show(0xff,0xff,0xff);
    } else if(point==2) {
        RGB_Show(0x00,0xff,0x00);
    } else if(point==3) {
        RGB_Show(0x00,0xff,0xff);
    } else if(point==4) {
        RGB_Show(0xff,0x00,0x00);
    } else if(point==5) {
        RGB_Show(0xff,0x00,0xff);
    } else if(point==6) {
        RGB_Show(0xff,0xff,0x00);
    } else if(point==7) {
        RGB_Show(0xff,0xff,0xff);
    } else if(point==8) {
        RGB_Show(0x00,0x00,0xff);
    }
}

void initialSetup(const Gap::AdvertisementCallbackParams_t *params)
{
    if ( params->advertisingData[1] == 0xAF ) {
        for(int i = 0; i < NUM_ROBOTS; i++) {
            if(initial_list_addr[i][5] == params->peerAddr[5]
                    && initial_list_addr[i][4] == params->peerAddr[4]
                    && initial_list_addr[i][3] == params->peerAddr[3]
                    && initial_list_addr[i][2] == params->peerAddr[2]
                    && initial_list_addr[i][1] == params->peerAddr[1]
                    && initial_list_addr[i][0] == params->peerAddr[0]) {
                break;
            }
            if(initial_list_addr[i][5] == 0  && initial_list_addr[i][4] == 0
                    && initial_list_addr[i][3] == 0  && initial_list_addr[i][2] == 0
                    && initial_list_addr[i][1] == 0  && initial_list_addr[i][0] == 0) {
                initial_list_addr[i][5] = params->peerAddr[5];
                initial_list_addr[i][4] = params->peerAddr[4];
                initial_list_addr[i][3] = params->peerAddr[3];
                initial_list_addr[i][2] = params->peerAddr[2];
                initial_list_addr[i][1] = params->peerAddr[1];
                initial_list_addr[i][0] = params->peerAddr[0];

                initial_list_addr[i][6] = 1;
                break;
            }
        }
        printf("initialsetup\r\n");
        printf("adv peerAddr[%02x %02x %02x %02x %02x %02x] rssi %d, isScanResponse %u, AdvertisementType %u, %0x %0x\r\n",
               params->peerAddr[5], params->peerAddr[4], params->peerAddr[3], params->peerAddr[2], params->peerAddr[1], params->peerAddr[0],
               params->rssi, params->isScanResponse, params->type, params->advertisingData[1], params->advertisingData[0]);
    }
}

void advertisementCallback(const Gap::AdvertisementCallbackParams_t *params)
{
    if ( params->rssi > -50 && params->advertisingData[1] == 0xAF ) {
        printf("adv peerAddr[%02x %02x %02x %02x %02x %02x] rssi %d, isScanResponse %u, AdvertisementType %u, %0x %0x %0x\r\n",
               params->peerAddr[5], params->peerAddr[4], params->peerAddr[3], params->peerAddr[2], params->peerAddr[1], params->peerAddr[0],
               params->rssi, params->isScanResponse, params->type, params->advertisingData[3], params->advertisingData[2], params->advertisingData[1]);

        ble.gap().connect(params->peerAddr, Gap::ADDR_TYPE_RANDOM_STATIC, NULL, NULL);
    }
    else{
        printf("adv peerAddr[%02x %02x %02x %02x %02x %02x] rssi %d, isScanResponse %u, AdvertisementType %u, %0x %0x %0x\r\n",
               params->peerAddr[5], params->peerAddr[4], params->peerAddr[3], params->peerAddr[2], params->peerAddr[1], params->peerAddr[0],
               params->rssi, params->isScanResponse, params->type, params->advertisingData[3], params->advertisingData[2], params->advertisingData[1]);
    }
    /*
    int i;

    for(i = 0; i < NUM_ROBOTS; i++) {
        if(list_addr[i][5] == params->peerAddr[5] && list_addr[i][4] == params->peerAddr[4] && list_addr[i][3] == params->peerAddr[3]
                && list_addr[i][2] == params->peerAddr[2] && list_addr[i][1] == params->peerAddr[1] && list_addr[i][0] == params->peerAddr[0]) {
            list_rssi[i] = params->rssi;
            break;
        }
        if(list_addr[i][5] == 0 && list_addr[i][4] == 0 && list_addr[i][3] == 0 && list_addr[i][2] == 0 && list_addr[i][1] == 0 && list_addr[i][0] == 0) {
            list_addr[i][5] = params->peerAddr[5];
            list_addr[i][4] = params->peerAddr[4];
            list_addr[i][3] = params->peerAddr[3];
            list_addr[i][2] = params->peerAddr[2];
            list_addr[i][1] = params->peerAddr[1];
            list_addr[i][0] = params->peerAddr[0];

            list_addr[i][6] = params->advertisingData[0];

            list_rssi[i] = params->rssi;

            if ( i==NUM_ROBOTS-1 ) {
                for(unsigned j=0; j<NUM_ROBOTS; j++) {
                    for(unsigned k=j ; k<NUM_ROBOTS ; k++) {
                        if(list_rssi[j] < list_rssi[k]) {
                            temp = list_rssi[j];
                            list_rssi[j] = list_rssi[k];
                            list_rssi[k] = temp;
                            for(unsigned l = 0; l < 6 ; l++) {
                                temp = list_addr[j][l];
                                list_addr[j][l] = list_addr[k][l];
                                list_addr[k][l] = temp;
                            }
                        }
                    }
                }
                break;
            }
        }

        if( i == NUM_ROBOTS-1 ) {
            if( i == 2 ) {
                ble.gap().stopScan();
                ble.gap().connect(list_addr[0], Gap::ADDR_TYPE_RANDOM_STATIC, NULL, NULL);
            }
        }
    }
    */
}

void serviceDiscoveryCallback(const DiscoveredService *service)
{
}

void characteristicDiscoveryCallback(const DiscoveredCharacteristic *characteristicP)
{
    if (characteristicP->getUUID() == 0xA001) { /* !ALERT! Alter this filter to suit your device. */
        characteristics_discovered        = *characteristicP;
        printf("characteristic discovered\r\t\n");
        characteristics_discovered.read();
    }
}

void connectionCallback(const Gap::ConnectionCallbackParams_t *params)
{
    printf("connected\r\n");
    ble.gap().stopAdvertising();
    ble.gap().stopScan();
    if (params->role == Gap::CENTRAL) {
        printf("CENTRAL\r\n");
        ble.gattClient().launchServiceDiscovery(params->handle, serviceDiscoveryCallback, characteristicDiscoveryCallback, 0xA000, 0xA001);
    } else {
        printf("PERIPHERAL\r\n");
    }
}

void triggerToggledWrite(const GattReadCallbackParams *response)
{
    if (response->handle == characteristics_discovered.getValueHandle()) {
        for(int i=0; i < NUM_ROBOTS; i++) {
            info_t[i] = info[i] || response->data[i];
            info[i] = info_t[i];
        }
        characteristics_discovered.write(NUM_ROBOTS, info);
    }
}

void onDataWriteCallback(const GattWriteCallbackParams *params)
{
    characteristics_discovered.read();
}

void onDataWrittenCallback(const GattWriteCallbackParams *params)
{
    for(int i=0; i<NUM_ROBOTS; i++) {
        info[i] = params->data[i];
    }
    printf("data is written well\r\n");
    
    ble.gap().disconnect(params->connHandle,Gap::REMOTE_USER_TERMINATED_CONNECTION);
}

void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
{
    printf("disconnected\r\n");
    
    for(int i=0; i<NUM_ROBOTS; i++) {
        printf("       %u", info[i]);
    }
    printf("\r\n");
    
    ble.gap().startAdvertising();
    ble.gap().startScan(advertisementCallback);
}


int main(void)
{
    Ticker ticker;
    ticker.attach(periodicCallback, 0.5);

    int num = 0;

    ble.init();
    ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_SCANNABLE_UNDIRECTED);
    ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
    ble.accumulateAdvertisingPayload(GapAdvertisingData::RABOT_REDBEAR_BLE_NANO, (uint8_t *)secure_code, sizeof(secure_code));
    ble.setAdvertisingType(GapAdvertisingParams::ADV_NON_CONNECTABLE_UNDIRECTED);
    ble.gap().setAdvertisingInterval(32);
    ble.gap().startAdvertising();
    
    ble.gap().setScanParams(1014 , 884);
    ble.gap().startScan(initialSetup);
    
    wait(5);
    
    ble.gap().stopAdvertising();
    ble.gap().stopScan();
    
    ble.gap().clearAdvertisingPayload();
        
    for(int i = 0; i< NUM_ROBOTS; i++) {
        num += initial_list_addr[i][6];
    }
    info[num] = 1;

    printf("initial setting is done\r\n");

    printf("initial info : ");
    for(int i=0; i<NUM_ROBOTS; i++) {
        printf("%u \t",info[i]);
    }
    printf("\r\n");
    

    //advertising for real experiment
    printf("communication for connection is started\r\n");

    ble.onConnection(connectionCallback);
    ble.onDisconnection(disconnectionCallback);

    ble.gattClient().onDataRead(triggerToggledWrite);
    ble.gattClient().onDataWrite(onDataWriteCallback);
    ble.gattServer().onDataWritten(onDataWrittenCallback);
    ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
    ble.accumulateAdvertisingPayload(GapAdvertisingData::RABOT_REDBEAR_BLE_NANO, (uint8_t *)secure_code, sizeof(secure_code));
    ble.gap().setAdvertisingInterval(29);

    ble.addService(customService);
    ble.gap().startAdvertising();
    ble.gap().setScanParams(190 /* scan interval */, 181 /* scan window */);
    ble.gap().startScan(advertisementCallback);

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