main.cpp

00001 #include "mbed.h"
00002 #include "BLEDevice.h"
00003 #include "RCBController.h"
00004 #include "TB6612.h"
00005 
00006 #define DBG 0
00007 
00008 BLEDevice  ble;
00009 #if DBG
00010 Serial  pc(USBTX, USBRX);
00011 #endif
00012 /* LEDs for indication: */
00013 DigitalOut  ModeLed(P0_19);
00014 DigitalOut  ConnectStateLed(P0_18);
00015 DigitalOut  outlow(P0_20);
00016 //PwmOut  ControllerStateLed(LED2);
00017 
00018 AnalogIn fsen1(P0_2);
00019 AnalogIn fsen2(P0_3);
00020 AnalogIn fsen3(P0_4);
00021 AnalogIn fsen4(P0_5);
00022 #if 1
00023 TB6612 left(P0_29,P0_23,P0_24);
00024 TB6612 right(P0_28,P0_30,P0_0);
00025 #else
00026 TB6612 left(P0_29,P0_24,P0_23);
00027 TB6612 right(P0_28,P0_0,P0_30);
00028 #endif
00029 Ticker ticker;
00030 
00031 DigitalIn   sw1(P0_16);
00032 DigitalIn   sw2(P0_17);
00033 
00034 DigitalIn encl1(P0_6);
00035 DigitalIn encl2(P0_7);
00036 DigitalIn encr1(P0_8);
00037 DigitalIn encr2(P0_10);
00038 
00039 
00040 int base_fsen[4];
00041 int line_mode = 0;
00042 int challenge_mode = 0;
00043 char bValue = 0;
00044 
00045 int get_line(int num);
00046 
00047 /* RCBController Service */
00048 static const uint16_t RCBController_service_uuid = 0xFFF0;
00049 static const uint16_t RCBController_Characteristic_uuid = 0xFFF1;
00050 static const uint16_t RCBController_b_Characteristic_uuid = 0xFFF3;
00051 uint8_t RCBControllerPayload[10] = {0,};
00052 
00053 GattCharacteristic  ControllerChar (RCBController_Characteristic_uuid,RCBControllerPayload,10, 10,
00054                                 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | 
00055                                 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
00056 //static uint8_t _bValue = 0x00;
00057 static uint8_t _mValue[10] = {0,};
00058 GattCharacteristic b_Char(RCBController_b_Characteristic_uuid, _mValue, sizeof(_mValue), sizeof(_mValue),
00059                        GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
00060 
00061 GattCharacteristic *ControllerChars[] = {&ControllerChar,&b_Char};
00062 GattService         RCBControllerService(RCBController_service_uuid, ControllerChars, sizeof(ControllerChars) / sizeof(GattCharacteristic *));
00063 
00064 RCBController controller;
00065 
00066 void onConnected(Gap::Handle_t handle, const Gap::ConnectionParams_t *params)
00067 {
00068     ConnectStateLed = 0;
00069 #if DBG
00070     pc.printf("Connected\n\r");
00071 #endif
00072 }
00073 
00074 void onDisconnected(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
00075 {
00076     left = 0;
00077     right = 0;
00078 
00079     ble.startAdvertising();
00080     ConnectStateLed = 1;
00081 #if DBG
00082     pc.printf("Disconnected\n\r");
00083 #endif
00084 }
00085 
00086 void periodicCallback(void)
00087 {
00088     if (!ble.getGapState().connected) {
00089         return;
00090     }
00091     int line = get_line(0) ? 1 : 0;
00092         line |= get_line(1) ? 2 : 0;
00093         line |= get_line(2) ? 4 : 0;
00094         line |= get_line(3) ? 8 : 0;
00095     if( (bValue == 0)&&(line != 0) )
00096     {
00097         // game over
00098         left = 0.0;
00099         right = 0.0;
00100         bValue = 10;  
00101     }
00102     if( bValue > 0 )
00103     {
00104         memcpy( _mValue , "GAME OVER",10);
00105         ble.updateCharacteristicValue (b_Char.getValueAttribute().getHandle(), (uint8_t *)_mValue, sizeof(_mValue));
00106         ModeLed = !ModeLed;
00107         bValue--;
00108         if( bValue == 0 )
00109         {
00110             ModeLed = 1;
00111             challenge_mode = 0;
00112             ticker.detach();
00113         }
00114     }
00115 }
00116 
00117 
00118 // GattEvent
00119 void onDataWritten(const GattCharacteristicWriteCBParams *params)
00120 {
00121     if( (params->charHandle == ControllerChar.getValueAttribute().getHandle()) && (line_mode == 0))
00122     {
00123         memcpy( &controller.data[0], params->data , params->len );
00124         //memcpy( &controller.data[0], RCBControllerPayload, sizeof(controller));
00125 #if DBG
00126 
00127         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],
00128                                                                controller.data[5],controller.data[6],controller.data[7],controller.data[8],controller.data[9]);
00129 #endif
00130         float right_factor;
00131         float left_factor;
00132 
00133         left_factor = ((float)((int)controller.status.LeftAnalogUD -128) / 128.0);
00134         right_factor = ((float)((int)controller.status.RightAnalogUD -128) / 128.0);
00135         
00136         if(challenge_mode == 1)
00137         {
00138             if( bValue == 0 )
00139             {
00140                 float factor = ((float)((int)controller.status.AcceleX -128) / 128.0); 
00141     
00142                 float right_factor = ((factor <= 0.0) ? 1.0 : 1.0 - (factor*2));
00143                 float left_factor = ((factor >= 0.0) ? 1.0 : 1.0 - (-factor*2));
00144     
00145                 if( controller.status.B == 1 )
00146                 {
00147                     left = left_factor;
00148                     right = right_factor;
00149                 }
00150                 else if( controller.status.A == 1 )
00151                 {
00152                     left = -right_factor;
00153                     right = -left_factor;
00154                 }
00155                 else
00156                 {
00157                     left = 0;
00158                     right = 0;
00159                 }
00160             }
00161         }
00162         else if( (left_factor != 0.0)||(right_factor != 0.0) )
00163         {
00164             left = left_factor;
00165             right = right_factor;
00166         }
00167         else
00168         {
00169             float factor = ((float)((int)controller.status.AcceleX -128) / 128.0); 
00170 
00171             float right_factor = ((factor <= 0.0) ? 1.0 : 1.0 - (factor*2));
00172             float left_factor = ((factor >= 0.0) ? 1.0 : 1.0 - (-factor*2));
00173 
00174             if( controller.status.B == 1 )
00175             {
00176                 left = left_factor;
00177                 right = right_factor;
00178             }
00179             else if( controller.status.A == 1 )
00180             {
00181                 left = -right_factor;
00182                 right = -left_factor;
00183             }
00184             else if( controller.status.UP == 1 )
00185             {
00186                 left = 1.0;
00187                 right = 1.0;
00188             }
00189             else if( controller.status.DOWN == 1 )
00190             {
00191                 left = -1.0;
00192                 right = -1.0;
00193             }
00194             else if( controller.status.RIGHT == 1 )
00195             {
00196                 left = 1.0;
00197                 right = -1.0;
00198             }
00199             else if( controller.status.LEFT == 1 )
00200             {
00201                 left = -1.0;
00202                 right = 1.0;
00203             }
00204             else
00205             {
00206                 left = 0.0;
00207                 right = 0.0;    
00208             }
00209             if((controller.status.UP == 1)&&(controller.status.DOWN == 1))
00210             {
00211                 left = 0.0;
00212                 right = 0.0;    
00213                 ModeLed = 0;
00214                 challenge_mode = 1;
00215                 ticker.attach(periodicCallback, 0.1);
00216 
00217             }
00218         }
00219         //ControllerStateLed = (float)controller.status.LeftAnalogLR / 255.0;            
00220     }
00221 }
00222 
00223 int get_fsen(int num)
00224 {
00225     switch(num)
00226     {
00227     case 0: 
00228         return((int)fsen1.read_u16());
00229     case 1: 
00230         return((int)fsen2.read_u16());
00231     case 2: 
00232         return((int)fsen3.read_u16());
00233     case 3: 
00234         return((int)fsen4.read_u16());
00235     }
00236     return(0);
00237 }
00238 
00239 void base()
00240 {
00241     wait(0.5);
00242     
00243     for(int i=0;i<4;i++)
00244     {
00245         base_fsen[i] = 0;
00246     }
00247     
00248     for(int j=0;j<10;j++)
00249     {
00250         for(int i=0;i<4;i++)
00251         {
00252             base_fsen[i] +=  get_fsen(i);
00253         }
00254         wait_ms(50);
00255     }
00256     for(int i=0;i<4;i++)
00257     {
00258         base_fsen[i] =  base_fsen[i] / 10;
00259     }
00260 #if DBG
00261     pc.printf("[0]:%05d[1]:%05d[2]:%05d[3]:%05d\n\r",base_fsen[0],base_fsen[1],base_fsen[2],base_fsen[3]);
00262 #endif
00263 }
00264 
00265 int get_line(int num)
00266 {
00267     int in = get_fsen(num);
00268     int ret = 0;
00269     
00270 #if 1
00271     if(in > 700)
00272 #else
00273     if( (in > (base_fsen[num] + 200))||(in < (base_fsen[num] - 200)))
00274 #endif
00275     {
00276         ret = 1;
00277     }
00278     return(ret);
00279 }
00280 
00281 void line(void)
00282 {
00283     ModeLed = 0;
00284     wait(1);
00285     while(sw1 != 0)
00286     {
00287 #if 0
00288         int line = get_line(0) ? 0 : 1;
00289             line |= get_line(1) ? 0 : 2;
00290             line |= get_line(2) ? 0 : 4;
00291             line |= get_line(3) ? 0 : 8;
00292 #else
00293         int line = get_line(0) ? 1 : 0;
00294             line |= get_line(1) ? 2 : 0;
00295             line |= get_line(2) ? 4 : 0;
00296             line |= get_line(3) ? 8 : 0;
00297 #endif
00298 
00299 #if DBG
00300         pc.printf("line=%02x %04x %04x %04x %04x\n\r",line,base_fsen[0],base_fsen[1],base_fsen[2],base_fsen[3]);
00301 #endif
00302 #if 1
00303         switch(line)
00304         {
00305             case 1:                 // ○○○●
00306                 left = 1.0;
00307                 right = -1.0;
00308                 break;
00309             case 3:                 // ○○●●
00310                 left = 1.0;
00311                 right = -0.5;
00312                 break;
00313             case 2:                 // ○○●○
00314                 left = 1.0;
00315                 right = 0.5;
00316                 break;
00317             case 6:                 // ○●●○
00318                 left = 1.0;
00319                 right = 1.0;
00320                 break;
00321             case 4:                 // ○●○○
00322                 left = 0.5;
00323                 right = 1.0;
00324                 break;
00325             case 12:                // ●●○○
00326                 left = -0.5;
00327                 right = 1.0;
00328                 break;
00329             case 8:                 // ●○○○
00330                 left = -1.0;
00331                 right = 1.0;
00332                 break;
00333             default:
00334                 left = 1.0;
00335                 right = 1.0;
00336                 break;
00337         }
00338 #endif
00339     }
00340     ModeLed = 1;
00341     left = 0.0;
00342     right = 0.0;
00343     wait(1);
00344 }
00345 
00346 #if 0
00347 int counter1 = 0;
00348 void p1_rise()
00349 {
00350     if( pin2 == 1 )
00351     {
00352         counter1++;
00353     }
00354     else
00355     {
00356         counter1--;
00357     }
00358 }
00359 #endif
00360 
00361 /**************************************************************************/
00362 /*!
00363     @brief  Program entry point
00364 */
00365 /**************************************************************************/
00366 int main(void)
00367 {
00368     sw1.mode(PullUp);
00369     sw2.mode(PullUp);
00370 
00371     encl1.mode(PullNone);
00372     encl2.mode(PullNone);
00373     encr1.mode(PullNone);
00374     encr2.mode(PullNone);
00375 
00376     ModeLed = 1;
00377     ConnectStateLed = 1;
00378 #if DBG
00379     //pc.baud(921600);
00380     pc.baud(9600);
00381     pc.printf("Start\n\r");
00382 #endif
00383     outlow = 0;
00384     
00385     if(sw2 == 0)
00386     {
00387 //        pin1.mode(PullDown);
00388 //        pin1.rise(&p1_rise);
00389         while(1)
00390         {
00391             //int in1 = pin1;
00392             //int in2 = pin2;
00393             //ModeLed = pin1;
00394             //pc.printf("dat = %d %d\r\n",in1,in2);
00395             base();
00396 #if 0
00397             left = 1.0;
00398             right = 1.0;
00399             wait(5);           
00400             left = -1.0;
00401             right = -1.0;
00402             wait(5);
00403 #endif        
00404         }
00405     }
00406 
00407     ble.init(); 
00408     ble.onConnection(onConnected);
00409     ble.onDisconnection(onDisconnected);
00410     ble.onDataWritten(onDataWritten);
00411     
00412     /* setup advertising */
00413     ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
00414     ble.setAdvertisingType (GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
00415     ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
00416                                     (const uint8_t *)"mbed WallbotBLE", sizeof("mbed WallbotBLE") - 1);
00417     ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,
00418                                     (const uint8_t *)RCBController_service_uuid, sizeof(RCBController_service_uuid));
00419 
00420     ble.setAdvertisingInterval (160); /* 100ms; in multiples of 0.625ms. */
00421     ble.startAdvertising();
00422 
00423     ble.addService(RCBControllerService);
00424     
00425 
00426     while (true) {
00427         if(sw1 == 0)
00428         {
00429             bValue = 1;
00430             line_mode = 1;
00431             line();       
00432             line_mode = 0;
00433             bValue = 0;
00434         }
00435         ble.waitForEvent();
00436     }
00437 }
00438