Ble Demo with Raspberry PI updated

Dependencies:   BLE_API mbed nRF51822

Fork of BLE_DEMO_SPI by HM_IOT_Demo

Files at this revision

API Documentation at this revision

Comitter:
hmiot
Date:
Sat Oct 14 15:00:30 2017 +0000
Parent:
18:0eefac249a4b
Child:
20:671b7e0ba5f3
Commit message:
updated

Changed in this revision

Light_Header/hm_config.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
--- a/Light_Header/hm_config.h	Fri Oct 13 07:25:54 2017 +0000
+++ b/Light_Header/hm_config.h	Sat Oct 14 15:00:30 2017 +0000
@@ -10,17 +10,31 @@
 #include "ble/Gap.h"
 
 #define MAX_SERVICES 4
+#define DEVICE_MAC_ADDR_1   0xE5 //Raspberry PI
+#define DEVICE_MAC_ADDR_2   0x51 //Magic Light1
+#define DEVICE_MAC_ADDR_3   0x5A //Magic Light2
+#define DEVICE_MAC_ADDR_4   0x4F //MiniBeacon_22824
+#define DEVICE_MAC_ADDR_5   0xE6 //MiniBeacon_22829
+#define MAX_DEVICES         5
+
+static const size_t MAC_ADDR_LEN = 6;
+
 //class Magic_Light{
-static bool connect_status = false;
+bool connect_status = false;
 bool charDiscover =  false;
-//uint8_t pre_write_data[16];
+
+uint8_t pre_write_data[16];
+int spiRX[8];
+uint8_t write_bufferSize;
+uint8_t write_data[16];
+
 static ble_error_t error_status;
-static bool serviceDiscover = true;
+bool serviceDiscover = true;
 bool first_on=true;
-//int spiRX[8];
-uint8_t bufferSize;
-//uint8_t write_data[16]; //[8] = {0x00,};//={0x56,0x00,0xff,0x00,0x00,0xf0,0xaa};
-
+int receiveBufSize = 0;
+bool read_flag = true;
+bool write_flag = false;
+bool disconnect_status = true;
 void AddCharNodes(DiscoveredCharacteristic light_Characteristic);
 void light_actions(int service_char[]);
 
@@ -42,16 +56,19 @@
     MAX_COLOR
 }light_color;
 
-/*
+
 uint8_t lightColor[MAX_COLOR][3] = {{0xFF,0x00,0x00},{0x00,0xFF,0x00},
                                     {0x00,0x00,0xFF},{0xFF,0xFF,0x00},
                                     {0x00,0xFF,0xFF},{0xFF,0x00,0xFF}
                                    };
-*/
+
+
+uint8_t Ble_address[MAX_DEVICES][MAC_ADDR_LEN];
+
 struct lightCharacteristic_t{
-    uint8_t u_characteristicID;
-    Gap::Handle_t connHandle;
-    GattAttribute::Handle_t valueHandle;
+    uint8_t DeviceID;
+    uint8_t Ble_address[MAC_ADDR_LEN];
+    DiscoveredCharacteristic DiscoverCharacteristics;
     //struct lightCharacteristic_t *nextChar;
 };
 
--- a/main.cpp	Fri Oct 13 07:25:54 2017 +0000
+++ b/main.cpp	Sat Oct 14 15:00:30 2017 +0000
@@ -2,7 +2,7 @@
 *                                                                       *
 *                                                                       *
 ************************************************************************/
-
+#include "mbed.h"
 #include "hm_config.h"
 
 DiscoveredCharacteristic lightCharacteristic;
@@ -12,51 +12,116 @@
 void bleint();
 void waitBleEvent();
 void ble_actions(int spi_data);
+
 uint8_t statusWR;
+uint8_t read_buffer[128];
+uint8_t nDiscoveredDevice = 0;
+bool scanFlag = false;
+bool first_connection = true;
+
 SPISlave spiSlave(P0_9, P0_11, P0_8, P0_10);
 lightCharacteristic_t ptr_temp_char;
 
 
 void read_Characteristics();
-
+void AddMacAddress(const uint8_t *u8MacAddress);
+void scanThread();
+void connectDevices(int device_type);
 //DiscoveredCharacteristic ptr_temp_char;
 
 bool connection_handle = 0;
 
-uint8_t Ble_address[6];
+void AddMacAddress(const uint8_t *u8MacAddress)
+{
+    printf("Add MAC Address::\r\n");
+    int i;
+    for(i=0; i< MAX_DEVICES; i++)
+    {
+        if(Ble_address[i][0] == '\0')
+        {
+            for(unsigned j=0; j< MAX_DEVICES; j++)
+            {
+                if(Ble_address[j][0] == u8MacAddress[0])
+                {
+                    printf("Already Added MAC Address \r\n");
+                    return;
+                }
+            }
+            memset(Ble_address[i],0,MAC_ADDR_LEN);
+            memcpy(Ble_address[i],u8MacAddress,MAC_ADDR_LEN);
+            nDiscoveredDevice++;
+            if(first_connection && Ble_address[i][0] == DEVICE_MAC_ADDR_1)
+            {   
+                first_connection = false;
+                connectDevices(DEVICE_MAC_ADDR_1);
+            }
+            printf("MAC Address \r\n");
+            for (unsigned index = 0; index < MAC_ADDR_LEN; index++) 
+            {
+                printf("%02x ", Ble_address[i][index]);
+            }
+            break;
+        }
+        else
+        {
+            printf("MAC Address index[%d] is not NULL \r\n", i);            
+        }
+    }
+    
+}
 
+void scanThread()
+{
+    printf("Scan Thread\r\n");
+    
+    while(scanFlag)
+    {
+        wait(15.0);
+        scanFlag = false;
+        BLE::Instance().gap().stopScan();
+    }
+    
+}
 
 void advertisementCallback(const Gap::AdvertisementCallbackParams_t *params) {
-    uint8_t con_status =0;
-    int i;
-     //0x51 for Magic light 
-    //0x5A
-    
-        /*pc.printf("adv peerAddr[%02x %02x %02x %02x %02x %02x] rssi %d, isScanResponse %u, AdvertisementType %u\r\ndata: %s\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);*/
-        //if (params->peerAddr[0] == 0x5A){ // 0x2F for red bear1.5 /* !ALERT! Alter this filter to suit your device. */
-        if (params->peerAddr[0] == 0xE5){ // 0x2F for red bear1.5 /* !ALERT! Alter this filter to suit your device. */
+
+        if (params->peerAddr[0] == DEVICE_MAC_ADDR_1 || params->peerAddr[0] == DEVICE_MAC_ADDR_2 || 
+            params->peerAddr[0] == DEVICE_MAC_ADDR_3 || params->peerAddr[0] == DEVICE_MAC_ADDR_4 ||
+            params->peerAddr[0] == DEVICE_MAC_ADDR_5)
+        { // 0x2F for red bear1.5 /* !ALERT! Alter this filter to suit your device. */
        
            pc.printf("adv peerAddr[%02x %02x %02x %02x %02x %02x] rssi %d, isScanResponse %u, AdvertisementType %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);
            pc.printf("Data Length : %d\r\n",params->advertisingDataLen);
-            for (i = 0; i < 6; i++)
-                Ble_address[i] = params->peerAddr[i];
-                
-           con_status = BLE::Instance().gap().connect(params->peerAddr, Gap::ADDR_TYPE_PUBLIC, NULL, NULL);
-           pc.printf("Connection Status : %d\r\n",con_status);
-           connect_status = 1;
-           BLE::Instance().gap().stopScan();
-          
+           AddMacAddress(params->peerAddr);
+         
         }
-        else{
+        else
+        {
             printf("Not Matched your device\r\n");
             return;
         }
  }
 
+void connectDevices(int device_type)
+{
+         pc.printf("Connection : %d\r\n", disconnect_status);
+            pc.printf("Connection device Type: %d\r\n", device_type);
+            uint8_t con_status =0;
+            for(unsigned index = 0; index < nDiscoveredDevice; index++ )
+            {
+                if(Ble_address[index][0] == device_type)
+                {
+                    disconnect_status = false;
+                    con_status = BLE::Instance().gap().connect(Ble_address[index], Gap::ADDR_TYPE_PUBLIC, NULL, NULL);
+                    pc.printf("Connection Status : %d\r\n",con_status);
+                    connect_status = 1;
+                    return;
+                }
+            }
+
+}
 void serviceDiscoveryCallback(const DiscoveredService *service) {
     pc.printf("Service Discovery Callback\r\n");
     if (service->getUUID().shortOrLong() == UUID::UUID_TYPE_SHORT) {
@@ -74,12 +139,12 @@
 void characteristicDiscoveryCallback(const DiscoveredCharacteristic *characteristicP) {
     pc.printf("  C UUID-%x valueAttr[%u] props[%x]\r\n", characteristicP->getUUID().getShortUUID(), characteristicP->getValueHandle(), (uint8_t)characteristicP->getProperties().broadcast());
     
-    if (characteristicP->getUUID().getShortUUID() == 0xEC0E) { /* !ALERT! Alter this filter to suit your device. */
+    if (characteristicP->getUUID().getShortUUID() == 0xEC0E || characteristicP->getUUID().getShortUUID() == 0xFFE9) { /* !ALERT! Alter this filter to suit your device. */
        lightCharacteristic = *characteristicP;
        pc.printf("Matched char UUID\r\n");
        printf("Conn Handle = %dand %d \r\n",characteristicP->getConnectionHandle(), lightCharacteristic.getConnectionHandle());
        printf("Value Handle = %d and %d\r\n",characteristicP->getValueHandle(),lightCharacteristic.getValueHandle());
-       AddCharNodes(lightCharacteristic);
+       //AddCharNodes(lightCharacteristic);
        charDiscover = 1;
        serviceDiscover = false;
      }else {
@@ -113,11 +178,12 @@
     }*/
             
     /* assign data to the node */
-    ptr_temp_char.u_characteristicID = 1;
+   /* ptr_temp_char.u_characteristicID = 1;
     pc.printf("Conn Handle = %d\r\n",light_Characteristic.getConnectionHandle());
     pc.printf("Value Handle = %d\r\n",light_Characteristic.getValueHandle());
     ptr_temp_char.connHandle = light_Characteristic.getConnectionHandle();
     ptr_temp_char.valueHandle = light_Characteristic.getValueHandle();
+    */
     //ptr_temp_char.nextChar = NULL;
 }
 
@@ -129,15 +195,27 @@
 
 void connectionCallback(const Gap::ConnectionCallbackParams_t *params) {
     pc.printf("Connection Callback\n\r");
-    if (params->role == Gap::CENTRAL) {
-        //connectionHandle = params->handle;
-        pc.printf("Handle %u\r\n", params->handle);
+    if (params->role == Gap::CENTRAL) 
+    {
+        pc.printf("Handle %u and peer addr [%02x]\r\n", params->handle,params->peerAddr[0]);
         pc.printf("Service and characterstics discovery callback\r\n");
         BLE::Instance().gattClient().onServiceDiscoveryTermination(discoveryTerminationCallback);
-        error_status = BLE::Instance().gattClient().launchServiceDiscovery(params->handle, serviceDiscoveryCallback, characteristicDiscoveryCallback, 0xEC00, 0xEC0E);
+        switch(params->peerAddr[0])
+        {
+            case DEVICE_MAC_ADDR_1: // 0xec00 --> Services UUID and 0xec0e --> Characteristics UUID
+                error_status = BLE::Instance().gattClient().launchServiceDiscovery(params->handle, serviceDiscoveryCallback, characteristicDiscoveryCallback, 0xEC00, 0xEC0E);
+                break;
+                    
+            case DEVICE_MAC_ADDR_2: // 0xffe5 --> Services UUID and 0xffe9 --> Characteristics UUID
+                error_status = BLE::Instance().gattClient().launchServiceDiscovery(params->handle, serviceDiscoveryCallback, characteristicDiscoveryCallback, 0xffe5, 0xffe9);
+                break;
+            default:
+                printf("************Unhandled Device Address *************\r\n");
+                break;
+        }
+        
         printf("Service Discovery Error Status =%d\n\r",error_status);
-        // 0xffe5 --> Services UUID
-        // 0xffe9 --> Characteristics UUID
+
     }
 }
 
@@ -146,32 +224,44 @@
     pc.printf("written callback\r\n");    
 }
 
+void pairDevice()
+{
+    if(read_flag)
+    {
+        printf("Read Pairdevice\r\n");
+        connectDevices(DEVICE_MAC_ADDR_1);
+    }
+    else if(write_flag)
+    {
+        printf("Write Pairdevice\r\n");
+        connectDevices(DEVICE_MAC_ADDR_2);
+    }else
+    {
+        printf("invalid Pairdevice status\r\n");
+    }
+}
 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params) {
     pc.printf("disconnected\r\n");
+    disconnect_status = true;
     //ble.gap().startScan(advertisementCallback);
     connect_status = 0;
     charDiscover = 0;
-    bleint();
+    serviceDiscover = true;
+    pairDevice();
+    //bleint();
 }
 
-
-void hvxCallback(const GattHVXCallbackParams *params) {
-    printf("received HVX callback for handle %u; type %s\r\n", params->handle, (params->type == BLE_HVX_NOTIFICATION) ? "notification" : "indication");
-    for (unsigned index = 0; index < params->len; index++) {
-        printf(" %02x", params->data[index]);
-    }
-    printf("\r\n");
-}
 void triggerRead(const GattReadCallbackParams *response)
 {
     printf("reading data : \r\n");
-   // if (response->handle == GattCharacteristic.getValueHandle()) 
-  //  if (response->handle == characteristicP->getValueHandle())
+    if (response->handle == lightCharacteristic.getValueHandle())
     {
         printf("triggerToggledWrite: handle %u, offset %u, len %u\r\n", response->handle, response->offset, response->len);
+        memcpy(read_buffer,response->data,response->len);
+        receiveBufSize = response->len;
         for (unsigned index = 0; index < response->len; index++) 
         {
-            printf("%c[%02x]", response->data[index], response->data[index]);
+            printf("%c", read_buffer[index]);
         }
         printf("\r\n");
     }
@@ -192,55 +282,30 @@
  
     ble.gap().onConnection(connectionCallback);
     ble.gap().onDisconnection(disconnectionCallback);
+    ble.gattClient().onDataRead(triggerRead);
+    
     ble.gap().setScanParams(1000, 800);
     ble.gap().startScan(advertisementCallback);
-    //ble.gattClient().onHVX(hvxCallback);
-    //ble.gattClient().onDataRead(triggerRead);
-
-}
-
-void write_Characteristics()
-{   uint8_t write_data  = 0x45;
     
-    error_status = BLE::Instance().gattClient().write(GattClient::GATT_OP_WRITE_REQ, ptr_temp_char.connHandle,ptr_temp_char.valueHandle, sizeof(write_data), &write_data);
+    scanFlag = true;
     
-    printf("Error Status =%d\n\r",error_status);
-    //int i;
-    printf("write VALUE : ");
-    //for(i=0; i<16;i++)
-    //{
-        printf("%d \r\n", write_data);
-    //}
-    BLE::Instance().processEvents();
 }
 
 void read_Characteristics()
 {   uint16_t write_data;
     
-    error_status = BLE::Instance().gattClient().read(ptr_temp_char.connHandle,ptr_temp_char.valueHandle, write_data);
+    error_status = BLE::Instance().gattClient().read(lightCharacteristic.getConnectionHandle(),lightCharacteristic.getValueHandle(), write_data);
     
     printf("Error Status =%d\n\r",error_status);
-    //int i;
+
     printf("READ VALUE : ");
-    //for(i=0; i<16;i++)
-    //{
-        printf("%x \r\n", write_data);
-    //}
+    printf("%x \r\n", write_data);
     BLE::Instance().processEvents();
 }
 void light_actions(int * service_char)
 {
+    uint8_t write_data[16];
 
-/*
-    uint8_t lightColor[MAX_COLOR][3] = {{0xFF,0x00,0x00},{0x00,0xFF,0x00},
-                                    {0x00,0x00,0xFF},{0xFF,0xFF,0x00},
-                                    {0x00,0xFF,0xFF},{0xFF,0x00,0xFF}
-                                   };
-    uint8_t pre_write_data[16];
-    uint8_t write_data[16];
-    
-    //lightCharacteristic_t *ptr_tmp = NULL;
-    //ptr_tmp = lightChar.services_Char[0];
     uint8_t numCount;
     if(connect_status)
     {
@@ -255,26 +320,24 @@
                 write_data[4]=0x00;
                 write_data[5]=0xf0;
                 write_data[6]=0xaa;
-                //error_status = BLE::Instance().gattClient().write(GattClient::GATT_OP_WRITE_CMD,ptr_tmp->connHandle,ptr_tmp->valueHandle,8,write_data);
-                error_status = BLE::Instance().gattClient().write(GattClient::GATT_OP_WRITE_CMD,connectionHandle,ptr_temp_char.valueHandle,7,write_data);
+                
+                error_status = BLE::Instance().gattClient().write(GattClient::GATT_OP_WRITE_CMD,lightCharacteristic.getConnectionHandle(),lightCharacteristic.getValueHandle(),7,write_data);
                 BLE::Instance().processEvents();
-                //error_status = BLE::Instance().gattClient().write(GattClient::GATT_OP_WRITE_CMD,ptr_temp_char.connHandle,ptr_temp_char.valueHandle,7,write_data);
-                 //BLE::Instance().updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), write_data, 7);
                 break;
                        
             case TURN_ON: //on
-                pre_write_data[0]=0x56;
-                pre_write_data[1]=0xDE;
-                pre_write_data[2]=0x8E;
-                pre_write_data[3]=0xEE;
-                pre_write_data[4]=0x00;
-                pre_write_data[5]=0xf0;
-                pre_write_data[6]=0xaa;
-                
-                //error_status = BLE::Instance().gattClient().write(GattClient::GATT_OP_WRITE_CMD,ptr_tmp->connHandle,ptr_tmp->valueHandle,8,pre_write_data);
-                error_status = BLE::Instance().gattClient().write(GattClient::GATT_OP_WRITE_CMD,connectionHandle,ptr_temp_char.valueHandle,7,pre_write_data);
+                if(first_on)
+                {
+                    pre_write_data[0]=0x56;
+                    pre_write_data[1]=0xDE;
+                    pre_write_data[2]=0x8E;
+                    pre_write_data[3]=0xEE;
+                    pre_write_data[4]=0x00;
+                    pre_write_data[5]=0xf0;
+                    pre_write_data[6]=0xaa;
+                }
+                error_status = BLE::Instance().gattClient().write(GattClient::GATT_OP_WRITE_CMD,lightCharacteristic.getConnectionHandle(),lightCharacteristic.getValueHandle(),7,pre_write_data);
                 BLE::Instance().processEvents();
-                //error_status = BLE::Instance().DiscoveredCharacteristic().write(7,pre_write_data);
                 break;
                        
             case COLOR: // Color
@@ -289,19 +352,16 @@
                   pre_write_data[numCount] = write_data[numCount];
                 }
                 first_on=false;
-                //error_status = BLE::Instance().gattClient().write(GattClient::GATT_OP_WRITE_CMD,ptr_tmp->connHandle,ptr_tmp->valueHandle,8,write_data);
-                error_status = BLE::Instance().gattClient().write(GattClient::GATT_OP_WRITE_CMD,connectionHandle,ptr_temp_char.valueHandle,7,write_data);
+                error_status = BLE::Instance().gattClient().write(GattClient::GATT_OP_WRITE_CMD,lightCharacteristic.getConnectionHandle(),lightCharacteristic.getValueHandle(),7,write_data);
                 BLE::Instance().processEvents();
-                //error_status = BLE::Instance().gattClient().write(GattClient::GATT_OP_WRITE_CMD,ptr_temp_char.connHandle,ptr_temp_char.valueHandle,7,write_data);
-                
                 break;
                        
             default:
                 printf("Invalid Options\r\n");
         } 
-            printf("Error Status =%d\n\r",error_status);
+            printf(" light Error Status =%d\n\r",error_status);
     }
-*/
+
 }
 
 void datancpy(char pre_write[], char writeData[], int data_size){
@@ -317,6 +377,35 @@
     printf("Init\r\n");
     BLE &ble = BLE::Instance();
     BLE::Instance().init(bleInitComplete);
+    
+}
+
+bool checkBytes(int byteValue)
+{    
+    
+    bool status = false;
+    switch(byteValue)
+    {
+        case 0xAD:
+            read_flag = false;
+            write_flag = true;
+            BLE::Instance().gap().disconnect(lightCharacteristic.getConnectionHandle(),Gap::REMOTE_USER_TERMINATED_CONNECTION);
+            status = true;
+            break;
+        
+        case 0xAF:
+            read_flag = true;
+            write_flag = false;
+            BLE::Instance().gap().disconnect(lightCharacteristic.getConnectionHandle(),Gap::REMOTE_USER_TERMINATED_CONNECTION);
+            status = true;
+            break;
+            
+        default:
+            status = false;
+            break;
+    }
+    
+    return status;
 }
 
 int main()
@@ -325,99 +414,98 @@
     pc.baud(9600);
     wait(8.0);
     pc.printf("Start\n\r");
+    statusWR=1;
 
-    //spiSlave.reply(191);
-    statusWR=1;
+    int read_temp_data;
+    int write_temp_data;
+    uint8_t read_bufferSize = 0;
+    
     bleint();
-    bufferSize = 0;
-    /*int spiRX[5];
     
-    int temp = 0x00;
-    int color = 0x00;
-    int con_status,i;*/
-    while(1){
+    spiSlave.reply(12);
+
+    while(1)
+    {
         pc.printf("loop\r\n");
-        //wait(1.0);
         printf( "Connect Status %d  and Char Discover %d \r\n", connect_status, charDiscover);
+        
+        scanThread();
+        
         if(!(connect_status && charDiscover))
-        {
+        {   
             printf( " Wait Event Connect Status %d  and Char Discover %d \r\n", connect_status, charDiscover);
             waitBleEvent();
+            
         }
         printf("Readchar\r\n");
+      
         if(connect_status && charDiscover)
         {
-            pc.printf("Read\r\n");
-            
-            write_Characteristics();
-            wait(2.0);
-            read_Characteristics();
-        }
-           
-        /* 
-        if (temp == 2 && color == 3) {
-            temp = 0x00;
-            color = 0x00;
-        }
-        else if (temp == 2 && color < 3) {
-            color++;
-        }
-        else if (temp < 2)
-            temp++;
-            
-         spiRX[0] = 0xB3;
-         spiRX[1] = 1; 
-         spiRX[2] = temp;
-         spiRX[3] = color;
-         pc.printf("spix buffer is: ");
-        for (i = 0; i < 4; i++) {
-                pc.printf("%d ", spiRX[i]);
-        }
-        pc.printf("\n");
-
-        //if(spiSlave.receive()){
-             //printf("Slave Read loop1\r\n");
-            //spiRX[bufferSize] = spiSlave.read();
-            //spiSlave.reply(spiRX[bufferSize]);
-            //bufferSize++;
-            bufferSize = 5;
-            
-            if(bufferSize >= 5){
-                bufferSize=0;
-                light_actions(spiRX);
-                //BLE::Instance().gattClient().reset();
-                spiSlave.reply(spiRX[2]); //(0xAA);
-               
+            if(read_flag)
+            {
+                pc.printf("Read\r\n");
+                read_Characteristics();
+                read_bufferSize = 0;
+                if(spiSlave.receive())
+                {   
+                    read_temp_data = spiSlave.read();
+                                            
+                    while(read_bufferSize < receiveBufSize) 
+                    {   
+                        if(!checkBytes(read_temp_data))
+                            break;
+                            
+                        spiSlave.reply(read_buffer[read_bufferSize]);
+                        read_bufferSize++;
+                        read_temp_data = spiSlave.read();
+                    }
                 
-                if(error_status > 7)
+                    if(read_bufferSize >= receiveBufSize)
+                    {
+                        read_bufferSize =0;
+                        spiSlave.reply(0xFE);// Send 0xFE to Master to indicate stop polling the data.
+                    }        
+                }
+            }
+            if(write_flag && connect_status && charDiscover)
+            {
+                if(spiSlave.receive())
                 {
-                    error_status = BLE_ERROR_NONE;
-                    //wait_ms(500);
-                    BLE::Instance().shutdown();
-                    statusWR =1;
-                    connect_status =0;
-                    charDiscover =0;
-                    serviceDiscover = true;
-                    bleint();
+                    spiRX[write_bufferSize] = spiSlave.read();
+                    write_temp_data = spiRX[write_bufferSize];
+                    write_bufferSize = 0;
+                    while(write_flag)
+                    {
+                        if(!checkBytes(write_temp_data))
+                            break;
+                            
+                        spiSlave.reply(spiRX[write_bufferSize]);
+                        write_bufferSize++;
+                        spiRX[write_bufferSize] = spiSlave.read();
+                        write_temp_data = spiRX[write_bufferSize];
+                        
+                        if(write_bufferSize >= 5 )
+                        {
+                            light_actions(spiRX);
+                            spiSlave.reply(0xFE); // Send 0xFE to Master to indicate stop polling the data.
+                              
+                        }
+                    }
                 }
-                //statusWR++;
-                 wait_ms(3000);
             }
-            
-        //}  */   
-        
+        }  
     }
 
 }
 
 void waitBleEvent()
 {
-    //|| BLE::Instance().gattClient().isServiceDiscoveryActive()
-    while(serviceDiscover ){
-                    printf("Service Discover =%d\r\n",serviceDiscover);
-                    printf(" Service Status = %d \r\n",BLE::Instance().gattClient().isServiceDiscoveryActive());
-                    BLE::Instance().waitForEvent();
-                    printf( "waitBleEvent loop\r\n");
+    while(serviceDiscover )
+    {
+        printf("Service Discover =%d\r\n",serviceDiscover);
+        printf(" Service Status = %d \r\n",BLE::Instance().gattClient().isServiceDiscoveryActive());
+        BLE::Instance().waitForEvent();
+        printf( "waitBleEvent loop\r\n");
     }
     printf( "waitBleEvent End\r\n"); 
 }