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 void line(void)
00286 {
00287     ModeLed = 0;
00288     wait(1);
00289     while(sw1 != 0)
00290     {
00291 #if 0
00292         int line = get_line(0) ? 0 : 1;
00293             line |= get_line(1) ? 0 : 2;
00294             line |= get_line(2) ? 0 : 4;
00295             line |= get_line(3) ? 0 : 8;
00296 #else
00297         int line = get_line(0) ? 1 : 0;
00298             line |= get_line(1) ? 2 : 0;
00299             line |= get_line(2) ? 4 : 0;
00300             line |= get_line(3) ? 8 : 0;
00301 #endif
00302 
00303 #if DBG
00304         pc.printf("line=%02x %04x %04x %04x %04x\n\r",line,base_fsen[0],base_fsen[1],base_fsen[2],base_fsen[3]);
00305 #endif
00306 #if 1
00307         switch(line)
00308         {
00309             case 1:                 // ○○○●
00310                 left = 1.0;
00311                 right = -1.0;
00312                 break;
00313             case 3:                 // ○○●●
00314                 left = 1.0;
00315                 right = -0.5;
00316                 break;
00317             case 2:                 // ○○●○
00318                 left = 1.0;
00319                 right = 0.5;
00320                 break;
00321             case 6:                 // ○●●○
00322                 left = 1.0;
00323                 right = 1.0;
00324                 break;
00325             case 4:                 // ○●○○
00326                 left = 0.5;
00327                 right = 1.0;
00328                 break;
00329             case 12:                // ●●○○
00330                 left = -0.5;
00331                 right = 1.0;
00332                 break;
00333             case 8:                 // ●○○○
00334                 left = -1.0;
00335                 right = 1.0;
00336                 break;
00337             default:
00338                 left = 1.0;
00339                 right = 1.0;
00340                 break;
00341         }
00342 #endif
00343     }
00344     ModeLed = 1;
00345     left = 0.0;
00346     right = 0.0;
00347     wait(1);
00348 }
00349 
00350 #if 0
00351 int counter1 = 0;
00352 void p1_rise()
00353 {
00354     if( pin2 == 1 )
00355     {
00356         counter1++;
00357     }
00358     else
00359     {
00360         counter1--;
00361     }
00362 }
00363 #endif
00364 
00365 /**************************************************************************/
00366 /*!
00367     @brief  Program entry point
00368 */
00369 /**************************************************************************/
00370 int main(void)
00371 {
00372     sw1.mode(PullUp);
00373     sw2.mode(PullUp);
00374 
00375     encl1.mode(PullNone);
00376     encl2.mode(PullNone);
00377     encr1.mode(PullNone);
00378     encr2.mode(PullNone);
00379 
00380     ModeLed = 1;
00381     ConnectStateLed = 1;
00382 #if DBG
00383     //pc.baud(921600);
00384     pc.baud(9600);
00385     pc.printf("Start\n\r");
00386 #endif
00387     outlow = 0;
00388     
00389     if(sw2 == 0)
00390     {
00391 //        pin1.mode(PullDown);
00392 //        pin1.rise(&p1_rise);
00393         while(1)
00394         {
00395             //int in1 = pin1;
00396             //int in2 = pin2;
00397             //ModeLed = pin1;
00398             //pc.printf("dat = %d %d\r\n",in1,in2);
00399             base();
00400 #if 0
00401             left = 1.0;
00402             right = 1.0;
00403             wait(5);           
00404             left = -1.0;
00405             right = -1.0;
00406             wait(5);
00407 #endif        
00408         }
00409     }
00410 
00411     // MPU6050 Initialize
00412     mpu.initialize();
00413     mpu.setAcceleroRange(MPU6050_ACCELERO_RANGE_8G);
00414     mpu.setGyroRange(MPU6050_GYRO_RANGE_1000);
00415 
00416     ble.init(); 
00417     ble.onConnection(onConnected);
00418     ble.onDisconnection(onDisconnected);
00419     ble.onDataWritten(onDataWritten);
00420     
00421     /* setup advertising */
00422     ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
00423     ble.setAdvertisingType (GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
00424     ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
00425                                     (const uint8_t *)"mbed WallbotBLE", sizeof("mbed WallbotBLE") - 1);
00426     ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,
00427                                     (const uint8_t *)RCBController_service_uuid, sizeof(RCBController_service_uuid));
00428 
00429     ble.setAdvertisingInterval (160); /* 100ms; in multiples of 0.625ms. */
00430     ble.startAdvertising();
00431 
00432     ble.addService(RCBControllerService);
00433     
00434 
00435     while (true) {
00436         if(sw1 == 0)
00437         {
00438             bValue = 1;
00439             line_mode = 1;
00440             line();       
00441             line_mode = 0;
00442             bValue = 0;
00443         }
00444         ble.waitForEvent();
00445     }
00446 }
00447