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 Serial  pc(USBTX, USBRX);
00013 
00014 /* LEDs for indication: */
00015 DigitalOut  ModeLed(P0_19);
00016 DigitalOut  ConnectStateLed(P0_18);
00017 DigitalOut  outlow(P0_20);
00018 //PwmOut  ControllerStateLed(LED2);
00019 
00020 AnalogIn fsen1(P0_2);
00021 AnalogIn fsen2(P0_3);
00022 AnalogIn fsen3(P0_4);
00023 AnalogIn fsen4(P0_5);
00024 #if 1
00025 TB6612 left(P0_29,P0_23,P0_24);
00026 TB6612 right(P0_28,P0_30,P0_0);
00027 #else
00028 TB6612 left(P0_29,P0_24,P0_23);
00029 TB6612 right(P0_28,P0_0,P0_30);
00030 #endif
00031 Ticker ticker;
00032 
00033 DigitalIn   sw1(P0_16);
00034 DigitalIn   sw2(P0_17);
00035 
00036 DigitalIn encl1(P0_6);
00037 DigitalIn encl2(P0_7);
00038 DigitalIn encr1(P0_8);
00039 DigitalIn encr2(P0_10);
00040 
00041 
00042 int base_fsen[4];
00043 int line_mode = 0;
00044 int challenge_mode = 0;
00045 char bValue = 0;
00046 
00047 int get_line(int num);
00048 
00049 /* RCBController Service */
00050 static const uint16_t RCBController_service_uuid = 0xFFF0;
00051 static const uint16_t RCBController_Characteristic_uuid = 0xFFF1;
00052 static const uint16_t RCBController_b_Characteristic_uuid = 0xFFF3;
00053 uint8_t RCBControllerPayload[10] = {0,};
00054 
00055 GattCharacteristic  ControllerChar (RCBController_Characteristic_uuid,RCBControllerPayload,10, 10,
00056                                 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | 
00057                                 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
00058 //static uint8_t _bValue = 0x00;
00059 static uint8_t _mValue[10] = {0,};
00060 GattCharacteristic b_Char(RCBController_b_Characteristic_uuid, _mValue, sizeof(_mValue), sizeof(_mValue),
00061                        GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
00062 
00063 GattCharacteristic *ControllerChars[] = {&ControllerChar,&b_Char};
00064 GattService         RCBControllerService(RCBController_service_uuid, ControllerChars, sizeof(ControllerChars) / sizeof(GattCharacteristic *));
00065 
00066 RCBController controller;
00067 
00068 void onConnected(Gap::Handle_t handle, const Gap::ConnectionParams_t *params)
00069 {
00070     ConnectStateLed = 0;
00071 #if DBG
00072     pc.printf("Connected\n\r");
00073 #endif
00074 }
00075 
00076 void onDisconnected(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
00077 {
00078     left = 0;
00079     right = 0;
00080 
00081     ble.startAdvertising();
00082     ConnectStateLed = 1;
00083 #if DBG
00084     pc.printf("Disconnected\n\r");
00085 #endif
00086 }
00087 
00088 void periodicCallback(void)
00089 {
00090     if (!ble.getGapState().connected) {
00091         return;
00092     }
00093     int line = get_line(0) ? 1 : 0;
00094         line |= get_line(1) ? 2 : 0;
00095         line |= get_line(2) ? 4 : 0;
00096         line |= get_line(3) ? 8 : 0;
00097     if( (bValue == 0)&&(line != 0) )
00098     {
00099         // game over
00100         left = 0.0;
00101         right = 0.0;
00102         bValue = 10;  
00103     }
00104     if( bValue > 0 )
00105     {
00106         memcpy( _mValue , "GAME OVER",10);
00107         ble.updateCharacteristicValue (b_Char.getValueAttribute().getHandle(), (uint8_t *)_mValue, sizeof(_mValue));
00108         ModeLed = !ModeLed;
00109         bValue--;
00110         if( bValue == 0 )
00111         {
00112             ModeLed = 1;
00113             challenge_mode = 0;
00114             ticker.detach();
00115         }
00116     }
00117 }
00118 
00119 
00120 // GattEvent
00121 void onDataWritten(const GattCharacteristicWriteCBParams *params)
00122 {
00123     if( (params->charHandle == ControllerChar.getValueAttribute().getHandle()) && (line_mode == 0))
00124     {
00125         memcpy( &controller.data[0], params->data , params->len );
00126         //memcpy( &controller.data[0], RCBControllerPayload, sizeof(controller));
00127 #if DBG
00128 
00129         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],
00130                                                                controller.data[5],controller.data[6],controller.data[7],controller.data[8],controller.data[9]);
00131 #endif
00132         float right_factor;
00133         float left_factor;
00134 
00135         left_factor = ((float)((int)controller.status.LeftAnalogUD -128) / 128.0);
00136         right_factor = ((float)((int)controller.status.RightAnalogUD -128) / 128.0);
00137         
00138         if(challenge_mode == 1)
00139         {
00140             if( bValue == 0 )
00141             {
00142                 float factor = ((float)((int)controller.status.AcceleX -128) / 128.0); 
00143     
00144                 float right_factor = ((factor <= 0.0) ? 1.0 : 1.0 - (factor*2));
00145                 float left_factor = ((factor >= 0.0) ? 1.0 : 1.0 - (-factor*2));
00146     
00147                 if( controller.status.B == 1 )
00148                 {
00149                     left = left_factor;
00150                     right = right_factor;
00151                 }
00152                 else if( controller.status.A == 1 )
00153                 {
00154                     left = -right_factor;
00155                     right = -left_factor;
00156                 }
00157                 else
00158                 {
00159                     left = 0;
00160                     right = 0;
00161                 }
00162             }
00163         }
00164         else if( (left_factor != 0.0)||(right_factor != 0.0) )
00165         {
00166             left = left_factor;
00167             right = right_factor;
00168         }
00169         else
00170         {
00171             float factor = ((float)((int)controller.status.AcceleX -128) / 128.0); 
00172 
00173             float right_factor = ((factor <= 0.0) ? 1.0 : 1.0 - (factor*2));
00174             float left_factor = ((factor >= 0.0) ? 1.0 : 1.0 - (-factor*2));
00175 
00176             if( controller.status.B == 1 )
00177             {
00178                 left = left_factor;
00179                 right = right_factor;
00180             }
00181             else if( controller.status.A == 1 )
00182             {
00183                 left = -right_factor;
00184                 right = -left_factor;
00185             }
00186             else if( controller.status.UP == 1 )
00187             {
00188                 left = 1.0;
00189                 right = 1.0;
00190             }
00191             else if( controller.status.DOWN == 1 )
00192             {
00193                 left = -1.0;
00194                 right = -1.0;
00195             }
00196             else if( controller.status.RIGHT == 1 )
00197             {
00198                 left = 1.0;
00199                 right = -1.0;
00200             }
00201             else if( controller.status.LEFT == 1 )
00202             {
00203                 left = -1.0;
00204                 right = 1.0;
00205             }
00206             else
00207             {
00208                 left = 0.0;
00209                 right = 0.0;    
00210             }
00211             if((controller.status.UP == 1)&&(controller.status.DOWN == 1))
00212             {
00213                 left = 0.0;
00214                 right = 0.0;    
00215                 ModeLed = 0;
00216                 challenge_mode = 1;
00217                 ticker.attach(periodicCallback, 0.1);
00218 
00219             }
00220         }
00221         //ControllerStateLed = (float)controller.status.LeftAnalogLR / 255.0;            
00222     }
00223 }
00224 
00225 int get_fsen(int num)
00226 {
00227     switch(num)
00228     {
00229     case 0: 
00230         return((int)fsen1.read_u16());
00231     case 1: 
00232         return((int)fsen2.read_u16());
00233     case 2: 
00234         return((int)fsen3.read_u16());
00235     case 3: 
00236         return((int)fsen4.read_u16());
00237     }
00238     return(0);
00239 }
00240 
00241 void base()
00242 {
00243     wait(0.5);
00244     
00245     for(int i=0;i<4;i++)
00246     {
00247         base_fsen[i] = 0;
00248     }
00249     
00250     for(int j=0;j<10;j++)
00251     {
00252         for(int i=0;i<4;i++)
00253         {
00254             base_fsen[i] +=  get_fsen(i);
00255         }
00256         wait_ms(50);
00257     }
00258     for(int i=0;i<4;i++)
00259     {
00260         base_fsen[i] =  base_fsen[i] / 10;
00261     }
00262 #if DBG
00263     pc.printf("[0]:%05d[1]:%05d[2]:%05d[3]:%05d\n\r",base_fsen[0],base_fsen[1],base_fsen[2],base_fsen[3]);
00264 #endif
00265 }
00266 
00267 int get_line(int num)
00268 {
00269     int in = get_fsen(num);
00270     int ret = 0;
00271     
00272 #if 1
00273     if(in > 700)
00274 #else
00275     if( (in > (base_fsen[num] + 200))||(in < (base_fsen[num] - 200)))
00276 #endif
00277     {
00278         ret = 1;
00279     }
00280     return(ret);
00281 }
00282 
00283 void line(void)
00284 {
00285     ModeLed = 0;
00286     wait(1);
00287     while(sw1 != 0)
00288     {
00289 #if 0
00290         int line = get_line(0) ? 0 : 1;
00291             line |= get_line(1) ? 0 : 2;
00292             line |= get_line(2) ? 0 : 4;
00293             line |= get_line(3) ? 0 : 8;
00294 #else
00295         int line = get_line(0) ? 1 : 0;
00296             line |= get_line(1) ? 2 : 0;
00297             line |= get_line(2) ? 4 : 0;
00298             line |= get_line(3) ? 8 : 0;
00299 #endif
00300 
00301 #if DBG
00302         pc.printf("line=%02x %04x %04x %04x %04x\n\r",line,base_fsen[0],base_fsen[1],base_fsen[2],base_fsen[3]);
00303 #endif
00304 #if 1
00305         switch(line)
00306         {
00307             case 1:                 // ○○○●
00308                 left = 1.0;
00309                 right = -1.0;
00310                 break;
00311             case 3:                 // ○○●●
00312                 left = 1.0;
00313                 right = -0.5;
00314                 break;
00315             case 2:                 // ○○●○
00316                 left = 1.0;
00317                 right = 0.5;
00318                 break;
00319             case 6:                 // ○●●○
00320                 left = 1.0;
00321                 right = 1.0;
00322                 break;
00323             case 4:                 // ○●○○
00324                 left = 0.5;
00325                 right = 1.0;
00326                 break;
00327             case 12:                // ●●○○
00328                 left = -0.5;
00329                 right = 1.0;
00330                 break;
00331             case 8:                 // ●○○○
00332                 left = -1.0;
00333                 right = 1.0;
00334                 break;
00335             default:
00336                 left = 1.0;
00337                 right = 1.0;
00338                 break;
00339         }
00340 #endif
00341     }
00342     ModeLed = 1;
00343     left = 0.0;
00344     right = 0.0;
00345     wait(1);
00346 }
00347 
00348 #if 0
00349 int counter1 = 0;
00350 void p1_rise()
00351 {
00352     if( pin2 == 1 )
00353     {
00354         counter1++;
00355     }
00356     else
00357     {
00358         counter1--;
00359     }
00360 }
00361 #endif
00362 
00363 /**************************************************************************/
00364 /*!
00365     @brief  Program entry point
00366 */
00367 
00368 //#define CONN_INTERVAL 25  /**< connection interval 250ms; in multiples of 0.125ms. (durationInMillis * 1000) / UNIT_0_625_MS; */
00369 //#define CONN_SUP_TIMEOUT  8000 /**< Connection supervisory timeout (6 seconds); in multiples of 0.125ms. */
00370 //#define SLAVE_LATENCY     0
00371 #define TICKER_INTERVAL   2.0f
00372 
00373 // ・MPU6050から値を取得
00374 // ・ループ実行されている関数
00375 void updateValue(void){
00376     // 値を格納する用の配列、変数
00377     float   acData[3];
00378     float   gyData[3];
00379                
00380     //加速度を取得
00381     Timer acTimer;
00382     acTimer.start();
00383     mpu.getAccelero(acData);    //加速度を取得 acDataに格納
00384     acTimer.stop();
00385 //  at = acTimer.read_ms();
00386     acTimer.reset();
00387 
00388     //ジャイロを取得
00389     Timer gyTimer;
00390     gyTimer.start();
00391     mpu.getGyro(gyData);    //ジャイロの値（角加速度）を取得 gyDataに格納
00392     gyTimer.stop();
00393 //  gt = gyTimer.read_ms();
00394     gyTimer.reset();
00395 
00396     //floatの値を合計5桁、小数点以下3桁の形でPCへ送信
00397     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]);   
00398 }
00399 
00400 int test()
00401 {
00402     
00403     ///180601 MPU6050センサの初期化処理
00404     mpu.initialize();
00405 //  mpu.setAcceleroRange(MPU6050_ACCELERO_RANGE_8G);    //加速度センサ 計測レンジ設定（今回は2Gか4Gがよさそう）
00406     mpu.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G); 
00407 //   mpu.setGyroRange(MPU6050_GYRO_RANGE_1000);          //ジャイロセンサ 計測レンジ設定（ここも250か500がよさそう（そんなに早く回転しないので））
00408     mpu.setGyroRange(MPU6050_GYRO_RANGE_250);
00409     ///180601 
00410     
00411     if( mpu.testConnection() ){
00412       //  pc.printf("mpu test:OK\n\r");
00413     }else{
00414       //  pc.printf("mpu test:NG\n\r");
00415     }
00416 
00417     float ticker_ms = (TICKER_INTERVAL / 100.0f);
00418     Ticker ticker;
00419     ticker.attach(periodicCallback, ticker_ms);//0.02f //.2f-sec
00420     
00421      pc.printf("Start!! \n");
00422 
00423     while(true) {
00424             left = 1.0;
00425             right = 1.0;
00426   //          wait(5);   
00427             updateValue();
00428             wait(0.5);
00429     }
00430 }
00431 
00432 
00433 /**************************************************************************/
00434 int main(void)
00435 {
00436     sw1.mode(PullUp);
00437     sw2.mode(PullUp);
00438 
00439     encl1.mode(PullNone);
00440     encl2.mode(PullNone);
00441     encr1.mode(PullNone);
00442     encr2.mode(PullNone);
00443 
00444     ModeLed = 1;
00445     ConnectStateLed = 1;
00446 #if DBG
00447     //pc.baud(921600);
00448     pc.baud(9600);
00449     pc.printf("Start\n\r");
00450 #endif
00451     outlow = 0;
00452     
00453     if(sw2 == 0)
00454     {
00455 //        pin1.mode(PullDown);
00456 //        pin1.rise(&p1_rise);
00457         while(1)
00458         {
00459             //int in1 = pin1;
00460             //int in2 = pin2;
00461             //ModeLed = pin1;
00462             //pc.printf("dat = %d %d\r\n",in1,in2);
00463             base();
00464 #if 0
00465             left = 1.0;
00466             right = 1.0;
00467             wait(5);           
00468             left = -1.0;
00469             right = -1.0;
00470             wait(5);
00471 #endif        
00472         }
00473     }
00474 
00475     // MPU6050 Initialize
00476     mpu.initialize();
00477     mpu.setAcceleroRange(MPU6050_ACCELERO_RANGE_8G);
00478     mpu.setGyroRange(MPU6050_GYRO_RANGE_1000);
00479 
00480     ble.init(); 
00481     ble.onConnection(onConnected);
00482     ble.onDisconnection(onDisconnected);
00483     ble.onDataWritten(onDataWritten);
00484     
00485     /* setup advertising */
00486     ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
00487     ble.setAdvertisingType (GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
00488     ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
00489                                     (const uint8_t *)"mbed WallbotBLE", sizeof("mbed WallbotBLE") - 1);
00490     ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,
00491                                     (const uint8_t *)RCBController_service_uuid, sizeof(RCBController_service_uuid));
00492 
00493     ble.setAdvertisingInterval (160); /* 100ms; in multiples of 0.625ms. */
00494     ble.startAdvertising();
00495 
00496     ble.addService(RCBControllerService);
00497     
00498 
00499     while (true) {
00500         if(sw1 == 0)
00501         {
00502             bValue = 1;
00503             line_mode = 1;
00504             test();       
00505             line_mode = 0;
00506             bValue = 0;
00507         }
00508         ble.waitForEvent();
00509     }
00510 }
00511