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@21:14cf80622dd5, 2018-06-04 (annotated)

Committer:
c201075
Date:
Mon Jun 04 01:41:37 2018 +0000
Revision:
21:14cf80622dd5
Parent:
20:b0e1fa37856d 
add motor feed forward term

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 21:14cf80622dd5 161 #ifndef DEBUG
c201075 20:b0e1fa37856d 162 wb.auto_calibrate(); //自動キャリブレーション
c201075 21:14cf80622dd5 163 #endif
c201075 6:9fd87d75a24b 164 #ifdef DEBUG
c201075 5:eeabd90b6d62 165 for(int i = 0 ; i < 4 ; i++)
c201075 5:eeabd90b6d62 166 {
c201075 5:eeabd90b6d62 167 pc.printf("(%d,%d) ",wb._calibratedMinimum[i],wb._calibratedMaximum[i]);
c201075 5:eeabd90b6d62 168 }
c201075 5:eeabd90b6d62 169 pc.printf("\n\rauto calibrate end\n\r");
c201075 6:9fd87d75a24b 170 #endif
jksoft 0:8468a4403fea 171
c201075 5:eeabd90b6d62 172 #if 0 //エンコーダキャリブレーション用コード
c201075 4:6b4563aaee2c 173 BusIn enc(P0_8,P0_10,P0_6,P0_7);
c201075 4:6b4563aaee2c 174 enc.mode(PullNone);
c201075 4:6b4563aaee2c 175 while(1){
c201075 4:6b4563aaee2c 176 char c = enc.read();
c201075 4:6b4563aaee2c 177 pc.putc(c);
c201075 4:6b4563aaee2c 178 wait_ms(10);
c201075 4:6b4563aaee2c 179 }
c201075 4:6b4563aaee2c 180 #endif
c201075 12:96fbb4cb7c48 181
c201075 13:ed867b8003c2 182 //速度制御
c201075 12:96fbb4cb7c48 183 //wb.control_enable(true);
c201075 21:14cf80622dd5 184 //Timer time;
c201075 21:14cf80622dd5 185 //int now;
c201075 21:14cf80622dd5 186 //time.start();
jksoft 0:8468a4403fea 187 while (true) {
c201075 21:14cf80622dd5 188 ///////////////////PID調整
c201075 21:14cf80622dd5 189 //now = time.read();
c201075 21:14cf80622dd5 190 //if((now % 10) == 0){
c201075 21:14cf80622dd5 191 // pc.printf("set RPM %d ",now % 50 +10);
c201075 21:14cf80622dd5 192 // wb.SetRPM(now % 50 +10,now % 50 +10);
c201075 21:14cf80622dd5 193 //}
c201075 21:14cf80622dd5 194 ///////////////////////
c201075 11:8142dc8348f4 195
c201075 8:0268032d5849 196 //センサ値の取得
c201075 8:0268032d5849 197 line = wb.GetLinePosition();
c201075 8:0268032d5849 198 rrpm = wb.get_right_rpm();
c201075 8:0268032d5849 199 lrpm = wb.get_left_rpm();
c201075 14:8a589bb0868c 200 mpu.getAcceleroRaw(mpu_a);
c201075 14:8a589bb0868c 201 mpu.getGyroRaw(mpu_g);
c201075 8:0268032d5849 202
c201075 18:8bab3e5c4ea2 203 //送信の準備(リトルエンディアン)
c201075 8:0268032d5849 204 memcpy(sen_buf ,&line,sizeof(line));
c201075 8:0268032d5849 205 memcpy(sen_buf+sizeof(line) ,&rrpm,sizeof(rrpm));
c201075 8:0268032d5849 206 memcpy(sen_buf+sizeof(line)+sizeof(rrpm),&lrpm,sizeof(lrpm));
c201075 8:0268032d5849 207
c201075 14:8a589bb0868c 208 memcpy(mpu_buf ,mpu_a,sizeof(mpu_a));
c201075 14:8a589bb0868c 209 memcpy(mpu_buf+sizeof(mpu_a),mpu_g,sizeof(mpu_g));
c201075 8:0268032d5849 210
c201075 11:8142dc8348f4 211 //送信
c201075 8:0268032d5849 212 ble.updateCharacteristicValue(senChar.getValueAttribute().getHandle(), (uint8_t *)sen_buf, sizeof(sen_buf));
c201075 8:0268032d5849 213 ble.updateCharacteristicValue(mpuChar.getValueAttribute().getHandle(), (uint8_t *)mpu_buf, sizeof(mpu_buf));
c201075 8:0268032d5849 214
c201075 8:0268032d5849 215 //
c201075 4:6b4563aaee2c 216 #ifdef DEBUG
c201075 21:14cf80622dd5 217 pc.printf("Pulse(%d,%d)",wb._left_pulses,wb._right_pulses);
c201075 21:14cf80622dd5 218 pc.printf("RPM(%.2f,%.2f)",lrpm, rrpm);
c201075 8:0268032d5849 219 pc.printf("LinePos:%d ",line);
c201075 14:8a589bb0868c 220 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 221 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 222
c201075 5:eeabd90b6d62 223 pc.printf("\n\r");
c201075 4:6b4563aaee2c 224 #endif
c201075 8:0268032d5849 225 wait_ms(10);
c201075 14:8a589bb0868c 226
jksoft 0:8468a4403fea 227 }
jksoft 0:8468a4403fea 228 }
jksoft 0:8468a4403fea 229