Wallbot_CaaS
Dependencies: MPU6050 mbed PID
Fork of BLE_MPU6050_test6_challenge_sb by
main.cpp@20:b0e1fa37856d, 2018-05-25 (annotated)
- Committer:
- c201075
- Date:
- Fri May 25 04:52:06 2018 +0000
- Revision:
- 20:b0e1fa37856d
- Parent:
- 19:5f1e153ef63e
- Child:
- 21:14cf80622dd5
bug fix
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" |
c201075 | 6:9fd87d75a24b | 5 | //#include "RCBController.h" |
c201075 | 6:9fd87d75a24b | 6 | //#include "Adafruit_LEDBackpack.h" |
c201075 | 6:9fd87d75a24b | 7 | //#include "Adafruit_GFX.h" |
c201075 | 6:9fd87d75a24b | 8 | //#include "pictLIB.h" |
jksoft | 0:8468a4403fea | 9 | |
c201075 | 20:b0e1fa37856d | 10 | //#define DEBUG |
jksoft | 0:8468a4403fea | 11 | |
jksoft | 0:8468a4403fea | 12 | Serial pc(USBTX, USBRX); |
jksoft | 0:8468a4403fea | 13 | BLEDevice ble; |
c201075 | 14:8a589bb0868c | 14 | MPU6050 mpu(I2C_SDA,I2C_SCL); |
jksoft | 0:8468a4403fea | 15 | wallbotble wb; |
jksoft | 0:8468a4403fea | 16 | |
c201075 | 7:5aa479fe5d0b | 17 | //------------------------------------------------------------ |
c201075 | 7:5aa479fe5d0b | 18 | //Service & Characteristic Setting |
c201075 | 7:5aa479fe5d0b | 19 | //------------------------------------------------------------ |
c201075 | 7:5aa479fe5d0b | 20 | //Service UUID |
c201075 | 7:5aa479fe5d0b | 21 | static const uint16_t base_uuid = 0xFFF0; |
c201075 | 7:5aa479fe5d0b | 22 | |
c201075 | 7:5aa479fe5d0b | 23 | //Characteristic UUID |
c201075 | 7:5aa479fe5d0b | 24 | static const uint16_t cmd_uuid = 0xFFF1; |
c201075 | 7:5aa479fe5d0b | 25 | static const uint16_t sen_uuid = 0xFFF2; |
c201075 | 7:5aa479fe5d0b | 26 | static const uint16_t mpu_uuid = 0xFFF3; |
c201075 | 7:5aa479fe5d0b | 27 | |
c201075 | 8:0268032d5849 | 28 | //Characteristic buffer ? |
c201075 | 7:5aa479fe5d0b | 29 | uint8_t cmdPayload[8] = {0,}; |
c201075 | 7:5aa479fe5d0b | 30 | uint8_t senPayload[10] = {0,}; |
c201075 | 7:5aa479fe5d0b | 31 | uint8_t mpuPayload[12] = {0,}; |
c201075 | 7:5aa479fe5d0b | 32 | |
c201075 | 19:5f1e153ef63e | 33 | |
c201075 | 7:5aa479fe5d0b | 34 | //Characteristic Property Setting etc |
c201075 | 7:5aa479fe5d0b | 35 | GattCharacteristic cmdChar (cmd_uuid, cmdPayload, sizeof(cmdPayload), sizeof(cmdPayload), GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE); |
c201075 | 7:5aa479fe5d0b | 36 | GattCharacteristic senChar (sen_uuid, senPayload, sizeof(senPayload), sizeof(senPayload), GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY); |
c201075 | 7:5aa479fe5d0b | 37 | GattCharacteristic mpuChar (mpu_uuid, mpuPayload, sizeof(mpuPayload), sizeof(mpuPayload), GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY); |
c201075 | 7:5aa479fe5d0b | 38 | GattCharacteristic *myChars[] = {&cmdChar, &senChar, &mpuChar}; |
c201075 | 7:5aa479fe5d0b | 39 | |
c201075 | 7:5aa479fe5d0b | 40 | //Service Setting |
c201075 | 7:5aa479fe5d0b | 41 | GattService myService(base_uuid, myChars, sizeof(myChars) / sizeof(GattCharacteristic *)); |
c201075 | 7:5aa479fe5d0b | 42 | |
c201075 | 7:5aa479fe5d0b | 43 | //====================================================================== |
c201075 | 7:5aa479fe5d0b | 44 | //onDisconnection |
c201075 | 7:5aa479fe5d0b | 45 | //====================================================================== |
c201075 | 8:0268032d5849 | 46 | void onDisconnected(Gap::Handle_t handle, Gap::DisconnectionReason_t reason) |
c201075 | 7:5aa479fe5d0b | 47 | { |
c201075 | 8:0268032d5849 | 48 | wb.control_enable(false); |
c201075 | 7:5aa479fe5d0b | 49 | wb.stop(); |
c201075 | 7:5aa479fe5d0b | 50 | |
c201075 | 7:5aa479fe5d0b | 51 | ble.startAdvertising(); |
c201075 | 7:5aa479fe5d0b | 52 | wb.set_led2(0); |
c201075 | 13:ed867b8003c2 | 53 | #ifdef DEBUG |
c201075 | 19:5f1e153ef63e | 54 | //割り込みハンドラの中でシリアル通信しないこと。 |
c201075 | 19:5f1e153ef63e | 55 | //pc.printf("Disconnected\n\r"); |
c201075 | 7:5aa479fe5d0b | 56 | #endif |
c201075 | 7:5aa479fe5d0b | 57 | } |
c201075 | 7:5aa479fe5d0b | 58 | //====================================================================== |
c201075 | 7:5aa479fe5d0b | 59 | //onConnection |
c201075 | 7:5aa479fe5d0b | 60 | //====================================================================== |
c201075 | 7:5aa479fe5d0b | 61 | void onConnected(Gap::Handle_t handle, Gap::addr_type_t peerAddrType ,const Gap::address_t peerAddr,const Gap::ConnectionParams_t *params) |
c201075 | 7:5aa479fe5d0b | 62 | { |
c201075 | 7:5aa479fe5d0b | 63 | wb.set_led2(1); |
c201075 | 7:5aa479fe5d0b | 64 | |
c201075 | 7:5aa479fe5d0b | 65 | wb.control_enable(true); |
c201075 | 7:5aa479fe5d0b | 66 | wb.SetRPM(0,0); |
c201075 | 13:ed867b8003c2 | 67 | #ifdef DEBUG |
c201075 | 19:5f1e153ef63e | 68 | //割り込みハンドラの中でシリアル通信しないこと。 |
c201075 | 19:5f1e153ef63e | 69 | //pc.printf("Connected\n\r"); |
c201075 | 7:5aa479fe5d0b | 70 | #endif |
c201075 | 7:5aa479fe5d0b | 71 | } |
c201075 | 7:5aa479fe5d0b | 72 | |
c201075 | 7:5aa479fe5d0b | 73 | //====================================================================== |
c201075 | 7:5aa479fe5d0b | 74 | //onDataWritten |
c201075 | 7:5aa479fe5d0b | 75 | //====================================================================== |
c201075 | 19:5f1e153ef63e | 76 | |
c201075 | 8:0268032d5849 | 77 | void onDataWritten(const GattCharacteristicWriteCBParams *params) |
c201075 | 7:5aa479fe5d0b | 78 | { |
c201075 | 19:5f1e153ef63e | 79 | float right_cmd = 0; |
c201075 | 19:5f1e153ef63e | 80 | float left_cmd = 0; |
c201075 | 18:8bab3e5c4ea2 | 81 | memcpy( &right_cmd, params->data , sizeof(right_cmd)); |
c201075 | 18:8bab3e5c4ea2 | 82 | memcpy( &left_cmd, params->data + sizeof(right_cmd), sizeof(left_cmd)); |
c201075 | 20:b0e1fa37856d | 83 | wb.SetRPM(left_cmd,right_cmd); |
c201075 | 8:0268032d5849 | 84 | #ifdef DEBUG |
c201075 | 19:5f1e153ef63e | 85 | //割り込みハンドラの中でシリアル通信しないこと。 |
c201075 | 19:5f1e153ef63e | 86 | //printf("SetRPM:%f,%f",left_cmd,right_cmd) |
c201075 | 8:0268032d5849 | 87 | #endif |
c201075 | 7:5aa479fe5d0b | 88 | } |
jksoft | 0:8468a4403fea | 89 | /**************************************************************************/ |
jksoft | 0:8468a4403fea | 90 | /*! |
jksoft | 0:8468a4403fea | 91 | @brief Program entry point |
jksoft | 0:8468a4403fea | 92 | */ |
jksoft | 0:8468a4403fea | 93 | /**************************************************************************/ |
jksoft | 0:8468a4403fea | 94 | int main(void) |
jksoft | 0:8468a4403fea | 95 | { |
jksoft | 0:8468a4403fea | 96 | |
c201075 | 14:8a589bb0868c | 97 | int16_t mpu_a[3]; |
c201075 | 14:8a589bb0868c | 98 | int16_t mpu_g[3]; |
c201075 | 8:0268032d5849 | 99 | short line; |
c201075 | 8:0268032d5849 | 100 | float rrpm,lrpm; |
c201075 | 4:6b4563aaee2c | 101 | |
c201075 | 8:0268032d5849 | 102 | uint8_t sen_buf[10]; |
c201075 | 8:0268032d5849 | 103 | uint8_t mpu_buf[12]; |
c201075 | 8:0268032d5849 | 104 | |
c201075 | 14:8a589bb0868c | 105 | pc.baud(9600); |
c201075 | 13:ed867b8003c2 | 106 | |
c201075 | 14:8a589bb0868c | 107 | //MPU 成功するまでリトライ |
c201075 | 13:ed867b8003c2 | 108 | wb.set_led1(1); |
c201075 | 14:8a589bb0868c | 109 | wait(1); |
c201075 | 14:8a589bb0868c | 110 | |
c201075 | 14:8a589bb0868c | 111 | if(!mpu.testConnection()){ |
c201075 | 14:8a589bb0868c | 112 | pc.printf("MPU connection fail, -> soft reset \n\r"); |
c201075 | 14:8a589bb0868c | 113 | NVIC_SystemReset(); |
c201075 | 14:8a589bb0868c | 114 | } |
c201075 | 14:8a589bb0868c | 115 | |
c201075 | 13:ed867b8003c2 | 116 | wb.set_led1(0); |
c201075 | 13:ed867b8003c2 | 117 | pc.printf("MPU6050 initialize passed \n\r"); |
jksoft | 0:8468a4403fea | 118 | |
c201075 | 16:aea4df56fa6b | 119 | |
c201075 | 18:8bab3e5c4ea2 | 120 | const uint8_t address1[] = {0xe7,0xAA,0xAA,0xAA,0xAA,0xAA}; |
c201075 | 18:8bab3e5c4ea2 | 121 | ble.setAddress(Gap::ADDR_TYPE_PUBLIC, address1); |
c201075 | 16:aea4df56fa6b | 122 | |
c201075 | 16:aea4df56fa6b | 123 | ble.init(); |
c201075 | 4:6b4563aaee2c | 124 | //イベント時のコールバック関数 |
c201075 | 7:5aa479fe5d0b | 125 | ble.onConnection(onConnected); |
c201075 | 7:5aa479fe5d0b | 126 | ble.onDisconnection(onDisconnected); |
c201075 | 7:5aa479fe5d0b | 127 | ble.onDataWritten(onDataWritten); |
c201075 | 4:6b4563aaee2c | 128 | |
jksoft | 0:8468a4403fea | 129 | /* setup advertising */ |
c201075 | 4:6b4563aaee2c | 130 | //クラシックBTはサポートせず、BLEデバイスとして認識してもらう |
c201075 | 7:5aa479fe5d0b | 131 | ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED); |
c201075 | 4:6b4563aaee2c | 132 | //デバイスがセントラルに接続可能であることを設定 |
c201075 | 7:5aa479fe5d0b | 133 | ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED); |
c201075 | 4:6b4563aaee2c | 134 | //LOCAL NAMEの設定、BLEでは終端記号は不要なので-1する |
c201075 | 7:5aa479fe5d0b | 135 | ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,(const uint8_t *)"mbed WallbotBLE", sizeof("mbed WallbotBLE") - 1); |
c201075 | 7:5aa479fe5d0b | 136 | |
c201075 | 4:6b4563aaee2c | 137 | //16Bit短縮UUIDの設定 |
c201075 | 8:0268032d5849 | 138 | //HACK:リバースが必要?「mbed BLE APIで128bit UUIDを含むGATT Serviceを2つ以上登録すると Service UUIDがひっくり返る?」 |
c201075 | 7:5aa479fe5d0b | 139 | ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,(const uint8_t *)base_uuid, sizeof(base_uuid)); |
c201075 | 7:5aa479fe5d0b | 140 | |
c201075 | 4:6b4563aaee2c | 141 | //Advertiseパケットの送信周期 |
c201075 | 7:5aa479fe5d0b | 142 | ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */ |
jksoft | 0:8468a4403fea | 143 | |
c201075 | 4:6b4563aaee2c | 144 | //サービスの登録 |
c201075 | 7:5aa479fe5d0b | 145 | ble.addService(myService); |
c201075 | 4:6b4563aaee2c | 146 | |
c201075 | 4:6b4563aaee2c | 147 | //Advertizeの開始 |
c201075 | 7:5aa479fe5d0b | 148 | ble.startAdvertising(); |
jksoft | 0:8468a4403fea | 149 | |
c201075 | 13:ed867b8003c2 | 150 | //ラインセンサキャリブレーション |
c201075 | 13:ed867b8003c2 | 151 | //pc.printf("auto calibrate start\n\r"); |
c201075 | 18:8bab3e5c4ea2 | 152 | |
c201075 | 18:8bab3e5c4ea2 | 153 | //適当キャリブレーション |
c201075 | 20:b0e1fa37856d | 154 | /* |
c201075 | 18:8bab3e5c4ea2 | 155 | for(int i = 0 ; i < 4 ; i++) |
c201075 | 18:8bab3e5c4ea2 | 156 | { |
c201075 | 18:8bab3e5c4ea2 | 157 | wb._calibratedMinimum[i] = 300; |
c201075 | 18:8bab3e5c4ea2 | 158 | wb._calibratedMaximum[i] = 900; |
c201075 | 18:8bab3e5c4ea2 | 159 | } |
c201075 | 20:b0e1fa37856d | 160 | */ |
c201075 | 20:b0e1fa37856d | 161 | |
c201075 | 20:b0e1fa37856d | 162 | wb.auto_calibrate(); //自動キャリブレーション |
c201075 | 6:9fd87d75a24b | 163 | #ifdef DEBUG |
c201075 | 5:eeabd90b6d62 | 164 | for(int i = 0 ; i < 4 ; i++) |
c201075 | 5:eeabd90b6d62 | 165 | { |
c201075 | 5:eeabd90b6d62 | 166 | pc.printf("(%d,%d) ",wb._calibratedMinimum[i],wb._calibratedMaximum[i]); |
c201075 | 5:eeabd90b6d62 | 167 | } |
c201075 | 5:eeabd90b6d62 | 168 | pc.printf("\n\rauto calibrate end\n\r"); |
c201075 | 6:9fd87d75a24b | 169 | #endif |
jksoft | 0:8468a4403fea | 170 | |
c201075 | 5:eeabd90b6d62 | 171 | #if 0 //エンコーダキャリブレーション用コード |
c201075 | 4:6b4563aaee2c | 172 | BusIn enc(P0_8,P0_10,P0_6,P0_7); |
c201075 | 4:6b4563aaee2c | 173 | enc.mode(PullNone); |
c201075 | 4:6b4563aaee2c | 174 | while(1){ |
c201075 | 4:6b4563aaee2c | 175 | char c = enc.read(); |
c201075 | 4:6b4563aaee2c | 176 | pc.putc(c); |
c201075 | 4:6b4563aaee2c | 177 | wait_ms(10); |
c201075 | 4:6b4563aaee2c | 178 | } |
c201075 | 4:6b4563aaee2c | 179 | #endif |
c201075 | 12:96fbb4cb7c48 | 180 | |
c201075 | 13:ed867b8003c2 | 181 | //速度制御 |
c201075 | 12:96fbb4cb7c48 | 182 | //wb.control_enable(true); |
c201075 | 12:96fbb4cb7c48 | 183 | //wb.SetRPM(-30,30); |
c201075 | 12:96fbb4cb7c48 | 184 | |
jksoft | 0:8468a4403fea | 185 | while (true) { |
c201075 | 11:8142dc8348f4 | 186 | |
c201075 | 8:0268032d5849 | 187 | //センサ値の取得 |
c201075 | 8:0268032d5849 | 188 | line = wb.GetLinePosition(); |
c201075 | 8:0268032d5849 | 189 | rrpm = wb.get_right_rpm(); |
c201075 | 8:0268032d5849 | 190 | lrpm = wb.get_left_rpm(); |
c201075 | 14:8a589bb0868c | 191 | mpu.getAcceleroRaw(mpu_a); |
c201075 | 14:8a589bb0868c | 192 | mpu.getGyroRaw(mpu_g); |
c201075 | 8:0268032d5849 | 193 | |
c201075 | 18:8bab3e5c4ea2 | 194 | //送信の準備(リトルエンディアン) |
c201075 | 8:0268032d5849 | 195 | memcpy(sen_buf ,&line,sizeof(line)); |
c201075 | 8:0268032d5849 | 196 | memcpy(sen_buf+sizeof(line) ,&rrpm,sizeof(rrpm)); |
c201075 | 8:0268032d5849 | 197 | memcpy(sen_buf+sizeof(line)+sizeof(rrpm),&lrpm,sizeof(lrpm)); |
c201075 | 8:0268032d5849 | 198 | |
c201075 | 14:8a589bb0868c | 199 | memcpy(mpu_buf ,mpu_a,sizeof(mpu_a)); |
c201075 | 14:8a589bb0868c | 200 | memcpy(mpu_buf+sizeof(mpu_a),mpu_g,sizeof(mpu_g)); |
c201075 | 8:0268032d5849 | 201 | |
c201075 | 11:8142dc8348f4 | 202 | //送信 |
c201075 | 8:0268032d5849 | 203 | ble.updateCharacteristicValue(senChar.getValueAttribute().getHandle(), (uint8_t *)sen_buf, sizeof(sen_buf)); |
c201075 | 8:0268032d5849 | 204 | ble.updateCharacteristicValue(mpuChar.getValueAttribute().getHandle(), (uint8_t *)mpu_buf, sizeof(mpu_buf)); |
c201075 | 8:0268032d5849 | 205 | |
c201075 | 8:0268032d5849 | 206 | // |
c201075 | 4:6b4563aaee2c | 207 | #ifdef DEBUG |
c201075 | 11:8142dc8348f4 | 208 | pc.printf("Pulse(%d,%d) RPM(%.2f,%.2f) ",wb._left_pulses,wb._right_pulses,lrpm, rrpm); |
c201075 | 8:0268032d5849 | 209 | pc.printf("LinePos:%d ",line); |
c201075 | 14:8a589bb0868c | 210 | pc.printf("Sens(%d,%d,%d,%d)",wb.sensor_values[0],wb.sensor_values[1],wb.sensor_values[2],wb.sensor_values[3]); |
c201075 | 14:8a589bb0868c | 211 | pc.printf("MPU(%d,%d,%d, %d,%d,%d)",mpu_a[0],mpu_a[1],mpu_a[2],mpu_g[0],mpu_g[1],mpu_g[2]); |
c201075 | 14:8a589bb0868c | 212 | |
c201075 | 5:eeabd90b6d62 | 213 | pc.printf("\n\r"); |
c201075 | 4:6b4563aaee2c | 214 | #endif |
c201075 | 8:0268032d5849 | 215 | wait_ms(10); |
c201075 | 14:8a589bb0868c | 216 | |
jksoft | 0:8468a4403fea | 217 | } |
jksoft | 0:8468a4403fea | 218 | } |
jksoft | 0:8468a4403fea | 219 |