Moto Sbk / Mbed 2 deprecated Wallbot_CaaS

Wallbot_CaaS

Dependencies:   MPU6050 mbed PID

Fork of BLE_MPU6050_test6_challenge_sb by Junichi Katsu

main.cpp@4:6b4563aaee2c, 2018-05-16 (annotated)

Committer:
c201075
Date:
Wed May 16 09:52:29 2018 +0000
Revision:
4:6b4563aaee2c
Parent:
old_main.cpp@2:64f85c9eb556
Child:
5:eeabd90b6d62
??????????????2??3????????????????????????????????????????

Who changed what in which revision?

UserRevisionLine numberNew 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 4:6b4563aaee2c 230 wb.right_motor(0.5);
c201075 4:6b4563aaee2c 231 wb.left_motor(0.5);
jksoft 0:8468a4403fea 232
c201075 4:6b4563aaee2c 233 #ifdef ENCCALIB //エンコーダキャリブレーション用コード
c201075 4:6b4563aaee2c 234 BusIn enc(P0_8,P0_10,P0_6,P0_7);
c201075 4:6b4563aaee2c 235 enc.mode(PullNone);
c201075 4:6b4563aaee2c 236 while(1){
c201075 4:6b4563aaee2c 237 char c = enc.read();
c201075 4:6b4563aaee2c 238 pc.putc(c);
c201075 4:6b4563aaee2c 239 wait_ms(10);
c201075 4:6b4563aaee2c 240 }
c201075 4:6b4563aaee2c 241 #endif
jksoft 0:8468a4403fea 242 while (true) {
c201075 4:6b4563aaee2c 243
c201075 4:6b4563aaee2c 244 //mpu.getMotion6(&ax, &ay, &az, &gx, &gy, &gz);
c201075 4:6b4563aaee2c 245 #ifdef DEBUG
jksoft 0:8468a4403fea 246
c201075 4:6b4563aaee2c 247 //pc.printf("pulse(%d,%d) state(%d,%d)",wb.left_getPulses(),wb.right_getPulses(),wb._left_enc.getCurrentState(),wb._right_enc.getCurrentState());
c201075 4:6b4563aaee2c 248 //pc.printf("%d%d%d%d ",((enc.read() >> 3) & 1), ((enc.read() >> 2) & 1), ((enc.read() >> 1) & 1), (enc.read() & 1));
c201075 4:6b4563aaee2c 249 //pc.printf("LinePos:%d RPM(%d,%d)",wb.GetLinePosition(),wb.get_left_rpm(),wb.get_right_rpm());
c201075 4:6b4563aaee2c 250 //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 251 //pc.printf("MPU6050(%d;%d;%d;%d;%d;%d)\n\r",ax,ay,az,gx,gy,gz);
c201075 4:6b4563aaee2c 252
c201075 4:6b4563aaee2c 253 //pc.printf("\n\r");
c201075 4:6b4563aaee2c 254 wait_ms(10);
c201075 4:6b4563aaee2c 255 #endif
jksoft 0:8468a4403fea 256 }
jksoft 0:8468a4403fea 257 }
jksoft 0:8468a4403fea 258