under constructiong
Fork of BLE_WallbotBLE_Challenge 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 0 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 int get_line(int num); 00050 00051 /* RCBController Service */ 00052 static const uint16_t RCBController_service_uuid = 0xFFF0; 00053 static const uint16_t RCBController_Characteristic_uuid = 0xFFF1; 00054 static const uint16_t RCBController_b_Characteristic_uuid = 0xFFF3; 00055 uint8_t RCBControllerPayload[10] = {0,}; 00056 00057 GattCharacteristic ControllerChar (RCBController_Characteristic_uuid,RCBControllerPayload,10, 10, 00058 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | 00059 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE); 00060 //static uint8_t _bValue = 0x00; 00061 static uint8_t _mValue[10] = {0,}; 00062 GattCharacteristic b_Char(RCBController_b_Characteristic_uuid, _mValue, sizeof(_mValue), sizeof(_mValue), 00063 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY); 00064 00065 GattCharacteristic *ControllerChars[] = {&ControllerChar,&b_Char}; 00066 GattService RCBControllerService(RCBController_service_uuid, ControllerChars, sizeof(ControllerChars) / sizeof(GattCharacteristic *)); 00067 00068 RCBController controller; 00069 00070 void onConnected(Gap::Handle_t handle, const Gap::ConnectionParams_t *params) 00071 { 00072 ConnectStateLed = 0; 00073 #if DBG 00074 pc.printf("Connected\n\r"); 00075 #endif 00076 } 00077 00078 void onDisconnected(Gap::Handle_t handle, Gap::DisconnectionReason_t reason) 00079 { 00080 left = 0; 00081 right = 0; 00082 00083 ble.startAdvertising(); 00084 ConnectStateLed = 1; 00085 #if DBG 00086 pc.printf("Disconnected\n\r"); 00087 #endif 00088 } 00089 00090 void periodicCallback(void) 00091 { 00092 if (!ble.getGapState().connected) { 00093 return; 00094 } 00095 int line = get_line(0) ? 1 : 0; 00096 line |= get_line(1) ? 2 : 0; 00097 line |= get_line(2) ? 4 : 0; 00098 line |= get_line(3) ? 8 : 0; 00099 if( (bValue == 0)&&(line != 0) ) 00100 { 00101 // game over 00102 left = 0.0; 00103 right = 0.0; 00104 bValue = 10; 00105 } 00106 if( bValue > 0 ) 00107 { 00108 memcpy( _mValue , "GAME OVER",10); 00109 ble.updateCharacteristicValue (b_Char.getValueAttribute().getHandle(), (uint8_t *)_mValue, sizeof(_mValue)); 00110 ModeLed = !ModeLed; 00111 bValue--; 00112 if( bValue == 0 ) 00113 { 00114 ModeLed = 1; 00115 challenge_mode = 0; 00116 ticker.detach(); 00117 } 00118 } 00119 } 00120 00121 00122 // GattEvent 00123 void onDataWritten(const GattCharacteristicWriteCBParams *params) 00124 { 00125 if( (params->charHandle == ControllerChar.getValueAttribute().getHandle()) && (line_mode == 0)) 00126 { 00127 memcpy( &controller.data[0], params->data , params->len ); 00128 //memcpy( &controller.data[0], RCBControllerPayload, sizeof(controller)); 00129 #if DBG 00130 00131 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], 00132 controller.data[5],controller.data[6],controller.data[7],controller.data[8],controller.data[9]); 00133 #endif 00134 float right_factor; 00135 float left_factor; 00136 00137 left_factor = ((float)((int)controller.status.LeftAnalogUD -128) / 128.0); 00138 right_factor = ((float)((int)controller.status.RightAnalogUD -128) / 128.0); 00139 00140 if(challenge_mode == 1) 00141 { 00142 if( bValue == 0 ) 00143 { 00144 float factor = ((float)((int)controller.status.AcceleX -128) / 128.0); 00145 00146 float right_factor = ((factor <= 0.0) ? 1.0 : 1.0 - (factor*2)); 00147 float left_factor = ((factor >= 0.0) ? 1.0 : 1.0 - (-factor*2)); 00148 00149 if( controller.status.B == 1 ) 00150 { 00151 left = left_factor; 00152 right = right_factor; 00153 } 00154 else if( controller.status.A == 1 ) 00155 { 00156 left = -right_factor; 00157 right = -left_factor; 00158 } 00159 else 00160 { 00161 left = 0; 00162 right = 0; 00163 } 00164 } 00165 } 00166 else if( (left_factor != 0.0)||(right_factor != 0.0) ) 00167 { 00168 left = left_factor; 00169 right = right_factor; 00170 } 00171 else 00172 { 00173 float factor = ((float)((int)controller.status.AcceleX -128) / 128.0); 00174 00175 float right_factor = ((factor <= 0.0) ? 1.0 : 1.0 - (factor*2)); 00176 float left_factor = ((factor >= 0.0) ? 1.0 : 1.0 - (-factor*2)); 00177 00178 if( controller.status.B == 1 ) 00179 { 00180 left = left_factor; 00181 right = right_factor; 00182 } 00183 else if( controller.status.A == 1 ) 00184 { 00185 left = -right_factor; 00186 right = -left_factor; 00187 } 00188 else if( controller.status.UP == 1 ) 00189 { 00190 left = 1.0; 00191 right = 1.0; 00192 } 00193 else if( controller.status.DOWN == 1 ) 00194 { 00195 left = -1.0; 00196 right = -1.0; 00197 } 00198 else if( controller.status.RIGHT == 1 ) 00199 { 00200 left = 1.0; 00201 right = -1.0; 00202 } 00203 else if( controller.status.LEFT == 1 ) 00204 { 00205 left = -1.0; 00206 right = 1.0; 00207 } 00208 else 00209 { 00210 left = 0.0; 00211 right = 0.0; 00212 } 00213 if((controller.status.UP == 1)&&(controller.status.DOWN == 1)) 00214 { 00215 left = 0.0; 00216 right = 0.0; 00217 ModeLed = 0; 00218 challenge_mode = 1; 00219 ticker.attach(periodicCallback, 0.1); 00220 00221 } 00222 } 00223 //ControllerStateLed = (float)controller.status.LeftAnalogLR / 255.0; 00224 } 00225 } 00226 00227 int get_fsen(int num) 00228 { 00229 switch(num) 00230 { 00231 case 0: 00232 return((int)fsen1.read_u16()); 00233 case 1: 00234 return((int)fsen2.read_u16()); 00235 case 2: 00236 return((int)fsen3.read_u16()); 00237 case 3: 00238 return((int)fsen4.read_u16()); 00239 } 00240 return(0); 00241 } 00242 00243 void base() 00244 { 00245 wait(0.5); 00246 00247 for(int i=0;i<4;i++) 00248 { 00249 base_fsen[i] = 0; 00250 } 00251 00252 for(int j=0;j<10;j++) 00253 { 00254 for(int i=0;i<4;i++) 00255 { 00256 base_fsen[i] += get_fsen(i); 00257 } 00258 wait_ms(50); 00259 } 00260 for(int i=0;i<4;i++) 00261 { 00262 base_fsen[i] = base_fsen[i] / 10; 00263 } 00264 #if DBG 00265 pc.printf("[0]:%05d[1]:%05d[2]:%05d[3]:%05d\n\r",base_fsen[0],base_fsen[1],base_fsen[2],base_fsen[3]); 00266 #endif 00267 } 00268 00269 int get_line(int num) 00270 { 00271 int in = get_fsen(num); 00272 int ret = 0; 00273 00274 #if 1 00275 if(in > 700) 00276 #else 00277 if( (in > (base_fsen[num] + 200))||(in < (base_fsen[num] - 200))) 00278 #endif 00279 { 00280 ret = 1; 00281 } 00282 return(ret); 00283 } 00284 00285 //ライントレース関数 00286 void line(void) 00287 { 00288 ModeLed = 0; 00289 wait(1); 00290 while(sw1 != 0) 00291 { 00292 #if 0 00293 int line = get_line(0) ? 0 : 1; 00294 line |= get_line(1) ? 0 : 2; 00295 line |= get_line(2) ? 0 : 4; 00296 line |= get_line(3) ? 0 : 8; 00297 #else 00298 int line = get_line(0) ? 1 : 0; 00299 line |= get_line(1) ? 2 : 0; 00300 line |= get_line(2) ? 4 : 0; 00301 line |= get_line(3) ? 8 : 0; 00302 #endif 00303 00304 #if DBG 00305 pc.printf("line=%02x %04x %04x %04x %04x\n\r",line,base_fsen[0],base_fsen[1],base_fsen[2],base_fsen[3]); 00306 #endif 00307 00308 #if 1 00309 switch(line) 00310 { 00311 case 1: // ○○○● 00312 left = 1.0; 00313 right = -1.0; 00314 break; 00315 case 3: // ○○●● 00316 left = 1.0; 00317 right = -0.5; 00318 break; 00319 case 2: // ○○●○ 00320 left = 1.0; 00321 right = 0.5; 00322 break; 00323 case 6: // ○●●○ 00324 left = 1.0; 00325 right = 1.0; 00326 break; 00327 case 4: // ○●○○ 00328 left = 0.5; 00329 right = 1.0; 00330 break; 00331 case 12: // ●●○○ 00332 left = -0.5; 00333 right = 1.0; 00334 break; 00335 case 8: // ●○○○ 00336 left = -1.0; 00337 right = 1.0; 00338 break; 00339 default: 00340 left = 1.0; 00341 right = 1.0; 00342 break; 00343 } 00344 #endif 00345 00346 } 00347 ModeLed = 1; 00348 left = 0.0; 00349 right = 0.0; 00350 wait(1); 00351 } 00352 00353 //動作パターン関数(工事中) 00354 void wb_control(void) 00355 { 00356 ModeLed = 0; 00357 wait(1); 00358 while(sw1 != 0) 00359 { 00360 #if 0 00361 int line = get_line(0) ? 0 : 1; 00362 line |= get_line(1) ? 0 : 2; 00363 line |= get_line(2) ? 0 : 4; 00364 line |= get_line(3) ? 0 : 8; 00365 #else 00366 int line = get_line(0) ? 1 : 0; 00367 line |= get_line(1) ? 2 : 0; 00368 line |= get_line(2) ? 4 : 0; 00369 line |= get_line(3) ? 8 : 0; 00370 #endif 00371 00372 #if DBG 00373 pc.printf("line=%02x %04x %04x %04x %04x\n\r",line,base_fsen[0],base_fsen[1],base_fsen[2],base_fsen[3]); 00374 #endif 00375 00376 //改造中のソース 00377 00378 #if 1 00379 switch(line) 00380 { 00381 case 1: //直進モード上から下へ ○○○● とりあえず書いてみたパターン 00382 if(ay<0){ 00383 left = 1.0; 00384 right = 1.0; 00385 } 00386 else if(ay>0){ 00387 left = 1.0; 00388 right = 1.0; 00389 } 00390 00391 break; 00392 case 3: //直進モード下から上へ ○○●● 00393 left = 1.0; 00394 right = -0.5; 00395 break; 00396 case 2: //回転モード上から下へ○○●○ 00397 left = 1.0; 00398 right = 0.5; 00399 break; 00400 case 6: //回転モード下から上へ○●●○ 00401 left = 1.0; 00402 right = 1.0; 00403 break; 00404 case 4: //後進モード上から下へ○●○○ 00405 left = 0.5; 00406 right = 1.0; 00407 break; 00408 case 12: //後進モード下から上へ●●○○ 00409 left = -0.5; 00410 right = 1.0; 00411 break; 00412 case 8: // ●○○○ 00413 left = -1.0; 00414 right = 1.0; 00415 break; 00416 default: 00417 left = 1.0; 00418 right = 1.0; 00419 break; 00420 } 00421 #endif 00422 00423 } 00424 ModeLed = 1; 00425 left = 0.0; 00426 right = 0.0; 00427 wait(1); 00428 } 00429 00430 #if 0 00431 int counter1 = 0; 00432 void p1_rise() 00433 { 00434 if( pin2 == 1 ) 00435 { 00436 counter1++; 00437 } 00438 else 00439 { 00440 counter1--; 00441 } 00442 } 00443 #endif 00444 00445 /**************************************************************************/ 00446 /*! 00447 @brief Program entry point 00448 */ 00449 /**************************************************************************/ 00450 int main(void) 00451 { 00452 sw1.mode(PullUp); 00453 sw2.mode(PullUp); 00454 00455 encl1.mode(PullNone); 00456 encl2.mode(PullNone); 00457 encr1.mode(PullNone); 00458 encr2.mode(PullNone); 00459 00460 ModeLed = 1; 00461 ConnectStateLed = 1; 00462 #if DBG 00463 //pc.baud(921600); 00464 pc.baud(9600); 00465 pc.printf("Start\n\r"); 00466 #endif 00467 outlow = 0; 00468 00469 if(sw2 == 0) 00470 { 00471 // pin1.mode(PullDown); 00472 // pin1.rise(&p1_rise); 00473 while(1) 00474 { 00475 //int in1 = pin1; 00476 //int in2 = pin2; 00477 //ModeLed = pin1; 00478 //pc.printf("dat = %d %d\r\n",in1,in2); 00479 base(); 00480 #if 0 00481 left = 1.0; 00482 right = 1.0; 00483 wait(5); 00484 left = -1.0; 00485 right = -1.0; 00486 wait(5); 00487 #endif 00488 } 00489 } 00490 00491 // MPU6050 Initialize 00492 mpu.initialize(); 00493 mpu.setAcceleroRange(MPU6050_ACCELERO_RANGE_8G); 00494 mpu.setGyroRange(MPU6050_GYRO_RANGE_1000); 00495 00496 ble.init(); 00497 ble.onConnection(onConnected); 00498 ble.onDisconnection(onDisconnected); 00499 ble.onDataWritten(onDataWritten); 00500 00501 /* setup advertising */ 00502 ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED); 00503 ble.setAdvertisingType (GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED); 00504 ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME, 00505 (const uint8_t *)"mbed WallbotBLE", sizeof("mbed WallbotBLE") - 1); 00506 ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, 00507 (const uint8_t *)RCBController_service_uuid, sizeof(RCBController_service_uuid)); 00508 00509 ble.setAdvertisingInterval (160); /* 100ms; in multiples of 0.625ms. */ 00510 ble.startAdvertising(); 00511 00512 ble.addService(RCBControllerService); 00513 00514 00515 while (true) { 00516 if(sw1 == 0) 00517 { 00518 bValue = 1; 00519 line_mode = 1; 00520 //line(); 00521 wb_control(); //動作モード関数 00522 line_mode = 0; 00523 bValue = 0; 00524 } 00525 ble.waitForEvent(); 00526 } 00527 } 00528
Generated on Tue Jul 12 2022 20:46:10 by
1.7.2
