Wallbot_CaaS
Dependencies: MPU6050 mbed PID
Fork of BLE_MPU6050_test6_challenge_sb by
main.cpp@5:eeabd90b6d62, 2018-05-17 (annotated)
- Committer:
- c201075
- Date:
- Thu May 17 01:41:56 2018 +0000
- Revision:
- 5:eeabd90b6d62
- Parent:
- 4:6b4563aaee2c
- Child:
- 6:9fd87d75a24b
???????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jksoft | 0:8468a4403fea | 1 | #include "mbed.h" |
jksoft | 0:8468a4403fea | 2 | #include "MPU6050.h" |
jksoft | 0:8468a4403fea | 3 | #include "BLEDevice.h" |
jksoft | 0:8468a4403fea | 4 | #include "wallbotble.h" |
jksoft | 0:8468a4403fea | 5 | #include "RCBController.h" |
jksoft | 0:8468a4403fea | 6 | #include "Adafruit_LEDBackpack.h" |
jksoft | 0:8468a4403fea | 7 | #include "Adafruit_GFX.h" |
jksoft | 0:8468a4403fea | 8 | #include "pictLIB.h" |
jksoft | 0:8468a4403fea | 9 | |
jksoft | 0:8468a4403fea | 10 | |
c201075 | 4:6b4563aaee2c | 11 | #define DEBUG |
jksoft | 0:8468a4403fea | 12 | |
jksoft | 0:8468a4403fea | 13 | enum _mode { |
c201075 | 4:6b4563aaee2c | 14 | Nomal = 0, |
c201075 | 4:6b4563aaee2c | 15 | LineFollow, |
c201075 | 4:6b4563aaee2c | 16 | Challenge, |
c201075 | 4:6b4563aaee2c | 17 | CaaS |
jksoft | 0:8468a4403fea | 18 | }; |
jksoft | 0:8468a4403fea | 19 | |
jksoft | 0:8468a4403fea | 20 | Serial pc(USBTX, USBRX); |
jksoft | 0:8468a4403fea | 21 | BLEDevice ble; |
jksoft | 0:8468a4403fea | 22 | Ticker ticker; |
jksoft | 0:8468a4403fea | 23 | MPU6050 mpu; |
jksoft | 0:8468a4403fea | 24 | wallbotble wb; |
jksoft | 0:8468a4403fea | 25 | |
jksoft | 0:8468a4403fea | 26 | myI2C i2c(P0_22,P0_21); |
c201075 | 4:6b4563aaee2c | 27 | //Adafruit_8x8mat96rix matrix1 = Adafruit_8x8matrix(&i2c); |
c201075 | 4:6b4563aaee2c | 28 | //Adafruit_8x8matrix matrix2 = Adafruit_8x8matrix(&i2c); |
c201075 | 4:6b4563aaee2c | 29 | //PICTLIB pl(16,16); |
jksoft | 0:8468a4403fea | 30 | |
jksoft | 0:8468a4403fea | 31 | int mode = Nomal; |
jksoft | 0:8468a4403fea | 32 | |
jksoft | 0:8468a4403fea | 33 | char bValue = 0; |
jksoft | 0:8468a4403fea | 34 | |
jksoft | 0:8468a4403fea | 35 | |
jksoft | 0:8468a4403fea | 36 | /* RCBController Service */ |
jksoft | 0:8468a4403fea | 37 | static const uint16_t RCBController_service_uuid = 0xFFF0; |
jksoft | 0:8468a4403fea | 38 | static const uint16_t RCBController_Characteristic_uuid = 0xFFF1; |
jksoft | 0:8468a4403fea | 39 | static const uint16_t RCBController_b_Characteristic_uuid = 0xFFF3; |
jksoft | 0:8468a4403fea | 40 | uint8_t RCBControllerPayload[10] = {0,}; |
jksoft | 0:8468a4403fea | 41 | |
jksoft | 0:8468a4403fea | 42 | GattCharacteristic ControllerChar (RCBController_Characteristic_uuid,RCBControllerPayload,10, 10, |
c201075 | 4:6b4563aaee2c | 43 | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | |
c201075 | 4:6b4563aaee2c | 44 | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE); |
jksoft | 0:8468a4403fea | 45 | //static uint8_t _bValue = 0x00; |
jksoft | 0:8468a4403fea | 46 | static uint8_t _mValue[10] = {0,}; |
jksoft | 0:8468a4403fea | 47 | GattCharacteristic b_Char(RCBController_b_Characteristic_uuid, _mValue, sizeof(_mValue), sizeof(_mValue), |
c201075 | 4:6b4563aaee2c | 48 | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_READ | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY); |
jksoft | 0:8468a4403fea | 49 | |
jksoft | 0:8468a4403fea | 50 | GattCharacteristic *ControllerChars[] = {&ControllerChar,&b_Char}; |
jksoft | 0:8468a4403fea | 51 | GattService RCBControllerService(RCBController_service_uuid, ControllerChars, sizeof(ControllerChars) / sizeof(GattCharacteristic *)); |
jksoft | 0:8468a4403fea | 52 | |
jksoft | 0:8468a4403fea | 53 | RCBController controller; |
jksoft | 0:8468a4403fea | 54 | |
jksoft | 0:8468a4403fea | 55 | |
jksoft | 0:8468a4403fea | 56 | void SendMessage(char *msg) |
jksoft | 0:8468a4403fea | 57 | { |
jksoft | 0:8468a4403fea | 58 | if (!ble.getGapState().connected) { |
jksoft | 0:8468a4403fea | 59 | return; |
jksoft | 0:8468a4403fea | 60 | } |
jksoft | 0:8468a4403fea | 61 | int len = strlen(msg); |
c201075 | 4:6b4563aaee2c | 62 | |
c201075 | 4:6b4563aaee2c | 63 | if(len < 10) { |
jksoft | 0:8468a4403fea | 64 | strcpy( (char*)_mValue ,msg ); |
jksoft | 0:8468a4403fea | 65 | ble.updateCharacteristicValue(b_Char.getValueAttribute().getHandle(), (uint8_t *)_mValue, sizeof(_mValue)); |
jksoft | 0:8468a4403fea | 66 | } |
jksoft | 0:8468a4403fea | 67 | } |
jksoft | 0:8468a4403fea | 68 | |
jksoft | 0:8468a4403fea | 69 | void onConnected(Gap::Handle_t handle, Gap::addr_type_t peerAddrType ,const Gap::address_t peerAddr,const Gap::ConnectionParams_t *params) |
jksoft | 0:8468a4403fea | 70 | { |
jksoft | 0:8468a4403fea | 71 | wb.set_led2(1); |
c201075 | 4:6b4563aaee2c | 72 | #ifdef DEBUG |
jksoft | 0:8468a4403fea | 73 | pc.printf("Connected\n\r"); |
jksoft | 0:8468a4403fea | 74 | #endif |
jksoft | 0:8468a4403fea | 75 | } |
jksoft | 0:8468a4403fea | 76 | |
jksoft | 0:8468a4403fea | 77 | void onDisconnected(Gap::Handle_t handle, Gap::DisconnectionReason_t reason) |
jksoft | 0:8468a4403fea | 78 | { |
jksoft | 0:8468a4403fea | 79 | wb.stop(); |
jksoft | 0:8468a4403fea | 80 | |
jksoft | 0:8468a4403fea | 81 | ble.startAdvertising(); |
jksoft | 0:8468a4403fea | 82 | wb.set_led2(0); |
c201075 | 4:6b4563aaee2c | 83 | #ifdef DEBUG |
jksoft | 0:8468a4403fea | 84 | pc.printf("Disconnected\n\r"); |
jksoft | 0:8468a4403fea | 85 | #endif |
jksoft | 0:8468a4403fea | 86 | } |
jksoft | 0:8468a4403fea | 87 | |
jksoft | 0:8468a4403fea | 88 | void periodicCallback(void) |
jksoft | 0:8468a4403fea | 89 | { |
jksoft | 0:8468a4403fea | 90 | if (!ble.getGapState().connected) { |
jksoft | 0:8468a4403fea | 91 | return; |
jksoft | 0:8468a4403fea | 92 | } |
c201075 | 4:6b4563aaee2c | 93 | if( (bValue == 0)&&(wb.GetLinePosition() != 0) ) { |
jksoft | 0:8468a4403fea | 94 | // Game over |
jksoft | 0:8468a4403fea | 95 | wb.stop(); |
c201075 | 4:6b4563aaee2c | 96 | bValue = 10; |
jksoft | 0:8468a4403fea | 97 | } |
c201075 | 4:6b4563aaee2c | 98 | if( bValue > 0 ) { |
jksoft | 0:8468a4403fea | 99 | wb.stop(); |
jksoft | 0:8468a4403fea | 100 | SendMessage("GAME OVER"); |
jksoft | 0:8468a4403fea | 101 | bValue--; |
c201075 | 4:6b4563aaee2c | 102 | if( bValue == 0 ) { |
jksoft | 0:8468a4403fea | 103 | wb.set_led1(0); |
jksoft | 0:8468a4403fea | 104 | SendMessage(" NOMAL "); |
jksoft | 0:8468a4403fea | 105 | mode = Nomal; |
jksoft | 0:8468a4403fea | 106 | ticker.detach(); |
jksoft | 0:8468a4403fea | 107 | } |
jksoft | 0:8468a4403fea | 108 | } |
jksoft | 0:8468a4403fea | 109 | } |
jksoft | 0:8468a4403fea | 110 | |
jksoft | 0:8468a4403fea | 111 | |
jksoft | 0:8468a4403fea | 112 | // GattEvent |
jksoft | 0:8468a4403fea | 113 | void onDataWritten(const GattCharacteristicWriteCBParams *params) |
jksoft | 0:8468a4403fea | 114 | { |
c201075 | 4:6b4563aaee2c | 115 | if( (params->charHandle == ControllerChar.getValueAttribute().getHandle()) && ( mode != LineFollow )) { |
jksoft | 0:8468a4403fea | 116 | float right_factor; |
jksoft | 0:8468a4403fea | 117 | float left_factor; |
jksoft | 0:8468a4403fea | 118 | |
jksoft | 0:8468a4403fea | 119 | memcpy( &controller.data[0], params->data , params->len ); |
c201075 | 4:6b4563aaee2c | 120 | #ifdef DEBUG |
jksoft | 0:8468a4403fea | 121 | 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], |
c201075 | 4:6b4563aaee2c | 122 | controller.data[5],controller.data[6],controller.data[7],controller.data[8],controller.data[9]); |
c201075 | 4:6b4563aaee2c | 123 | #endif |
c201075 | 4:6b4563aaee2c | 124 | |
c201075 | 4:6b4563aaee2c | 125 | if(mode == Challenge) { |
c201075 | 4:6b4563aaee2c | 126 | float factor = ((float)((int)controller.status.AcceleX -128) / 128.0); |
jksoft | 0:8468a4403fea | 127 | |
jksoft | 0:8468a4403fea | 128 | float right_factor = ((factor <= 0.0) ? 1.0 : 1.0 - (factor*2)); |
jksoft | 0:8468a4403fea | 129 | float left_factor = ((factor >= 0.0) ? 1.0 : 1.0 - (-factor*2)); |
jksoft | 0:8468a4403fea | 130 | |
c201075 | 4:6b4563aaee2c | 131 | if( /*controller.status.B ==*/ 1 ) { |
jksoft | 0:8468a4403fea | 132 | wb.left_motor(left_factor); |
jksoft | 0:8468a4403fea | 133 | wb.right_motor(right_factor); |
c201075 | 4:6b4563aaee2c | 134 | } else if( controller.status.A == 1 ) { |
jksoft | 0:8468a4403fea | 135 | wb.left_motor(-right_factor); |
jksoft | 0:8468a4403fea | 136 | wb.right_motor(-left_factor); |
c201075 | 4:6b4563aaee2c | 137 | } else { |
c201075 | 4:6b4563aaee2c | 138 | wb.forward(1.0); |
jksoft | 0:8468a4403fea | 139 | } |
c201075 | 4:6b4563aaee2c | 140 | } else { |
c201075 | 4:6b4563aaee2c | 141 | if( (controller.status.LeftAnalogUD != 128)||(controller.status.RightAnalogUD != 128) ) { |
jksoft | 0:8468a4403fea | 142 | left_factor = ((float)((int)controller.status.LeftAnalogUD -128) / 128.0); |
jksoft | 0:8468a4403fea | 143 | right_factor = ((float)((int)controller.status.RightAnalogUD -128) / 128.0); |
jksoft | 0:8468a4403fea | 144 | |
jksoft | 0:8468a4403fea | 145 | wb.left_motor(left_factor); |
jksoft | 0:8468a4403fea | 146 | wb.right_motor(right_factor); |
c201075 | 4:6b4563aaee2c | 147 | } else { |
c201075 | 4:6b4563aaee2c | 148 | float factor = ((float)((int)controller.status.AcceleX -128) / 128.0); |
c201075 | 4:6b4563aaee2c | 149 | |
jksoft | 0:8468a4403fea | 150 | right_factor = ((factor <= 0.0) ? 1.0 : 1.0 - (factor*2)); |
jksoft | 0:8468a4403fea | 151 | left_factor = ((factor >= 0.0) ? 1.0 : 1.0 - (-factor*2)); |
c201075 | 4:6b4563aaee2c | 152 | |
c201075 | 4:6b4563aaee2c | 153 | if( controller.status.B == 1 ) { |
jksoft | 0:8468a4403fea | 154 | wb.left_motor(left_factor); |
jksoft | 0:8468a4403fea | 155 | wb.right_motor(right_factor); |
c201075 | 4:6b4563aaee2c | 156 | } else if( controller.status.A == 1 ) { |
jksoft | 0:8468a4403fea | 157 | wb.left_motor(-right_factor); |
jksoft | 0:8468a4403fea | 158 | wb.right_motor(-left_factor); |
c201075 | 4:6b4563aaee2c | 159 | } else if( controller.status.UP == 1 ) { |
jksoft | 0:8468a4403fea | 160 | wb.forward(1.0); |
c201075 | 4:6b4563aaee2c | 161 | } else if( controller.status.DOWN == 1 ) { |
jksoft | 0:8468a4403fea | 162 | wb.backward(1.0); |
c201075 | 4:6b4563aaee2c | 163 | } else if( controller.status.RIGHT == 1 ) { |
jksoft | 0:8468a4403fea | 164 | wb.right_turn(1.0); |
c201075 | 4:6b4563aaee2c | 165 | } else if( controller.status.LEFT == 1 ) { |
jksoft | 0:8468a4403fea | 166 | wb.left_turn(1.0); |
c201075 | 4:6b4563aaee2c | 167 | } else { |
c201075 | 4:6b4563aaee2c | 168 | wb.stop(); |
jksoft | 0:8468a4403fea | 169 | } |
c201075 | 4:6b4563aaee2c | 170 | if((controller.status.UP == 1)&&(controller.status.DOWN == 1)) { // START BUTTON |
jksoft | 0:8468a4403fea | 171 | SendMessage("Challenge"); |
jksoft | 0:8468a4403fea | 172 | wb.set_led1(1); |
jksoft | 0:8468a4403fea | 173 | mode = Challenge; |
jksoft | 0:8468a4403fea | 174 | wb.stop(); |
jksoft | 0:8468a4403fea | 175 | ticker.attach(periodicCallback, 0.1); |
jksoft | 0:8468a4403fea | 176 | } |
jksoft | 0:8468a4403fea | 177 | } |
jksoft | 0:8468a4403fea | 178 | } |
jksoft | 0:8468a4403fea | 179 | } |
jksoft | 0:8468a4403fea | 180 | } |
jksoft | 0:8468a4403fea | 181 | |
jksoft | 0:8468a4403fea | 182 | |
jksoft | 0:8468a4403fea | 183 | /**************************************************************************/ |
jksoft | 0:8468a4403fea | 184 | /*! |
jksoft | 0:8468a4403fea | 185 | @brief Program entry point |
jksoft | 0:8468a4403fea | 186 | */ |
jksoft | 0:8468a4403fea | 187 | /**************************************************************************/ |
jksoft | 0:8468a4403fea | 188 | int main(void) |
jksoft | 0:8468a4403fea | 189 | { |
jksoft | 0:8468a4403fea | 190 | |
c201075 | 4:6b4563aaee2c | 191 | int16_t ax, ay, az; |
c201075 | 4:6b4563aaee2c | 192 | int16_t gx, gy, gz; |
c201075 | 4:6b4563aaee2c | 193 | |
c201075 | 4:6b4563aaee2c | 194 | //mpu.initialize(); |
jksoft | 0:8468a4403fea | 195 | |
c201075 | 4:6b4563aaee2c | 196 | //wb.auto_calibrate(); |
jksoft | 0:8468a4403fea | 197 | |
c201075 | 4:6b4563aaee2c | 198 | #ifdef DEBUG |
c201075 | 4:6b4563aaee2c | 199 | pc.baud(115200); |
c201075 | 4:6b4563aaee2c | 200 | pc.printf("Start\n\r"); |
c201075 | 4:6b4563aaee2c | 201 | #endif |
jksoft | 0:8468a4403fea | 202 | |
c201075 | 4:6b4563aaee2c | 203 | ble.init(); |
jksoft | 0:8468a4403fea | 204 | |
c201075 | 4:6b4563aaee2c | 205 | //イベント時のコールバック関数 |
jksoft | 0:8468a4403fea | 206 | ble.onConnection(onConnected); |
jksoft | 0:8468a4403fea | 207 | ble.onDisconnection(onDisconnected); |
jksoft | 0:8468a4403fea | 208 | ble.onDataWritten(onDataWritten); |
c201075 | 4:6b4563aaee2c | 209 | |
jksoft | 0:8468a4403fea | 210 | /* setup advertising */ |
c201075 | 4:6b4563aaee2c | 211 | //クラシックBTはサポートせず、BLEデバイスとして認識してもらう |
jksoft | 0:8468a4403fea | 212 | ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED); |
c201075 | 4:6b4563aaee2c | 213 | //デバイスがセントラルに接続可能であることを設定 |
jksoft | 0:8468a4403fea | 214 | ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED); |
c201075 | 4:6b4563aaee2c | 215 | //LOCAL NAMEの設定、BLEでは終端記号は不要なので-1する |
jksoft | 0:8468a4403fea | 216 | ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME, |
c201075 | 4:6b4563aaee2c | 217 | (const uint8_t *)"mbed WallbotBLE", sizeof("mbed WallbotBLE") - 1); |
c201075 | 4:6b4563aaee2c | 218 | //16Bit短縮UUIDの設定 |
jksoft | 0:8468a4403fea | 219 | ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, |
c201075 | 4:6b4563aaee2c | 220 | (const uint8_t *)RCBController_service_uuid, sizeof(RCBController_service_uuid)); |
c201075 | 4:6b4563aaee2c | 221 | //Advertiseパケットの送信周期 |
c201075 | 4:6b4563aaee2c | 222 | ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */ |
jksoft | 0:8468a4403fea | 223 | |
c201075 | 4:6b4563aaee2c | 224 | //サービスの登録 |
c201075 | 4:6b4563aaee2c | 225 | ble.addService(RCBControllerService); |
c201075 | 4:6b4563aaee2c | 226 | |
c201075 | 4:6b4563aaee2c | 227 | //Advertizeの開始 |
jksoft | 0:8468a4403fea | 228 | ble.startAdvertising(); |
jksoft | 0:8468a4403fea | 229 | |
c201075 | 5:eeabd90b6d62 | 230 | //wb.move(30,30); |
c201075 | 5:eeabd90b6d62 | 231 | pc.printf("auto calibrate start\n\r"); |
c201075 | 5:eeabd90b6d62 | 232 | wb.auto_calibrate(); |
c201075 | 5:eeabd90b6d62 | 233 | for(int i = 0 ; i < 4 ; i++) |
c201075 | 5:eeabd90b6d62 | 234 | { |
c201075 | 5:eeabd90b6d62 | 235 | pc.printf("(%d,%d) ",wb._calibratedMinimum[i],wb._calibratedMaximum[i]); |
c201075 | 5:eeabd90b6d62 | 236 | } |
c201075 | 5:eeabd90b6d62 | 237 | pc.printf("\n\rauto calibrate end\n\r"); |
jksoft | 0:8468a4403fea | 238 | |
c201075 | 5:eeabd90b6d62 | 239 | #if 0 //エンコーダキャリブレーション用コード |
c201075 | 4:6b4563aaee2c | 240 | BusIn enc(P0_8,P0_10,P0_6,P0_7); |
c201075 | 4:6b4563aaee2c | 241 | enc.mode(PullNone); |
c201075 | 4:6b4563aaee2c | 242 | while(1){ |
c201075 | 4:6b4563aaee2c | 243 | char c = enc.read(); |
c201075 | 4:6b4563aaee2c | 244 | pc.putc(c); |
c201075 | 4:6b4563aaee2c | 245 | wait_ms(10); |
c201075 | 4:6b4563aaee2c | 246 | } |
c201075 | 4:6b4563aaee2c | 247 | #endif |
c201075 | 5:eeabd90b6d62 | 248 | |
jksoft | 0:8468a4403fea | 249 | while (true) { |
c201075 | 4:6b4563aaee2c | 250 | //mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz); |
c201075 | 4:6b4563aaee2c | 251 | #ifdef DEBUG |
c201075 | 5:eeabd90b6d62 | 252 | pc.printf("Pulse(%d,%d) RPM(%.2f,%.2f) ",wb._left_pulses,wb._right_pulses,wb.get_left_rpm(),wb.get_right_rpm()); |
c201075 | 5:eeabd90b6d62 | 253 | pc.printf("LinePos:%d ",wb.GetLinePosition()); |
c201075 | 5:eeabd90b6d62 | 254 | pc.printf("calib(%d,%d,%d,%d)",wb.sensor_values[0],wb.sensor_values[1],wb.sensor_values[2],wb.sensor_values[3]); |
c201075 | 4:6b4563aaee2c | 255 | //pc.printf("MPU6050(%d;%d;%d;%d;%d;%d)\n\r",ax,ay,az,gx,gy,gz); |
c201075 | 5:eeabd90b6d62 | 256 | pc.printf("\n\r"); |
c201075 | 4:6b4563aaee2c | 257 | wait_ms(10); |
c201075 | 4:6b4563aaee2c | 258 | #endif |
jksoft | 0:8468a4403fea | 259 | } |
jksoft | 0:8468a4403fea | 260 | } |
jksoft | 0:8468a4403fea | 261 |