main.cpp

00001 #include "mbed.h"
00002 #include "MPU6050.h"
00003 #include "BLEDevice.h"
00004 #include "wallbotble.h"
00005 #include "RCBController.h"
00006 #include "Adafruit_LEDBackpack.h"
00007 #include "Adafruit_GFX.h"
00008 #include "pictLIB.h"
00009 
00010 
00011 #define DBG 0
00012 
00013 enum _mode {
00014     Nomal = 0,      // ????[?h
00015     LineFollow,     // ???C???g???[?X???[?h
00016     Challenge,      // ?`???????W???[?h        
00017 };
00018 
00019 Serial pc(USBTX, USBRX);
00020 BLEDevice  ble;
00021 Ticker ticker;
00022 MPU6050 mpu;
00023 wallbotble wb;
00024 
00025 myI2C i2c(P0_22,P0_21);
00026 
00027 Adafruit_8x8matrix matrix1 = Adafruit_8x8matrix(&i2c);
00028 Adafruit_8x8matrix matrix2 = Adafruit_8x8matrix(&i2c);
00029 
00030 PICTLIB pl(16,16);
00031 
00032 int mode = Nomal;
00033 
00034 char bValue = 0;
00035 
00036 #if 1
00037 char data[16][16] =
00038 {
00039 {0,0,0,0,0,0,0,1,1,0,0,0,0,0,0,0},
00040 {0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0},
00041 {0,0,0,0,0,1,1,1,1,1,1,0,0,0,0,0},
00042 {0,0,0,0,1,1,1,1,1,1,1,1,0,0,0,0},
00043 {0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0},
00044 {0,0,1,1,1,1,1,1,1,1,1,1,1,1,0,0},
00045 {0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0},
00046 {0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0},
00047 {0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0},
00048 {0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0},
00049 {0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0},
00050 {0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0},
00051 {0,0,0,0,0,0,1,1,1,1,0,0,0,0,0,0},
00052 {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
00053 {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
00054 {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0},
00055 };
00056 char data2[16][16] =
00057 {
00058 {0,0,0,0,0,1,1,1,1,1,1,0,0,0,0,0},
00059 {0,0,0,0,1,0,0,0,0,0,0,1,0,0,0,0},
00060 {0,0,0,1,0,0,0,0,0,0,0,0,1,0,0,0},
00061 {0,0,1,0,0,0,0,0,0,0,0,0,0,1,0,0},
00062 {0,1,0,0,0,0,0,0,0,0,0,0,0,0,1,0},
00063 {1,0,0,0,1,0,0,0,0,0,0,1,0,0,0,1},
00064 {1,0,0,0,1,0,0,0,0,0,0,1,0,0,0,1},
00065 {1,0,0,0,1,0,0,0,0,0,0,1,0,0,0,1},
00066 {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
00067 {1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,1},
00068 {1,0,0,0,1,0,0,0,0,0,0,1,0,0,0,1},
00069 {0,1,0,0,0,1,0,0,0,0,1,0,0,0,1,0},
00070 {0,0,1,0,0,0,1,1,1,1,0,0,0,1,0,0},
00071 {0,0,0,1,0,0,0,0,0,0,0,0,1,0,0,0},
00072 {0,0,0,0,1,0,0,0,0,0,0,1,0,0,0,0},
00073 {0,0,0,0,0,1,1,1,1,1,1,0,0,0,0,0},
00074 };
00075 #else
00076 char data[16][16] =
00077 {
00078 {0,0,0,0,1,0,0,0,0,0,0,1,0,0,0,0},
00079 {0,0,0,1,0,1,0,0,0,0,1,0,1,0,0,0},
00080 {0,0,0,1,0,1,0,0,0,0,1,0,1,0,0,0},
00081 {0,0,1,0,0,0,1,0,0,1,0,0,0,1,0,0},
00082 {0,0,1,0,0,0,1,1,1,1,0,0,0,1,0,0},
00083 {0,1,0,0,0,0,0,0,0,0,0,0,0,0,1,0},
00084 {0,1,0,0,0,1,0,0,0,0,1,0,0,0,1,0},
00085 {0,1,0,0,0,1,0,0,0,0,1,0,0,0,1,0},
00086 {0,1,0,0,0,1,0,0,0,0,1,0,0,0,1,0},
00087 {0,1,0,0,0,0,0,1,1,0,0,0,0,0,1,0},
00088 {0,1,0,0,0,0,1,0,0,1,0,0,0,0,1,0},
00089 {0,1,0,0,0,0,0,1,1,0,0,0,0,0,1,0},
00090 {0,1,0,0,1,0,0,1,1,0,0,1,0,0,1,0},
00091 {0,1,0,0,0,1,1,0,0,1,1,0,0,0,1,0},
00092 {0,0,1,0,0,0,0,0,0,0,0,0,0,1,0,0},
00093 {0,0,0,1,1,1,1,1,1,1,1,1,1,0,0,0},
00094 };
00095 #endif
00096 
00097 char    dsp_data[16][16];
00098 char    dsp_data2[16][16];
00099 
00100 /* RCBController Service */
00101 static const uint16_t RCBController_service_uuid = 0xFFF0;
00102 static const uint16_t RCBController_Characteristic_uuid = 0xFFF1;
00103 static const uint16_t RCBController_b_Characteristic_uuid = 0xFFF3;
00104 uint8_t RCBControllerPayload[10] = {0,};
00105 
00106 GattCharacteristic  ControllerChar (RCBController_Characteristic_uuid,RCBControllerPayload,10, 10,
00107                                 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | 
00108                                 GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
00109 //static uint8_t _bValue = 0x00;
00110 static uint8_t _mValue[10] = {0,};
00111 GattCharacteristic b_Char(RCBController_b_Characteristic_uuid, _mValue, sizeof(_mValue), sizeof(_mValue),
00112                        GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
00113 
00114 GattCharacteristic *ControllerChars[] = {&ControllerChar,&b_Char};
00115 GattService         RCBControllerService(RCBController_service_uuid, ControllerChars, sizeof(ControllerChars) / sizeof(GattCharacteristic *));
00116 
00117 RCBController controller;
00118 
00119 
00120 void SendMessage(char *msg)
00121 {
00122     if (!ble.getGapState().connected) {
00123         return;
00124     }
00125     int len = strlen(msg);
00126     
00127     if(len < 10)
00128     {
00129         strcpy( (char*)_mValue ,msg );
00130         ble.updateCharacteristicValue (b_Char.getValueAttribute().getHandle(), (uint8_t *)_mValue, sizeof(_mValue));
00131     }
00132 }
00133 
00134 void onConnected(Gap::Handle_t handle, Gap::addr_type_t peerAddrType ,const Gap::address_t peerAddr,const Gap::ConnectionParams_t *params)
00135 {
00136     wb.set_led2(1);
00137 #if DBG
00138     pc.printf("Connected\n\r");
00139 #endif
00140 }
00141 
00142 void onDisconnected(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
00143 {
00144     wb.stop();
00145 
00146     ble.startAdvertising();
00147     wb.set_led2(0);
00148 #if DBG
00149     pc.printf("Disconnected\n\r");
00150 #endif
00151 }
00152 
00153 void periodicCallback(void)
00154 {
00155     if (!ble.getGapState().connected) {
00156         return;
00157     }
00158     if( (bValue == 0)&&(wb.GetLinePosition() != 0) )
00159     {
00160         // Game over
00161         wb.stop();
00162         bValue = 10;  
00163     }
00164     if( bValue > 0 )
00165     {
00166         wb.stop();
00167         SendMessage("GAME OVER");
00168         bValue--;
00169         if( bValue == 0 )
00170         {
00171             wb.set_led1(0);
00172             SendMessage(" NOMAL  ");
00173             mode = Nomal;
00174             ticker.detach();
00175         }
00176     }
00177 }
00178 
00179 
00180 // GattEvent
00181 void onDataWritten(const GattCharacteristicWriteCBParams *params)
00182 {
00183     if( (params->charHandle == ControllerChar.getValueAttribute().getHandle()) && ( mode != LineFollow ))
00184     {
00185         float right_factor;
00186         float left_factor;
00187 
00188         memcpy( &controller.data[0], params->data , params->len );
00189 #if DBG
00190         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],
00191                                                                controller.data[5],controller.data[6],controller.data[7],controller.data[8],controller.data[9]);
00192 #endif        
00193         if(mode == Challenge)
00194         {
00195             float factor = ((float)((int)controller.status.AcceleX -128) / 128.0); 
00196 
00197             float right_factor = ((factor <= 0.0) ? 1.0 : 1.0 - (factor*2));
00198             float left_factor = ((factor >= 0.0) ? 1.0 : 1.0 - (-factor*2));
00199 
00200             if( /*controller.status.B ==*/ 1 )
00201             {
00202                 wb.left_motor(left_factor);
00203                 wb.right_motor(right_factor);
00204             }
00205             else if( controller.status.A == 1 )
00206             {
00207                 wb.left_motor(-right_factor);
00208                 wb.right_motor(-left_factor);
00209             }
00210             else
00211             {
00212                 wb.forward(1.0);
00213             }
00214         }
00215         else
00216         {
00217             if( (controller.status.LeftAnalogUD != 128)||(controller.status.RightAnalogUD != 128) )
00218             {
00219                 left_factor = ((float)((int)controller.status.LeftAnalogUD -128) / 128.0);
00220                 right_factor = ((float)((int)controller.status.RightAnalogUD -128) / 128.0);
00221 
00222                 wb.left_motor(left_factor);
00223                 wb.right_motor(right_factor);
00224             }
00225             else
00226             {
00227                 float factor = ((float)((int)controller.status.AcceleX -128) / 128.0); 
00228     
00229                 right_factor = ((factor <= 0.0) ? 1.0 : 1.0 - (factor*2));
00230                 left_factor = ((factor >= 0.0) ? 1.0 : 1.0 - (-factor*2));
00231     
00232                 if( controller.status.B == 1 )
00233                 {
00234                     wb.left_motor(left_factor);
00235                     wb.right_motor(right_factor);
00236                 }
00237                 else if( controller.status.A == 1 )
00238                 {
00239                     wb.left_motor(-right_factor);
00240                     wb.right_motor(-left_factor);
00241                 }
00242                 else if( controller.status.UP == 1 )
00243                 {
00244                     wb.forward(1.0);
00245                 }
00246                 else if( controller.status.DOWN == 1 )
00247                 {
00248                     wb.backward(1.0);
00249                 }
00250                 else if( controller.status.RIGHT == 1 )
00251                 {
00252                     wb.right_turn(1.0);
00253                 }
00254                 else if( controller.status.LEFT == 1 )
00255                 {
00256                     wb.left_turn(1.0);
00257                 }
00258                 else
00259                 {
00260                     wb.stop();   
00261                 }
00262                 if((controller.status.UP == 1)&&(controller.status.DOWN == 1))  // START BUTTON
00263                 {
00264                     SendMessage("Challenge");
00265                     wb.set_led1(1);
00266                     mode = Challenge;
00267                     wb.stop();
00268                     ticker.attach(periodicCallback, 0.1);
00269                 }
00270             }
00271         }
00272     }
00273 }
00274 
00275 void line_follow_mode(void)
00276 {
00277     switch(wb.GetLinePosition())
00278     {
00279         case 1:                 // ????????
00280             wb.left_motor(1.0);
00281             wb.right_motor(-1.0);
00282             break;
00283         case 3:                 // ????????
00284             wb.left_motor(1.0);
00285             wb.right_motor(-0.5);
00286             break;
00287         case 2:                 // ????????
00288             wb.left_motor(1.0);
00289             wb.right_motor(0.5);
00290             break;
00291         case 6:                 // ????????
00292             wb.left_motor(1.0);
00293             wb.right_motor(1.0);
00294             break;
00295         case 4:                 // ????????
00296             wb.left_motor(0.5);
00297             wb.right_motor(1.0);
00298             break;
00299         case 12:                // ????????
00300             wb.left_motor(-0.5);
00301             wb.right_motor(1.0);
00302             break;
00303         case 8:                 // ????????
00304             wb.left_motor(-1.0);
00305             wb.right_motor(1.0);
00306             break;
00307         default:
00308             wb.left_motor(1.0);
00309             wb.right_motor(1.0);
00310             break;
00311     }
00312 }
00313 
00314 float read_rad()
00315 {
00316     int16_t ax, ay, az;
00317     int16_t gx, gy, gz;
00318     static float old_rad = 1000.0;
00319 
00320     mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
00321     //pc.printf("%d;%d;%d;%d;%d;%d\n\r",ax,ay,az,gx,gy,gz);
00322     float rad;
00323     if( ax > 0.0 )
00324     {
00325         rad = ((float)ay / 16000.0) * 90.0;    
00326     }
00327     else
00328     {
00329         rad = ((float)ay / 16000.0) * 90.0;
00330         if( ay > 0 )
00331         {
00332             rad = 180 - rad;
00333         }
00334         else
00335         {
00336             rad = -180 - rad;
00337         }
00338     }
00339     int d;
00340     if( rad > 0 )
00341     {
00342         d = (rad + 0.5) / 10;
00343     }
00344     else
00345     {
00346         d = (rad - 0.5) / 10;
00347     }
00348     d *= 10;
00349     rad = -d;
00350     //pc.printf("%f;%d;%d\n\r",rad,ax,ay);
00351 
00352     if( old_rad != 1000.0 )
00353     {
00354         if( (old_rad + 20.0) >= rad)
00355         {
00356             old_rad = rad;    
00357         }
00358         else if( (old_rad - 20.0) <= rad)
00359         {
00360             
00361         }
00362         else
00363         {
00364             rad = old_rad;
00365         }
00366     }
00367     return(rad);
00368 }
00369 
00370 
00371 void drowDisplay()
00372 {
00373     for (int y = 0; y < 8; y++) {
00374       for (int x = 0; x < 16; x++) {
00375         if( dsp_data[x][y+8] == 1 )
00376         {
00377             matrix1.drawPixel(x, y, LED_ON);
00378         }
00379         else
00380         {
00381             matrix1.drawPixel(x, y, LED_OFF);
00382         }
00383         if( dsp_data[x][y] == 1 )
00384         {
00385             matrix2.drawPixel(x, y, LED_ON);
00386         }
00387         else
00388         {
00389             matrix2.drawPixel(x, y, LED_OFF);
00390         }
00391       }
00392     }
00393     matrix1.writeDisplay();  // write the changes we just made to the display
00394     matrix2.writeDisplay();  // write the changes we just made to the display
00395 
00396 }
00397 
00398 /**************************************************************************/
00399 /*!
00400     @brief  Program entry point
00401 */
00402 /**************************************************************************/
00403 int main(void)
00404 {
00405     char   in[ 16 ][ 16 ];  //  
00406     char   in2[ 16 ][ 16 ];  //  
00407     float rad_set = 0.0;
00408     float rad = 0.0;
00409     int count = 0;
00410 
00411     pc.printf("START\n\r");
00412     
00413     mpu.initialize();
00414 #if 1
00415     matrix1.begin(0x71);
00416     matrix2.begin(0x70);
00417 #else
00418     matrix1.begin(0x70);
00419     matrix2.begin(0x74);
00420 #endif
00421     matrix1.setBrightness(15);
00422     matrix2.setBrightness(15);
00423     matrix1.clear();
00424     matrix2.clear();
00425 
00426     for ( int i = 0; i < 16; i++ )
00427         for ( int j = 0; j < 16; j++ )
00428             in[15-i][j] = data[i][j];
00429 
00430     for ( int i = 0; i < 16; i++ )
00431         for ( int j = 0; j < 16; j++ )
00432             dsp_data[i][j] = in[i][j];
00433             
00434     for ( int i = 0; i < 16; i++ )
00435         for ( int j = 0; j < 16; j++ )
00436             in2[15-i][j] = data2[i][j];
00437 
00438     for ( int i = 0; i < 16; i++ )
00439         for ( int j = 0; j < 16; j++ )
00440             dsp_data[i][j] = in2[i][j];
00441             
00442     drowDisplay();
00443 
00444     wb.f_sensor_calibrate();
00445 
00446 #if DBG
00447     pc.baud(9600);
00448     DBG(pc.printf("Start\n\r");)
00449 #endif
00450     
00451     ble.init(); 
00452     ble.onConnection(onConnected);
00453     ble.onDisconnection(onDisconnected);
00454     ble.onDataWritten(onDataWritten);
00455     
00456     /* setup advertising */
00457     ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
00458     ble.setAdvertisingType (GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
00459     ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
00460                                     (const uint8_t *)"mbed WallbotBLE", sizeof("mbed WallbotBLE") - 1);
00461     ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,
00462                                     (const uint8_t *)RCBController_service_uuid, sizeof(RCBController_service_uuid));
00463 
00464     ble.setAdvertisingInterval (160); /* 100ms; in multiples of 0.625ms. */
00465     ble.startAdvertising();
00466 
00467     ble.addService(RCBControllerService);
00468     
00469 
00470     while (true) {
00471         
00472         if(wb.GetSw())
00473         {
00474             if( mode == LineFollow)
00475             {
00476                 wb.stop();
00477                 
00478                 mode = Nomal;
00479                 wb.set_led1(0);
00480             }
00481             else
00482             {
00483                 mode = LineFollow;
00484                 wb.set_led1(1);
00485             }
00486             wait(1.0);
00487         }
00488         if(mode == LineFollow)
00489         {
00490             line_follow_mode();
00491         }
00492         else
00493         {
00494             ble.waitForEvent();
00495         }
00496         if(mode == Challenge)
00497         {
00498             count++;
00499             if(count > 50)
00500             {
00501                 count = 0;
00502                 pl.scroll(&in[0][0],&dsp_data[0][0]);
00503                 drowDisplay();
00504                 for ( int i = 0; i < 16; i++ )
00505                     for ( int j = 0; j < 16; j++ )
00506                         in[i][j] = dsp_data[i][j];
00507             }
00508         }
00509         else
00510         {
00511         rad = read_rad();
00512         
00513         if( (rad > -45.0)&&(rad < 45.0 ) )
00514         {
00515             rad = 0.0;
00516         }
00517         else if( (rad > 45.0)&&(rad < 135.0 ) )
00518         {
00519             rad = 90.0;
00520         }
00521         else if( ((rad > 135.0)&&(rad < 180.0 ))||((rad > -180.0)&&(rad < -135.0 )) )
00522         {
00523             rad = 180.0;
00524         }
00525         else if( (rad > -135.0)&&(rad < -45.0 ) )
00526         {
00527             rad = -90.0;
00528         }
00529         else
00530         {
00531             
00532         }
00533         
00534         if( rad_set > rad )
00535         {
00536             if( (rad_set == 180)&&(rad <0) )
00537             {
00538                 rad_set = -175;
00539             }
00540             else
00541             {
00542                 rad_set -= 5.0;
00543             }
00544         }
00545         else if( rad_set < rad )
00546         {
00547             if( (rad == 180.0)&&(rad_set <0))
00548             {
00549                 rad_set -= 5.0;
00550             }
00551             else
00552             {
00553                 rad_set += 5.0;
00554             }
00555         }
00556         else
00557         {
00558             ;
00559         }
00560         if(rad_set == -180.0)
00561         {
00562             rad_set = 180.0;
00563         }
00564         pl.rotation(rad_set,&in2[0][0],&dsp_data[0][0]);
00565         drowDisplay();
00566         }
00567     }
00568 }
00569