under constructiong

Dependencies:   mbed

Fork of BLE_WallbotBLE_Challenge 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 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