6/18 wallbot

Dependencies:   mbed

Fork of BLE_WallbotBLE_Challenge_byYUTAKA by Yutaka Yoshida

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers main.cpp Source File

main.cpp

00001 #include "mbed.h"
00002 #include "BLEDevice.h"
00003 #include "RCBController.h"
00004 #include "TB6612.h"
00005 #include "MPU6050.h"
00006 
00007 #define DBG 1
00008 
00009 BLEDevice  ble;
00010 
00011 MPU6050 mpu(I2C_SDA, I2C_SCL);
00012 
00013 #if DBG
00014 Serial  pc(USBTX, USBRX);
00015 #endif
00016 /* LEDs for indication: */
00017 DigitalOut  ModeLed(P0_19);
00018 DigitalOut  ConnectStateLed(P0_18);
00019 DigitalOut  outlow(P0_20);
00020 //PwmOut  ControllerStateLed(LED2);
00021 
00022 AnalogIn fsen1(P0_2);
00023 AnalogIn fsen2(P0_3);
00024 AnalogIn fsen3(P0_4);
00025 AnalogIn fsen4(P0_5);
00026 #if 1
00027 TB6612 left(P0_29,P0_23,P0_24);
00028 TB6612 right(P0_28,P0_30,P0_0);
00029 #else
00030 TB6612 left(P0_29,P0_24,P0_23);
00031 TB6612 right(P0_28,P0_0,P0_30);
00032 #endif
00033 Ticker ticker;
00034 
00035 DigitalIn   sw1(P0_16);
00036 DigitalIn   sw2(P0_17);
00037 
00038 DigitalIn encl1(P0_6);
00039 DigitalIn encl2(P0_7);
00040 DigitalIn encr1(P0_8);
00041 DigitalIn encr2(P0_10);
00042 
00043 
00044 int base_fsen[4];
00045 int line_mode = 0;
00046 int challenge_mode = 0;
00047 char bValue = 0;
00048 
00049 // Wallbot State
00050 int stt_Mode;
00051 
00052 int get_line(int num);
00053 
00054 /* RCBController Service */
00055 static const uint16_t RCBController_service_uuid = 0xFFF0;
00056 static const uint16_t RCBController_Characteristic_uuid = 0xFFF1;
00057 static const uint16_t RCBController_b_Characteristic_uuid = 0xFFF3;
00058 uint8_t RCBControllerPayload[10] = {0,};
00059 
00060 GattCharacteristic  ControllerChar (RCBController_Characteristic_uuid,RCBControllerPayload,10, 10,
00061                                 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | 
00062                                 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
00063 //static uint8_t _bValue = 0x00;
00064 static uint8_t _mValue[10] = {0,};
00065 GattCharacteristic b_Char(RCBController_b_Characteristic_uuid, _mValue, sizeof(_mValue), sizeof(_mValue),
00066                        GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
00067 
00068 GattCharacteristic *ControllerChars[] = {&ControllerChar,&b_Char};
00069 GattService         RCBControllerService(RCBController_service_uuid, ControllerChars, sizeof(ControllerChars) / sizeof(GattCharacteristic *));
00070 
00071 RCBController controller;
00072 
00073 void onConnected(Gap::Handle_t handle, const Gap::ConnectionParams_t *params)
00074 {
00075     ConnectStateLed = 0;
00076 #if DBG
00077     pc.printf("Connected\n\r");
00078 #endif
00079 }
00080 
00081 void onDisconnected(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
00082 {
00083     left = 0;
00084     right = 0;
00085 
00086     ble.startAdvertising();
00087     ConnectStateLed = 1;
00088 #if DBG
00089     pc.printf("Disconnected\n\r");
00090 #endif
00091 }
00092 
00093 void periodicCallback(void)
00094 {
00095     if (!ble.getGapState().connected) {
00096         return;
00097     }
00098     int line = get_line(0) ? 1 : 0;
00099         line |= get_line(1) ? 2 : 0;
00100         line |= get_line(2) ? 4 : 0;
00101         line |= get_line(3) ? 8 : 0;
00102     if( (bValue == 0)&&(line != 0) )
00103     {
00104         // game over
00105         left = 0.0;
00106         right = 0.0;
00107         bValue = 10;  
00108     }
00109     if( bValue > 0 )
00110     {
00111         memcpy( _mValue , "GAME OVER",10);
00112         ble.updateCharacteristicValue (b_Char.getValueAttribute().getHandle(), (uint8_t *)_mValue, sizeof(_mValue));
00113         ModeLed = !ModeLed;
00114         bValue--;
00115         if( bValue == 0 )
00116         {
00117             ModeLed = 1;
00118             challenge_mode = 0;
00119             ticker.detach();
00120         }
00121     }
00122 }
00123 
00124 
00125 // GattEvent
00126 void onDataWritten(const GattCharacteristicWriteCBParams *params)
00127 {
00128     if( (params->charHandle == ControllerChar.getValueAttribute().getHandle()) && (line_mode == 0))
00129     {
00130         memcpy( &controller.data[0], params->data , params->len );
00131         //memcpy( &controller.data[0], RCBControllerPayload, sizeof(controller));
00132 #if DBG
00133 
00134         pc.printf("DATA:%02X %02X %d %d %d %d %d %d %d %02X\n\r",controller.data[0],controller.data[1],controller.data[2],controller.data[3],controller.data[4],
00135                                                                controller.data[5],controller.data[6],controller.data[7],controller.data[8],controller.data[9]);
00136 #endif
00137         float right_factor;
00138         float left_factor;
00139 
00140         left_factor = ((float)((int)controller.status.LeftAnalogUD -128) / 128.0);
00141         right_factor = ((float)((int)controller.status.RightAnalogUD -128) / 128.0);
00142         
00143         if(challenge_mode == 1)
00144         {
00145             if( bValue == 0 )
00146             {
00147                 float factor = ((float)((int)controller.status.AcceleX -128) / 128.0); 
00148     
00149                 float right_factor = ((factor <= 0.0) ? 1.0 : 1.0 - (factor*2));
00150                 float left_factor = ((factor >= 0.0) ? 1.0 : 1.0 - (-factor*2));
00151     
00152                 if( controller.status.B == 1 )
00153                 {
00154                     left = left_factor;
00155                     right = right_factor;
00156                 }
00157                 else if( controller.status.A == 1 )
00158                 {
00159                     left = -right_factor;
00160                     right = -left_factor;
00161                 }
00162                 else
00163                 {
00164                     left = 0;
00165                     right = 0;
00166                 }
00167             }
00168         }
00169         else if( (left_factor != 0.0)||(right_factor != 0.0) )
00170         {
00171             left = left_factor;
00172             right = right_factor;
00173         }
00174         else
00175         {
00176             float factor = ((float)((int)controller.status.AcceleX -128) / 128.0); 
00177 
00178             float right_factor = ((factor <= 0.0) ? 1.0 : 1.0 - (factor*2));
00179             float left_factor = ((factor >= 0.0) ? 1.0 : 1.0 - (-factor*2));
00180 
00181             if( controller.status.B == 1 )
00182             {
00183                 left = left_factor;
00184                 right = right_factor;
00185             }
00186             else if( controller.status.A == 1 )
00187             {
00188                 left = -right_factor;
00189                 right = -left_factor;
00190             }
00191             else if( controller.status.UP == 1 )
00192             {
00193                 left = 1.0;
00194                 right = 1.0;
00195             }
00196             else if( controller.status.DOWN == 1 )
00197             {
00198                 left = -1.0;
00199                 right = -1.0;
00200             }
00201             else if( controller.status.RIGHT == 1 )
00202             {
00203                 left = 1.0;
00204                 right = -1.0;
00205             }
00206             else if( controller.status.LEFT == 1 )
00207             {
00208                 left = -1.0;
00209                 right = 1.0;
00210             }
00211             else
00212             {
00213                 left = 0.0;
00214                 right = 0.0;    
00215             }
00216             if((controller.status.UP == 1)&&(controller.status.DOWN == 1))
00217             {
00218                 left = 0.0;
00219                 right = 0.0;    
00220                 ModeLed = 0;
00221                 challenge_mode = 1;
00222                 ticker.attach(periodicCallback, 0.1);
00223 
00224             }
00225         }
00226         //ControllerStateLed = (float)controller.status.LeftAnalogLR / 255.0;            
00227     }
00228 }
00229 
00230 int get_fsen(int num)
00231 {
00232     switch(num)
00233     {
00234     case 0: 
00235         return((int)fsen1.read_u16());
00236     case 1: 
00237         return((int)fsen2.read_u16());
00238     case 2: 
00239         return((int)fsen3.read_u16());
00240     case 3: 
00241         return((int)fsen4.read_u16());
00242     }
00243     return(0);
00244 }
00245 
00246 void base()
00247 {
00248     wait(0.5);
00249     
00250     for(int i=0;i<4;i++)
00251     {
00252         base_fsen[i] = 0;
00253     }
00254     
00255     for(int j=0;j<10;j++)
00256     {
00257         for(int i=0;i<4;i++)
00258         {
00259             base_fsen[i] +=  get_fsen(i);
00260         }
00261         wait_ms(50);
00262     }
00263     for(int i=0;i<4;i++)
00264     {
00265         base_fsen[i] =  base_fsen[i] / 10;
00266     }
00267 #if DBG
00268     pc.printf("[0]:%05d[1]:%05d[2]:%05d[3]:%05d\n\r",base_fsen[0],base_fsen[1],base_fsen[2],base_fsen[3]);
00269 #endif
00270 }
00271 
00272 int get_line(int num)
00273 {
00274     int in = get_fsen(num);
00275     int ret = 0;
00276     
00277 #if 1
00278     if(in > 700)
00279 #else
00280     if( (in > (base_fsen[num] + 200))||(in < (base_fsen[num] - 200)))
00281 #endif
00282     {
00283         ret = 1;
00284     }
00285     return(ret);
00286 }
00287 
00288 //ライントレース関数
00289 void line(void)
00290 {
00291     ModeLed = 0;
00292     wait(1);
00293     while(sw1 != 0)
00294     {
00295 #if 0
00296         int line = get_line(0) ? 0 : 1;
00297             line |= get_line(1) ? 0 : 2;
00298             line |= get_line(2) ? 0 : 4;
00299             line |= get_line(3) ? 0 : 8;
00300 #else
00301         int line = get_line(0) ? 1 : 0;
00302             line |= get_line(1) ? 2 : 0;
00303             line |= get_line(2) ? 4 : 0;
00304             line |= get_line(3) ? 8 : 0;
00305 #endif
00306 
00307 #if DBG
00308         pc.printf("line=%02x %04x %04x %04x %04x\n\r",line,base_fsen[0],base_fsen[1],base_fsen[2],base_fsen[3]);
00309 #endif
00310 
00311 #if 1
00312         switch(line)
00313         {
00314             case 1:                 // ○○○●
00315                 left = 1.0;
00316                 right = -1.0;
00317                 break;
00318             case 3:                 // ○○●●
00319                 left = 1.0;
00320                 right = -0.5;
00321                 break;
00322             case 2:                 // ○○●○
00323                 left = 1.0;
00324                 right = 0.5;
00325                 break;
00326             case 6:                 // ○●●○
00327                 left = 1.0;
00328                 right = 1.0;
00329                 break;
00330             case 4:                 // ○●○○
00331                 left = 0.5;
00332                 right = 1.0;
00333                 break;
00334             case 12:                // ●●○○
00335                 left = -0.5;
00336                 right = 1.0;
00337                 break;
00338             case 8:                 // ●○○○
00339                 left = -1.0;
00340                 right = 1.0;
00341                 break;
00342             default:
00343                 left = 1.0;
00344                 right = 1.0;
00345                 break;
00346         }
00347 #endif
00348 
00349     }
00350     ModeLed = 1;
00351     left = 0.0;
00352     right = 0.0;
00353     wait(1);
00354 }
00355 
00356 //動作パターン関数(工事中)
00357 void wb_control(void)
00358 {
00359     ModeLed = 0;
00360     wait(1);    
00361     
00362     // Wallbot State Initialize
00363     // Up Straight  : 1
00364     // Up Back      : 2
00365     // Up Rotate    : 3
00366     // Down Straight  : 4
00367     // Down Back      : 5
00368     // Down Rotate    : 6
00369     stt_Mode = 1;
00370     
00371     // 値を格納する用の配列、変数
00372     float   acData[3];
00373     float   gyData[3];
00374     float ax = 0;
00375     float ay = 0;
00376     float az = 0;
00377     float a = 0;
00378     float buf_ax[10];
00379     float mean_ax = 0;
00380     float sum_ax = 0;
00381     float log_ax[100];
00382     int cnt_loop = 0;
00383     int cnt_back = 0;
00384     int cnt_straight = 0;
00385     float thre_bump = 4.0;
00386     bool enable_ChangeMode = false;
00387     
00388         for(int i = 0; i < 10; i++)
00389         {
00390             buf_ax[i] = 0;
00391         }
00392     
00393         
00394         for(int i = 0; i < 100; i++)
00395         {
00396             log_ax[i] = 0;
00397         }
00398     
00399     while(sw1 != 0)
00400     {
00401 //        Wait(0.1);
00402         
00403         // Get value                   
00404         //加速度を取得
00405         Timer acTimer;
00406         acTimer.start();
00407         mpu.getAccelero(acData);    //加速度を取得 acDataに格納
00408         acTimer.stop();
00409     //  at = acTimer.read_ms();
00410         acTimer.reset();
00411      
00412         //ジャイロを取得
00413         Timer gyTimer;
00414         gyTimer.start();
00415         mpu.getGyro(gyData);    //ジャイロの値(角加速度)を取得 gyDataに格納
00416         gyTimer.stop();
00417     //  gt = gyTimer.read_ms();
00418         gyTimer.reset();
00419      
00420         //floatの値を合計5桁、小数点以下3桁の形でPCへ送信
00421 //        pc.printf("%5.3f:%5.3f:%5.3f:%5.3f:%5.3f:%5.3f \n", acData[0], acData[1], acData[2],gyData[0],gyData[1],gyData[2]);  
00422 //        pc.printf("%5.3f, ", acData[2]);
00423 
00424         ax = acData[0];
00425         ay = acData[1];
00426         az = acData[2];
00427 //        a = sqrt(ax * ax + ay * ay + az * az);
00428 //        pc.printf("%5.3f, ", ax);
00429         
00430 //        log_ax[cnt_loop] = ax;
00431         
00432 //        if(!enble_ChangeMode)
00433 //        {
00434 //            if(cnt_loop > 20)
00435 //            {
00436 //                enble_ChangeMode = true;
00437 //                }
00438 //            }
00439         
00440         // ax Buffer Store
00441         for(int i = 9; i > 0; i--)
00442         {
00443             buf_ax[i] = buf_ax[i - 1];
00444             }
00445         buf_ax[0] = ax;
00446         sum_ax += buf_ax[1];
00447         sum_ax -= buf_ax[9];
00448         mean_ax = sum_ax / 9;
00449         pc.printf("%5.3f, ", ax);
00450 //        
00451 //        for(int i = 29; i > 0; i--)
00452 //        {
00453 //            log_ax[i] = log_ax[i - 1];
00454 //            }
00455 //            log_ax[0] = ax;
00456         
00457         if(cnt_loop % 100 == 0)
00458         {
00459 //                        for(int i = 0; i < 100; i++)
00460 //            {
00461 //                pc.printf("%5.3f, ", log_ax[i]);
00462 //                }
00463 //            pc.printf("loop : %d\n", log_ax);
00464 //            pc.printf("loop : %d\n", cnt_loop);
00465 //            pc.printf("loop : %d\n", cnt_loop);
00466 //            pc.printf("loop : %d\n", cnt_loop);
00467 //            pc.printf("loop : %d\n", cnt_loop);
00468             }
00469         
00470 #if 1
00471         switch(stt_Mode)
00472         {
00473             case 1:                 // Up Straight
00474 //                pc.printf("Mode 1 \n");
00475             
00476 //            for(int i = 0; i < 30; i++)
00477 //            {
00478 //                pc.printf("%5.3f, ", log_ax[i]);
00479 //                }
00480 //                pc.printf("\n");
00481 //                pc.printf("\n");
00482 //                pc.printf("\n");
00483             
00484                 if(ay > 0.5){
00485                         left = 0.9;
00486                         right = 1.0;
00487 //                        pc.printf("Lean Right");
00488                     }
00489                     else if(ay < -0.5){
00490                         left = 1.0;
00491                         right = 0.9;
00492 //                        pc.printf("Lean Left");
00493                     }
00494                     else
00495                     {
00496                         left = 1.0;
00497                         right = 1.0;
00498                         }
00499                         
00500                     // Judge Bump
00501                     if(enable_ChangeMode)
00502                     {                        
00503                         if(ax - buf_ax[5] < -thre_bump || ax - buf_ax[5] > thre_bump)
00504                         {
00505                             stt_Mode = 2;
00506                             pc.printf("Mode 1 -> 2 \n");
00507                             
00508                             enable_ChangeMode = false;
00509                             }
00510                     }else
00511                     {
00512                         cnt_straight++;
00513                         if(cnt_straight > 100)
00514                         {
00515                             enable_ChangeMode = true;
00516                             cnt_straight = 0;
00517                             }
00518                         }
00519                 
00520                 break;
00521             case 2:             // Up Back
00522 //                pc.printf("Mode 2 \n");
00523                 if(cnt_back < 30)
00524                 {
00525                     left = 0;//-1.0;
00526                     right = 0;//-1.0;
00527                     //cnt_back++;
00528                     }
00529                     else{
00530                             cnt_back = 0;
00531                             stt_Mode = 3;
00532                             pc.printf("Mode 2 -> 3 \n");
00533                         }
00534                 
00535                 break;
00536             case 3:                 // Up Rotate
00537                 pc.printf("Mode 3 \n");
00538                 if(!(ax < -9.7/* && ay < 0.5 && ay >= -0.3*/))      // Change Using Gyro??
00539                 {
00540                     left = 1.0;
00541                     right = 0;                    
00542                     }
00543                     else
00544                     {
00545                         stt_Mode = 4;
00546                         pc.printf("Mode 3 - > 4 \n");
00547                         
00548                         }
00549                 
00550                 break;
00551             
00552             case 4:                 // Down Straight
00553                 pc.printf("Mode 4 \n");
00554                 if(ay > 0.5){
00555                         left = 1.0;
00556                         right = 0.9;
00557                     }
00558                     else if(ay < -0.5){
00559                         left = 0.9;
00560                         right = 1.0;
00561                     }
00562                     else
00563                     {
00564                         left = 1.0;
00565                         right = 1.0;
00566                         }
00567                         
00568                     // Judge Bump
00569                     if(enable_ChangeMode)
00570                     {
00571                         if(ax - buf_ax[1] > thre_bump /*&& (ax - buf_ax[2] > thre_bump)*/)
00572                         {
00573                             stt_Mode = 5;
00574                             pc.printf("Mode 4 -> 5 \n");
00575                             
00576                             enable_ChangeMode = false;
00577                             }
00578                     }else
00579                     {                        
00580                         cnt_straight++;
00581                         if(cnt_straight > 10)
00582                         {
00583                             enable_ChangeMode = true;
00584                             cnt_straight = 0;
00585                             }
00586                         }
00587                 
00588                 break;
00589             case 5:           
00590                 pc.printf("Mode 5 \n");
00591                 if(cnt_back < 30)
00592                 {
00593                     left = -1.0;
00594                     right = -1.0;
00595                     cnt_back++;
00596                     }
00597                     else{
00598                             cnt_back = 0;
00599                             stt_Mode = 6;
00600                             pc.printf(" 5 -> 6 \n");
00601                         }
00602                 
00603                 break;
00604             case 6:                 //
00605                 pc.printf("Mode 6 \n");
00606                 if(!(ax > 9.7 /*&& ay < 0.5 && ay >= 0*/))      // Change Using Gyro??
00607                 {
00608                     left = 0;
00609                     right = 1.0;                    
00610                     }
00611                     else
00612                     {
00613                         stt_Mode = 1;
00614                         pc.printf(" 6 -> 1 \n");
00615 //                        
00616 //                        for(int i = 0; i < 3; i++)
00617 //                        {
00618 //                            buf_ax[i] = ax;
00619 //                            }
00620                         }
00621                         
00622                 
00623                 break;
00624             default:
00625                 break;
00626         }
00627 #endif
00628 
00629         cnt_loop++;
00630 
00631         if(cnt_loop > 2000)
00632         {
00633             cnt_loop = 0;
00634             }
00635     }
00636     ModeLed = 1;
00637     left = 0.0;
00638     right = 0.0;
00639     wait(1);
00640 }
00641 
00642 #if 0
00643 int counter1 = 0;
00644 void p1_rise()
00645 {
00646     if( pin2 == 1 )
00647     {
00648         counter1++;
00649     }
00650     else
00651     {
00652         counter1--;
00653     }
00654 }
00655 #endif
00656 
00657 /**************************************************************************/
00658 /*!
00659     @brief  Program entry point
00660 */
00661 /**************************************************************************/
00662 int main(void)
00663 {
00664     sw1.mode(PullUp);
00665     sw2.mode(PullUp);
00666 
00667     encl1.mode(PullNone);
00668     encl2.mode(PullNone);
00669     encr1.mode(PullNone);
00670     encr2.mode(PullNone);
00671 
00672     ModeLed = 1;
00673     ConnectStateLed = 1;
00674 #if DBG
00675     //pc.baud(921600);
00676     pc.baud(9600);
00677     pc.printf("Start\n\r");
00678 #endif
00679     outlow = 0;
00680     
00681     if(sw2 == 0)
00682     {
00683 //        pin1.mode(PullDown);
00684 //        pin1.rise(&p1_rise);
00685         while(1)
00686         {
00687             //int in1 = pin1;
00688             //int in2 = pin2;
00689             //ModeLed = pin1;
00690             //pc.printf("dat = %d %d\r\n",in1,in2);
00691             base();
00692 #if 0
00693             left = 1.0;
00694             right = 1.0;
00695             wait(5);           
00696             left = -1.0;
00697             right = -1.0;
00698             wait(5);
00699 #endif        
00700         }
00701     }
00702 
00703 //    // MPU6050 Initialize
00704 //    mpu.initialize();
00705 //    mpu.setAcceleroRange(MPU6050_ACCELERO_RANGE_8G);
00706 //    mpu.setGyroRange(MPU6050_GYRO_RANGE_1000);
00707 
00708     ///180601 MPU6050センサの初期化処理
00709     mpu.initialize();
00710 //  mpu.setAcceleroRange(MPU6050_ACCELERO_RANGE_8G);    //加速度センサ 計測レンジ設定(今回は2Gか4Gがよさそう)
00711     mpu.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G); 
00712 //   mpu.setGyroRange(MPU6050_GYRO_RANGE_1000);          //ジャイロセンサ 計測レンジ設定(ここも250か500がよさそう(そんなに早く回転しないので))
00713     mpu.setGyroRange(MPU6050_GYRO_RANGE_250);
00714     ///180601 
00715 
00716     ble.init(); 
00717     ble.onConnection(onConnected);
00718     ble.onDisconnection(onDisconnected);
00719     ble.onDataWritten(onDataWritten);
00720     
00721     /* setup advertising */
00722     ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
00723     ble.setAdvertisingType (GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
00724     ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
00725                                     (const uint8_t *)"mbed WallbotBLE", sizeof("mbed WallbotBLE") - 1);
00726     ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,
00727                                     (const uint8_t *)RCBController_service_uuid, sizeof(RCBController_service_uuid));
00728 
00729     ble.setAdvertisingInterval (160); /* 100ms; in multiples of 0.625ms. */
00730     ble.startAdvertising();
00731 
00732     ble.addService(RCBControllerService);
00733 
00734 
00735     while (true) {
00736         if(sw1 == 0)
00737         {
00738             bValue = 1;
00739             line_mode = 1;
00740             //line();
00741             wb_control(); //動作モード関数
00742             line_mode = 0;
00743             bValue = 0;
00744         }
00745         ble.waitForEvent();
00746     }
00747 }
00748