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@18:8bab3e5c4ea2, 2018-05-24 (annotated)

Committer:
c201075
Date:
Thu May 24 03:32:29 2018 +0000
Revision:
18:8bab3e5c4ea2
Parent:
17:0a7b2afd075b
Child:
19:5f1e153ef63e
rough calib2

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