Yasushi TAUCHI / Mbed 2 deprecated BLE_HVC

オムロンのOKAO Vision HVC-C1BとBLE通信します。

Dependencies:   BLE_API mbed nRF51822

Fork of BLE_ButtonSense by Bluetooth Low Energy

オムロン　OKAO　Vision(ヒューマンビジョンコンポ）HVC-C1BとRedBearLab nRF51822 mbedでBLE通信を行うサンプルです。

HVC-C1Bは、人、手、顔、性別、年齢、表情、顔の向き、視線、目の閉じを推定し、値を返してくれます。 http://plus-sensing.omron.co.jp/egg-project/

main.cppの最初の定義の部分（IDや欲しい項目）を変更すると使えます。 欲しい項目を1に要らない項目は０にしてください。

タイムアウトの処理が入っていませんが、もし入れたい場合は、LEDのHartBeatの処理に追加すれば簡単に出来ると思います。

下記のような感じでメッセージが帰ってきます。

#Look at HVC-C1B peerAddr[d5 32 26 3b 47 3d] rssi -76, isScanResponse 0, AdvertisementType 0
# St#HVC-C1B Connect
#SCAN Stop
ate 1 Wait for Caracteristic Found
#short:
#  C UUID-2a00 valueAttr[3] props[0]
#short:
#  C UUID-2a01 valueAttr[5] props[0]
#short:
#  C UUID-2a04 valueAttr[7] props[0]
#short:
#  C UUID-2a05 valueAttr[10] props[0]
#long:
#  C UUID-35100003d13a4f398ab3bf64d4fbb4b4 valueAttr[14] props[0]
#HVCNotifyCharacteristic Found
#long:
#  C UUID-35100002d13a4f398ab3bf64d4fbb4b4 valueAttr[17] props[0]
#HVCWriteCharacteristic Found
#long:
#  C UUID-35100004d13a4f398ab3bf64d4fbb4b4 valueAttr[19] props[0]
#HVCDeviceCharacteristic Found
# discovery terminated CallBack 0
# State 2 Notify Enable
#send data 1
#State=2 Wited
# State3 kidoukakunin
#send data 1
#received HVX callback for handle 14; type notification
#received HVX callback for handle 14; type notification
# State3 HVC-C1B      1 1 0 96090000
# State4 DataSending
#send data 1
#received HVX callback for handle 14; type notification
#received HVX callback for handle 14; type notification
#received HVX callback for handle 14; type notification
HumanDetect:1
Human(0):x=199,y=251,size=330,depend=774
HandDetect:0
FaceDetect:1
Face(0):x=227,y=230,size=158,depend=670
FaceDirection(0):holizon=20,vertical=0,angle=15,depend=120
FaceOld(0):old=64,depend=333
FaceSex(0):Sex=0,depend=111
FaceLook(0):holizon=-2,vertical=-2
FaceEyeClose(0):xleft=229,right=326
FaceExpression(0):TopExpression=5,TopScore=46,look=-74
# State4 DataSending
#send data 1

Diff: main.cpp

Revision:
2:b94ca9b44b80
Parent:
1:e4ee755844de
--- a/main.cpp	Sun Jul 19 13:20:06 2015 +0000
+++ b/main.cpp	Wed Jul 22 09:45:56 2015 +0000
@@ -22,6 +22,19 @@
 #include "ble_gatt.h"
 
 char HVC_ADDRESS[]= {0xd5,0x32,0x26,0x3b,0x47,0x3d};
+bool Reconnect=true;
+char HumanDetect=1;
+char HandDetect=1;
+char FaceDetect=1;
+char FaceDirection=1;
+char FaceOld=1;
+char FaceSex=1;
+char FaceLook=1;
+char FaceEyeClose=1;
+char FaceExpression=1;
+const uint8_t WriteCharacteristicAddr[]= {0x35,0x10,0x00,0x02,0xd1,0x3a,0x4f,0x39,0x8a,0xb3,0xbf,0x64,0xd4,0xfb,0xb4,0xb4};
+const uint8_t NotifyCharacteristicAddr[]= {0x35,0x10,0x00,0x03,0xd1,0x3a,0x4f,0x39,0x8a,0xb3,0xbf,0x64,0xd4,0xfb,0xb4,0xb4};
+const uint8_t DeviceCharacteristicAddr[]= {0x35,0x10,0x00,0x04,0xd1,0x3a,0x4f,0x39,0x8a,0xb3,0xbf,0x64,0xd4,0xfb,0xb4,0xb4};
 BLE ble;
 Serial pc(USBTX, USBRX);
 
@@ -31,6 +44,7 @@
 bool foundHVCWriteCharacteristic = false;
 bool foundHVCNotifyCharacteristic = false;
 bool foundHVCDeviceCharacteristic = false;
+int state=-1;
 bool WaitSend;
 DiscoveredCharacteristic HVCWriteCharacteristic;
 DiscoveredCharacteristic HVCNotifyCharacteristic;
@@ -48,56 +62,103 @@
             (params->peerAddr[3] != HVC_ADDRESS[2]) ||
             (params->peerAddr[2] != HVC_ADDRESS[3]) ||
             (params->peerAddr[1] != HVC_ADDRESS[4]) ||
-            (params->peerAddr[0] != HVC_ADDRESS[5])) { /* !ALERT! Update this filter to suit your device. */
+            (params->peerAddr[0] != HVC_ADDRESS[5])) {
         //  printf("Not 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);
         return;
     }
-/*
-    printf("#adv peerAddr[%02x %02x %02x %02x %02x %02x] rssi %d, isScanResponse %u, AdvertisementType %u\r\n",
+    //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);
+    printf("#Look at HVC-C1B 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);*/
-    ble.gap().connect(params->peerAddr, Gap::ADDR_TYPE_RANDOM_STATIC, NULL, NULL);
+           params->rssi, params->isScanResponse, params->type);
+    ble.gap().connect(params->peerAddr, Gap::ADDR_TYPE_RANDOM_STATIC, NULL, NULL);  //Connection
 }
 
 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());*/
+        //       pc.printf("#S UUID-%x attrs[%u %u]\r\n", service->getUUID().getShortUUID(), service->getStartHandle(), service->getEndHandle());
     } else {
-/*        pc.printf("#S UUID-");*/
-        const uint8_t *longUUIDBytes = service->getUUID().getBaseUUID();
-        for (unsigned i = 0; i < UUID::LENGTH_OF_LONG_UUID; i++) {
-/*            pc.printf("%02x", longUUIDBytes[i]);*/
-        }
-/*        pc.printf(" attrs[%u %u]\r\n", service->getStartHandle(), service->getEndHandle());*/
+        //        pc.printf("#S UUID-");
+        //        const uint8_t *longUUIDBytes = service->getUUID().getBaseUUID();
+        //        for (unsigned i = 0; i < UUID::LENGTH_OF_LONG_UUID; i++) {
+        //            pc.printf("%02x", longUUIDBytes[i]);
+        //        }
+        //        pc.printf(" attrs[%u %u]\r\n", service->getStartHandle(), service->getEndHandle());
     }
 }
 
 void characteristicDiscoveryCallback(const DiscoveredCharacteristic *characteristicP)
 {
-/*    pc.printf("#  C UUID-%x valueAttr[%u] props[%x]\r\n", characteristicP->getShortUUID(), characteristicP->getValueHandle(), (uint8_t)characteristicP->getProperties().broadcast());*/
-    if (characteristicP->getShortUUID() == 2) { /* !ALERT! Update this filter to suit your device. */
-        HVCWriteCharacteristic      = *characteristicP;
-        foundHVCWriteCharacteristic = true;
-/*        printf("#HVCWriteCharacteristic Found\r\n");*/
-    }
-    if (characteristicP->getShortUUID() == 3) { /* !ALERT! Update this filter to suit your device. */
-        HVCNotifyCharacteristic      = *characteristicP;
-        foundHVCNotifyCharacteristic = true;
-/*        printf("#HVCNotifyCharacteristic Found\r\n");*/
-    }
-    if (characteristicP->getShortUUID() == 4) { /* !ALERT! Update this filter to suit your device. */
-        HVCDeviceCharacteristic      = *characteristicP;
-        foundHVCDeviceCharacteristic = true;
-/*        printf("#HVCDeviceCharacteristic Found\r\n");*/
+    bool flag;
+    if (characteristicP->getUUID().shortOrLong() == UUID::UUID_TYPE_SHORT) {
+        printf("#short:\r\n");
+        //dousasuruka wakaranai
+        pc.printf("#  C UUID-%x valueAttr[%u] props[%x]\r\n", characteristicP->getUUID().getShortUUID() , characteristicP->getValueHandle(), (uint8_t)characteristicP->getProperties().broadcast());
+        //  if (characteristicP->getShortUUID() == 2) {
+        if (characteristicP->getUUID().getShortUUID() == 2) {
+            HVCWriteCharacteristic      = *characteristicP;
+            foundHVCWriteCharacteristic = true;
+            printf("#HVCWriteCharacteristic Found\r\n");
+        }
+        //if (characteristicP->getShortUUID() == 3) {
+        if (characteristicP->getUUID().getShortUUID() == 3) {
+            HVCNotifyCharacteristic      = *characteristicP;
+            foundHVCNotifyCharacteristic = true;
+            printf("#HVCNotifyCharacteristic Found\r\n");
+        }
+        //if (characteristicP->getShortUUID() == 4) {
+        if (characteristicP->getUUID().getShortUUID() == 4) {
+            HVCDeviceCharacteristic      = *characteristicP;
+            foundHVCDeviceCharacteristic = true;
+            printf("#HVCDeviceCharacteristic Found\r\n");
+        }
+    } else {
+        printf("#long:\r\n");
+        const uint8_t *longUUIDBytes = characteristicP->getUUID().getBaseUUID();
+        pc.printf("#  C UUID-");
+        for (unsigned i = 0; i < UUID::LENGTH_OF_LONG_UUID; i++) {
+            printf("%02x", longUUIDBytes[i]);
+        }
+        printf(" valueAttr[%u] props[%x]\r\n",  characteristicP->getValueHandle(), (uint8_t)characteristicP->getProperties().broadcast());
+        flag=true;
+        for (unsigned i = 0; i < UUID::LENGTH_OF_LONG_UUID; i++) {
+            if( longUUIDBytes[i]!=WriteCharacteristicAddr[i])flag=false;
+        }
+        if (flag==true) {
+            HVCWriteCharacteristic      = *characteristicP;
+            foundHVCWriteCharacteristic = true;
+            printf("#HVCWriteCharacteristic Found\r\n");
+        } else {
+            flag=true;
+            for (unsigned i = 0; i < UUID::LENGTH_OF_LONG_UUID; i++) {
+                if( longUUIDBytes[i]!=NotifyCharacteristicAddr[i])flag=false;
+            }
+            if (flag==true) {
+                HVCNotifyCharacteristic      = *characteristicP;
+                foundHVCNotifyCharacteristic = true;
+                printf("#HVCNotifyCharacteristic Found\r\n");
+            } else {
+                flag=true;
+                for (unsigned i = 0; i < UUID::LENGTH_OF_LONG_UUID; i++) {
+                    if( longUUIDBytes[i]!=DeviceCharacteristicAddr[i])flag=false;
+                }
+                if (flag==true) {
+                    HVCDeviceCharacteristic      = *characteristicP;
+                    foundHVCDeviceCharacteristic = true;
+                    printf("#HVCDeviceCharacteristic Found\r\n");
+                }
+            }
+        }
     }
 }
 
 void discoveryTerminationCallback(Gap::Handle_t connectionHandle)
 {
-/*    pc.printf("# discovery terminated CallBack %u\r\n", connectionHandle);*/
+    pc.printf("# discovery terminated CallBack %u\r\n", connectionHandle);
 }
 
 void connectionCallback(const Gap::ConnectionCallbackParams_t *params)
@@ -107,15 +168,19 @@
         ble.gattClient().onServiceDiscoveryTermination(discoveryTerminationCallback);
         ble.gattClient().launchServiceDiscovery(params->handle, serviceDiscoveryCallback, characteristicDiscoveryCallback/*, 0xa000, 0xa001*/);
         ble.gap().stopScan();
-/*        pc.printf("#SCAN Stop\r\n");*/
+        pc.printf("#HVC-C1B Connect\r\n");
+        pc.printf("#SCAN Stop\r\n");
     }
 }
 
 void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
 {
-/*    pc.printf("#Disconnected\r\n");*/
-    // ble.gap().startScan(advertisementCallback);
-    // pc.printf("#RE SCAN Start\r\n");
+    pc.printf("#HVC-C1B Disconnected\r\n");
+    if(Reconnect==true) {
+        state=-1;
+        ble.gap().startScan(advertisementCallback);
+        pc.printf("#RE SCAN Start\r\n");
+    }
 }
 
 void hvxCallback(const GattHVXCallbackParams *params)
@@ -123,60 +188,178 @@
     static bool flag=false; //flag=true no toki tudukiga aru
     static int l;
     static unsigned char buffer[100];
-    int k,human_no,hand_no,face_no;
-    int a;
-/*    pc.printf("#received HVX callback for handle %u; type %s\r\n", params->handle, (params->type == BLE_HVX_NOTIFICATION) ? "notification" : "indication");*/
+    int k,j,j1,j2,human_no,hand_no,face_no;
+    pc.printf("#received HVX callback for handle %u; type %s\r\n", params->handle, (params->type == BLE_HVX_NOTIFICATION) ? "notification" : "indication");
+
     if(flag==false)l=0;
+
     for (unsigned index = 0; index < params->len; index++) {
-/*        pc.printf("# %02x", params->data[index]);*/
-        buffer[l]=params->data[l];
+        buffer[l]=params->data[index];
         l++;
     }
-/*    pc.printf("\r\n");*/
     if((buffer[0]==0xfe)||(buffer[1]==0x00)) {
-        k=buffer[2]+buffer[3]*0x100+buffer[4]*0x10000+buffer[5]*0x1000000;
-        human_no=buffer[6];
-        hand_no=buffer[7];
-        face_no=buffer[8];
-        if(l>2+4+4+8*human_no+8*hand_no+(8+3)*face_no-2)flag=false;
-        else flag=true;
-        if(flag==false) {
-/*            printf("# humanNo=%d HandNo=%d FaceNo=%d\r\n",human_no,hand_no,face_no);*/
-            if(face_no>0) {
-                a=buffer[9+human_no*8+hand_no*8+9];
-                printf("%d\r\n",a);
+        //Sucess
+        if(state==3) {
+            if((l+1)>(6+0x13-1)) {
+                printf("# State3 %c%c%c%c%c%c%c%c%c%c%c%c %x %x %x %x\r\n",
+                       buffer[6],buffer[7],buffer[8],buffer[9],buffer[10],buffer[11],
+                       buffer[12],buffer[13],buffer[14],buffer[15],buffer[16],buffer[17],
+                       buffer[18],buffer[19],buffer[20],
+                       buffer[21]*0x1000000+buffer[22]*0x10000+buffer[23]*0x100+buffer[24]);
+                WaitSend=true;
+                flag=false;
+            } else {
+                flag=true;
+            }
+        } else if(state==4) {
+            k=buffer[2]+buffer[3]*0x100+buffer[4]*0x10000+buffer[5]*0x1000000;
+            if((l+1)>(6+k-1)) {
+                human_no=buffer[6];
+                hand_no=buffer[7];
+                face_no=buffer[8];
+
+                if(human_no>0) {
+                    printf("HumanDetect:%d \r\n",human_no);
+                    for(k=0; k<human_no; k++) {
+                        printf("Human(%d):x=%d,y=%d,size=%d,depend=%d\r\n",
+                               k,buffer[10+k*8+0]+buffer[10+k*8+1]*0x100,
+                               buffer[10+k*8+2]+buffer[10+k*8+3]*0x100,
+                               buffer[10+k*8+4]+buffer[10+k*8+5]*0x100,
+                               buffer[10+k*8+6]+buffer[10+k*8+7]*0x100);
+                    }
+                } else {
+                    if(HumanDetect==1) {
+                        printf("HumanDetect:0\r\n");
+                    }
+                }
+                if(hand_no>0) {
+                    printf("HandDetect:%d \r\n",hand_no);
+                    j=10+human_no*8;
+                    for(k=0; k<hand_no; k++) {
+                        printf("Hand(%d):x=%d,y=%d,size=%d,depend=%d\r\n",
+                               k,buffer[j+k*8+0]+buffer[j+k*8+1]*0x100,
+                               buffer[j+k*8+2]+buffer[j+k*8+3]*0x100,
+                               buffer[j+k*8+4]+buffer[j+k*8+5]*0x100,
+                               buffer[j+k*8+6]+buffer[j+k*8+7]*0x100);
+                    }
+                } else {
+                    if(HandDetect==1) {
+                        printf("HandDetect:0\r\n");
+                    }
+                }
+                if(face_no>0) {
+                    printf("FaceDetect:%d \r\n",face_no);
+                    j=10+human_no*8+hand_no*8;
+                    j2=FaceDetect*8+FaceDirection*8+FaceOld*3+ FaceSex*3+ FaceLook*2+FaceEyeClose*4+FaceExpression*3;
+                    if(FaceDetect==1) {
+                        j1=0;
+                        for(k=0; k<face_no; k++) {
+                            printf("Face(%d):x=%d,y=%d,size=%d,depend=%d\r\n",
+                                   k,buffer[j+k*j2+j1+0]+buffer[j+k*j2+j1+1]*0x100,
+                                   buffer[j+k*j2+j1+2]+buffer[j+k*j2+j1+3]*0x100,
+                                   buffer[j+k*j2+j1+4]+buffer[j+k*j2+j1+5]*0x100,
+                                   buffer[j+k*j2+j1+6]+buffer[j+k*j2+j1+7]*0x100);
+                        }
+                    }
+                    if(FaceDirection==1) {
+                        j1=FaceDetect*8;
+                        for(k=0; k<face_no; k++) {
+                            printf("FaceDirection(%d):holizon=%d,vertical=%d,angle=%d,depend=%d\r\n",
+                                   k,(signed short)(buffer[j+k*j2+j1+0]+buffer[j+k*j2+j1+1]*0x100),
+                                   (signed short)(buffer[j+k*j2+j1+2]+buffer[j+k*j2+j1+3]*0x100),
+                                   (signed short)(buffer[j+k*j2+j1+4]+buffer[j+k*j2+j1+5]*0x100),
+                                   (signed short)(buffer[j+k*j2+j1+6]+buffer[j+k*j2+j1+7]*0x100));
+                        }
+                    }
+                    if(FaceOld==1) {
+                        j1=FaceDetect*8+FaceDirection*8;
+                        for(k=0; k<face_no; k++) {
+                            printf("FaceOld(%d):old=%d,depend=%d\r\n",
+                                   k,(signed char)(buffer[j+k*j2+j1+0]),
+                                   (short)(buffer[j+k*j2+j1+1]+buffer[j+k*j2+j1+2]*0x100));
+                        }
+                    }
+                    if(FaceSex==1) {
+                        j1=FaceDetect*8+FaceDirection*8+FaceOld*3;
+                        for(k=0; k<face_no; k++) {
+                            printf("FaceSex(%d):Sex=%d,depend=%d\r\n",
+                                   k,(signed char)(buffer[j+k*j2+j1+0]),  //0--feminine 1---male
+                                   (short)(buffer[j+k*j2+j1+1]+buffer[j+k*j2+j1+2]*0x100));
+                        }
+                    }
+                    if(FaceLook==1) {
+                        j1=FaceDetect*8+FaceDirection*8+FaceOld*3+FaceSex*3;
+                        for(k=0; k<face_no; k++) {
+                            printf("FaceLook(%d):holizon=%d,vertical=%d\r\n",
+                                   k,(signed char)(buffer[j+k*j2+j1+0]),  //0--feminine 1---male
+                                   (signed char)(buffer[j+k*j2+j1+1]));
+                        }
+                    }
+                    if(FaceEyeClose==1) {
+                        j1=FaceDetect*8+FaceDirection*8+FaceOld*3+FaceSex*3+FaceLook*2;
+                        for(k=0; k<face_no; k++) {
+                            printf("FaceEyeClose(%d):xleft=%d,right=%d\r\n",
+                                   k,(signed short)(buffer[j+k*j2+j1+0]+buffer[j+k*j2+j1+1]*0x100),
+                                   (signed short)(buffer[j+k*j2+j1+2]+buffer[j+k*j2+j1+3]*0x100));
+                        }
+                    }
+                    if(FaceExpression==1) {
+                        j1=FaceDetect*8+FaceDirection*8+FaceOld*3+FaceSex*3+FaceLook*2+FaceEyeClose*4;
+                        for(k=0; k<face_no; k++) {
+                            printf("FaceExpression(%d):TopExpression=%d,TopScore=%d,look=%d\r\n",
+                                   k,(signed char)(buffer[j+k*j2+j1+0]),
+                                   (signed char)(buffer[j+k*j2+j1+1]),
+                                   (signed char)(buffer[j+k*j2+j1+2]));
+                        }
+                    }
+
+                } else {
+                    if(FaceDetect==1)        printf("FaceDetect:0\r\n");
+                    if(FaceDirection==1)     printf("FaceDirection:0\r\n");
+                    if(FaceOld==1)           printf("FaceOld:0\r\n");
+                    if(FaceSex==1)           printf("FaceSex:0\r\n");
+                    if(FaceLook==1)          printf("FaceLook:0\r\n");
+                    if(FaceEyeClose==1)      printf("FaceEyeClose:0\r\n");
+                    if(FaceExpression==1)    printf("FaceExpression:0\r\n");
+                }
+                flag=false;
+                WaitSend=true;
+            } else {
+                flag=true;
             }
         }
+    } else {
+        flag=false;
     }
-    WaitSend=true;
 }
+
 void DataReadCallback(const GattReadCallbackParams *params)
 {
-/*    pc.printf("#recive dataread \r\n");*/
-    for (unsigned index = 0; index < params->len; index++) {
-/*        pc.printf("# %02x", params->data[index]);*/
-    }
-/*    pc.printf("\r\n");*/
+    //    pc.printf("#Data Readed \r\n");
+    //for (unsigned index = 0; index < params->len; index++) {
+    //        pc.printf("# %02x", params->data[index]);
+    //}
+    //    pc.printf("\r\n");
 }
 
-
 void DataWriteCallback(const GattWriteCallbackParams *params)
 {
-  //  pc.printf("#send data %x\r\n",params->writeOp);
-  //  for (unsigned index = 0; index < params->len; index++) {
-  //      pc.printf(" %02x", params->data[index]);
-  //  }
-  //  pc.printf("\r\n");
-    //  ble.gattClient().read(connectionHandle, HVCDeviceCharacteristic.getValueHandle(), 0);
-//WaitSend=true;
-//HVCNotifyCharacteristic.read(0);
+    pc.printf("#send data %x\r\n",params->writeOp);
+    //  for (unsigned index = 0; index < params->len; index++) {
+    //      pc.printf(" %02x", params->data[index]);
+    //  }
+    //  pc.printf("\r\n");
+    if(state==2) {
+        printf("#State=2 Wited\r\n");
+        WaitSend=true;
+    }
 }
 
 int main(void)
 {
     pc.baud(9600);
-/*    pc.printf("#Start\r\n");*/
-    Ticker ticker;
+    pc.printf("#Progran Start\r\n");
+    Ticker ticker;   //LED  HertBeat
     ticker.attach(periodicCallback, 1);
 
     ble.init();
@@ -191,51 +374,46 @@
     ble.gattClient().onDataWrite(DataWriteCallback);
     ble.gattClient().onHVX(hvxCallback);
 
-    uint8_t send[100]= {0xfe,03,03,0,4,0x1,0};
-    int state=-1;
+    uint8_t SendByte0[4]= {0xfe,00,00,00,};
+    uint8_t SendByte1[7]= {0xfe,03,03,0,4,0x1,0};
+    SendByte1[4]=FaceEyeClose*0x80+FaceLook*0x40+FaceSex*0x20+FaceOld*0x10+FaceDirection*0x8+FaceDetect*0x4+HandDetect*0x2+HumanDetect;
+    SendByte1[5]=FaceExpression;
+    state=-1;
+
     while (true) {
         if(state<0) {
             if (foundHVCWriteCharacteristic && foundHVCNotifyCharacteristic && foundHVCDeviceCharacteristic && !ble.gattClient().isServiceDiscoveryActive()) {
-            //    printf("# State 1 \r\n");
+                printf("# State 1 Wait for Caracteristic Found \r\n");
                 state=1;
             }
         } else if(state==1&&!ble.gattClient().isServiceDiscoveryActive()) {
-          //  printf("# State 2 \r\n");
+            printf("# State 2 Notify Enable \r\n");
             state=2;
-            ble.gattClient().onHVX(hvxCallback);
-
             uint16_t value = BLE_HVX_NOTIFICATION;
-            WaitSend=true;
+            WaitSend=false;
             ble.gattClient().write(GattClient::GATT_OP_WRITE_REQ,
                                    connectionHandle,
-                                   HVCNotifyCharacteristic.getValueHandle() + 1, /* HACK Alert. We're assuming that CCCD descriptor immediately follows the value attribute. */
-                                   sizeof(uint16_t),                          /* HACK Alert! size should be made into a BLE_API constant. */
+                                   HVCNotifyCharacteristic.getValueHandle() + 1,
+                                   sizeof(uint16_t),
                                    reinterpret_cast<const uint8_t *>(&value));
-            // HVCNotifyCharacteristic.write(   sizeof(uint16_t),     reinterpret_cast<const uint8_t *>(&value));                     /* HACK Alert! size should be made into a BLE_API constant. */
-            //
-            //HVCNotifyCharacteristic.enableNoticiation() ;
         } else if(state==2&&!ble.gattClient().isServiceDiscoveryActive()&&WaitSend) {
-
+            //3 KIDOUKAKUNIN
+            state=3;
             WaitSend=false;
-        //    pc.printf("#loop\r\n");
-            foundHVCWriteCharacteristic = false; /* need to do the following only once */
-
-            /* Note: Yuckiness alert! The following needs to be encapsulated in a neat API.
-             * It isn't clear whether we should provide a DiscoveredCharacteristic::enableNoticiation() or
-             * DiscoveredCharacteristic::discoverDescriptors() followed by DiscoveredDescriptor::write(...). */
-            // uint16_t value = BLE_HVX_NOTIFICATION;
-            // ble.gattClient().write(GattClient::GATT_OP_WRITE_REQ,
-            //                        connectionHandle,
-            //                        HVCNotifyCharacteristic.getValueHandle() + 1, /* HACK Alert. We're assuming that CCCD descriptor immediately follows the value attribute. */
-            //                        sizeof(uint16_t),                          /* HACK Alert! size should be made into a BLE_API constant. */
-            //                        reinterpret_cast<const uint8_t *>(&value));
+            pc.printf("# State3 kidoukakunin\r\n");
             ble.gattClient().write(GattClient::GATT_OP_WRITE_REQ,
                                    connectionHandle,
-                                   HVCWriteCharacteristic.getValueHandle(), /* HACK Alert. We're assuming that CCCD descriptor immediately follows the value attribute. */
-                                   7,                          /* HACK Alert! size should be made into a BLE_API constant. */
-                                   send);
-            //printf ( "snd_error %d \r\n" ,HVCWriteCharacteristic.write(4,send));
-
+                                   HVCWriteCharacteristic.getValueHandle(),
+                                   4,SendByte0);
+        } else if((state==3||state==4)&&!ble.gattClient().isServiceDiscoveryActive()&&WaitSend) {
+            //4 DataSend
+            state=4;
+            WaitSend=false;
+            pc.printf("# State4 DataSending\r\n");
+            ble.gattClient().write(GattClient::GATT_OP_WRITE_REQ,
+                                   connectionHandle,
+                                   HVCWriteCharacteristic.getValueHandle(),
+                                   7,SendByte1);
         }
         ble.waitForEvent();
     }