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@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?

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"
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を２つ以上登録すると 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