6/18 wallbot
Fork of BLE_WallbotBLE_Challenge_byYUTAKA by
Embed:
(wiki syntax)
Show/hide line numbers
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
Generated on Wed Jul 13 2022 01:26:57 by
1.7.2
