Wallbot_CaaS
Dependencies: MPU6050 mbed PID
Fork of BLE_MPU6050_test6_challenge_sb by
main.cpp
- Committer:
- c201075
- Date:
- 2018-05-17
- Revision:
- 5:eeabd90b6d62
- Parent:
- 4:6b4563aaee2c
- Child:
- 6:9fd87d75a24b
File content as of revision 5:eeabd90b6d62:
#include "mbed.h" #include "MPU6050.h" #include "BLEDevice.h" #include "wallbotble.h" #include "RCBController.h" #include "Adafruit_LEDBackpack.h" #include "Adafruit_GFX.h" #include "pictLIB.h" #define DEBUG enum _mode { Nomal = 0, LineFollow, Challenge, CaaS }; Serial pc(USBTX, USBRX); BLEDevice ble; Ticker ticker; MPU6050 mpu; wallbotble wb; myI2C i2c(P0_22,P0_21); //Adafruit_8x8mat96rix matrix1 = Adafruit_8x8matrix(&i2c); //Adafruit_8x8matrix matrix2 = Adafruit_8x8matrix(&i2c); //PICTLIB pl(16,16); int mode = Nomal; char bValue = 0; /* RCBController Service */ static const uint16_t RCBController_service_uuid = 0xFFF0; static const uint16_t RCBController_Characteristic_uuid = 0xFFF1; static const uint16_t RCBController_b_Characteristic_uuid = 0xFFF3; uint8_t RCBControllerPayload[10] = {0,}; GattCharacteristic ControllerChar (RCBController_Characteristic_uuid,RCBControllerPayload,10, 10, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE); //static uint8_t _bValue = 0x00; static uint8_t _mValue[10] = {0,}; GattCharacteristic b_Char(RCBController_b_Characteristic_uuid, _mValue, sizeof(_mValue), sizeof(_mValue), GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY); GattCharacteristic *ControllerChars[] = {&ControllerChar,&b_Char}; GattService RCBControllerService(RCBController_service_uuid, ControllerChars, sizeof(ControllerChars) / sizeof(GattCharacteristic *)); RCBController controller; void SendMessage(char *msg) { if (!ble.getGapState().connected) { return; } int len = strlen(msg); if(len < 10) { strcpy( (char*)_mValue ,msg ); ble.updateCharacteristicValue(b_Char.getValueAttribute().getHandle(), (uint8_t *)_mValue, sizeof(_mValue)); } } void onConnected(Gap::Handle_t handle, Gap::addr_type_t peerAddrType ,const Gap::address_t peerAddr,const Gap::ConnectionParams_t *params) { wb.set_led2(1); #ifdef DEBUG pc.printf("Connected\n\r"); #endif } void onDisconnected(Gap::Handle_t handle, Gap::DisconnectionReason_t reason) { wb.stop(); ble.startAdvertising(); wb.set_led2(0); #ifdef DEBUG pc.printf("Disconnected\n\r"); #endif } void periodicCallback(void) { if (!ble.getGapState().connected) { return; } if( (bValue == 0)&&(wb.GetLinePosition() != 0) ) { // Game over wb.stop(); bValue = 10; } if( bValue > 0 ) { wb.stop(); SendMessage("GAME OVER"); bValue--; if( bValue == 0 ) { wb.set_led1(0); SendMessage(" NOMAL "); mode = Nomal; ticker.detach(); } } } // GattEvent void onDataWritten(const GattCharacteristicWriteCBParams *params) { if( (params->charHandle == ControllerChar.getValueAttribute().getHandle()) && ( mode != LineFollow )) { float right_factor; float left_factor; memcpy( &controller.data[0], params->data , params->len ); #ifdef DEBUG 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], controller.data[5],controller.data[6],controller.data[7],controller.data[8],controller.data[9]); #endif if(mode == Challenge) { float factor = ((float)((int)controller.status.AcceleX -128) / 128.0); float right_factor = ((factor <= 0.0) ? 1.0 : 1.0 - (factor*2)); float left_factor = ((factor >= 0.0) ? 1.0 : 1.0 - (-factor*2)); if( /*controller.status.B ==*/ 1 ) { wb.left_motor(left_factor); wb.right_motor(right_factor); } else if( controller.status.A == 1 ) { wb.left_motor(-right_factor); wb.right_motor(-left_factor); } else { wb.forward(1.0); } } else { if( (controller.status.LeftAnalogUD != 128)||(controller.status.RightAnalogUD != 128) ) { left_factor = ((float)((int)controller.status.LeftAnalogUD -128) / 128.0); right_factor = ((float)((int)controller.status.RightAnalogUD -128) / 128.0); wb.left_motor(left_factor); wb.right_motor(right_factor); } else { float factor = ((float)((int)controller.status.AcceleX -128) / 128.0); right_factor = ((factor <= 0.0) ? 1.0 : 1.0 - (factor*2)); left_factor = ((factor >= 0.0) ? 1.0 : 1.0 - (-factor*2)); if( controller.status.B == 1 ) { wb.left_motor(left_factor); wb.right_motor(right_factor); } else if( controller.status.A == 1 ) { wb.left_motor(-right_factor); wb.right_motor(-left_factor); } else if( controller.status.UP == 1 ) { wb.forward(1.0); } else if( controller.status.DOWN == 1 ) { wb.backward(1.0); } else if( controller.status.RIGHT == 1 ) { wb.right_turn(1.0); } else if( controller.status.LEFT == 1 ) { wb.left_turn(1.0); } else { wb.stop(); } if((controller.status.UP == 1)&&(controller.status.DOWN == 1)) { // START BUTTON SendMessage("Challenge"); wb.set_led1(1); mode = Challenge; wb.stop(); ticker.attach(periodicCallback, 0.1); } } } } } /**************************************************************************/ /*! @brief Program entry point */ /**************************************************************************/ int main(void) { int16_t ax, ay, az; int16_t gx, gy, gz; //mpu.initialize(); //wb.auto_calibrate(); #ifdef DEBUG pc.baud(115200); pc.printf("Start\n\r"); #endif ble.init(); //イベント時のコールバック関数 ble.onConnection(onConnected); ble.onDisconnection(onDisconnected); ble.onDataWritten(onDataWritten); /* setup advertising */ //クラシックBTはサポートせず、BLEデバイスとして認識してもらう ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED); //デバイスがセントラルに接続可能であることを設定 ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED); //LOCAL NAMEの設定、BLEでは終端記号は不要なので-1する ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME, (const uint8_t *)"mbed WallbotBLE", sizeof("mbed WallbotBLE") - 1); //16Bit短縮UUIDの設定 ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (const uint8_t *)RCBController_service_uuid, sizeof(RCBController_service_uuid)); //Advertiseパケットの送信周期 ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */ //サービスの登録 ble.addService(RCBControllerService); //Advertizeの開始 ble.startAdvertising(); //wb.move(30,30); pc.printf("auto calibrate start\n\r"); wb.auto_calibrate(); for(int i = 0 ; i < 4 ; i++) { pc.printf("(%d,%d) ",wb._calibratedMinimum[i],wb._calibratedMaximum[i]); } pc.printf("\n\rauto calibrate end\n\r"); #if 0 //エンコーダキャリブレーション用コード BusIn enc(P0_8,P0_10,P0_6,P0_7); enc.mode(PullNone); while(1){ char c = enc.read(); pc.putc(c); wait_ms(10); } #endif while (true) { //mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); #ifdef DEBUG pc.printf("Pulse(%d,%d) RPM(%.2f,%.2f) ",wb._left_pulses,wb._right_pulses,wb.get_left_rpm(),wb.get_right_rpm()); pc.printf("LinePos:%d ",wb.GetLinePosition()); pc.printf("calib(%d,%d,%d,%d)",wb.sensor_values[0],wb.sensor_values[1],wb.sensor_values[2],wb.sensor_values[3]); //pc.printf("MPU6050(%d;%d;%d;%d;%d;%d)\n\r",ax,ay,az,gx,gy,gz); pc.printf("\n\r"); wait_ms(10); #endif } }