Diff: main.cpp

Revision:

1:c227f3064399

Parent:

0:482d3fcc94f4

Child:

3:3f4aa38b0fb7

--- a/main.cpp	Tue Aug 18 07:06:07 2015 +0000
+++ b/main.cpp	Thu Oct 01 17:08:49 2015 +0000
@@ -18,100 +18,324 @@
 #include "ble/BLE.h"
 #include "ble/DiscoveredCharacteristic.h"
 #include "ble/DiscoveredService.h"
+
 BLE        ble;
-DigitalOut led1(LED1);
-Serial pc(USBTX, USBRX);
+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)
 {
-    led1 = !led1; /* Do blinky on LED1 while we're waiting for BLE events */
+    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 advertisementCallback(const Gap::AdvertisementCallbackParams_t *params) {
-     if (params->peerAddr[0] == 0xcb) { /* !ALERT! Alter this filter to suit your device. */
-        return;
-    }
-    pc.printf("Adv peerAddr: [%02x %02x %02x %02x %02x %02x] rssi %d, ScanResp: %u, AdvType: %u\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);
+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];
 
-    for (unsigned index = 0; index < params->advertisingDataLen; index++) {
-        pc.printf("%02x ", params->advertisingData[index]);
+                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]);
     }
-    pc.printf("\r\n");
- /* DUMP_ADV_DATA */
-/*
-     ble.gap().connect(params->peerAddr, Gap::ADDR_TYPE_RANDOM_STATIC, NULL, NULL);
-*/
 }
 
-void serviceDiscoveryCallback(const DiscoveredService *service) {
-    if (service->getUUID().shortOrLong() == UUID::UUID_TYPE_SHORT) {
-        pc.printf("S UUID-%x attrs[%u %u]\r\n", service->getUUID().getShortUUID(), service->getStartHandle(), service->getEndHandle());
-    } else {
-        printf("S UUID-");
-        const uint8_t *longUUIDBytes = service->getUUID().getBaseUUID();
-        for (unsigned i = 0; i < UUID::LENGTH_OF_LONG_UUID; i++) {
-            printf("%02x", longUUIDBytes[i]);
+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;
         }
-        pc.printf(" attrs[%u %u]\r\n", service->getStartHandle(), service->getEndHandle());
+        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 characteristicDiscoveryCallback(const DiscoveredCharacteristic *characteristicP) {
-  
-}
- 
-void discoveryTerminationCallback(Gap::Handle_t connectionHandle) {
-    pc.printf("terminated SD for handle %u\r\n", connectionHandle);
+
+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 connectionCallback(const Gap::ConnectionCallbackParams_t *params) {
-    pc.printf("Connected!!\r\n");
-    ble.stopAdvertising();
-    wait(1);
-    if (params->role == Gap::CENTRAL) {
-        ble.gattClient().onServiceDiscoveryTermination(discoveryTerminationCallback);
-        ble.gattClient().launchServiceDiscovery(params->handle, serviceDiscoveryCallback, characteristicDiscoveryCallback, 0xa000, 0xa001);
+
+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);
     }
-    ble.gap().disconnect(0,Gap::REMOTE_USER_TERMINATED_CONNECTION);
+}
+
+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)
 {
-    pc.printf("Disconnected\r\n");
-    pc.printf("Rescan\r\n");
-    ble.startScan(advertisementCallback);
-    ble.startAdvertising();
+    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)
 {
-    led1 = 1;
     Ticker ticker;
-    ticker.attach(periodicCallback, 1);
+    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.init();
-    
-    pc.baud(9600);
-    pc.printf("Observer Init \r\n");
-
-
-    /* */
-     ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
+    ble.gattClient().onDataRead(triggerToggledWrite);
+    ble.gattClient().onDataWrite(onDataWriteCallback);
+    ble.gattServer().onDataWritten(onDataWrittenCallback);
     ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
-     ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
-                                    (const uint8_t *)"DDUDDU", sizeof("DDUDDU") - 1);
-   
-    ble.gap().setAdvertisingInterval(100); /* 1second. */
+    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();
     }
-}
+}
\ No newline at end of file