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@9:0b048a68de4d, 2018-05-17 (annotated)

Committer:
c201075
Date:
Thu May 17 04:58:33 2018 +0000
Revision:
9:0b048a68de4d
Parent:
8:0268032d5849
Child:
11:8142dc8348f4
i2c???

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;
jksoft 0:8468a4403fea 14 MPU6050 mpu;
jksoft 0:8468a4403fea 15 wallbotble wb;
jksoft 0:8468a4403fea 16
c201075 9:0b048a68de4d 17 myI2C i2c(P0_22,P0_21);//mpuの中で使っているはず
c201075 9:0b048a68de4d 18
c201075 7:5aa479fe5d0b 19 //------------------------------------------------------------
c201075 7:5aa479fe5d0b 20 //Service & Characteristic Setting
c201075 7:5aa479fe5d0b 21 //------------------------------------------------------------
c201075 7:5aa479fe5d0b 22 //Service UUID
c201075 7:5aa479fe5d0b 23 static const uint16_t base_uuid = 0xFFF0;
c201075 7:5aa479fe5d0b 24
c201075 7:5aa479fe5d0b 25 //Characteristic UUID
c201075 7:5aa479fe5d0b 26 static const uint16_t cmd_uuid = 0xFFF1;
c201075 7:5aa479fe5d0b 27 static const uint16_t sen_uuid = 0xFFF2;
c201075 7:5aa479fe5d0b 28 static const uint16_t mpu_uuid = 0xFFF3;
c201075 7:5aa479fe5d0b 29
c201075 8:0268032d5849 30 //Characteristic buffer ?
c201075 7:5aa479fe5d0b 31 uint8_t cmdPayload[8] = {0,};
c201075 7:5aa479fe5d0b 32 uint8_t senPayload[10] = {0,};
c201075 7:5aa479fe5d0b 33 uint8_t mpuPayload[12] = {0,};
c201075 7:5aa479fe5d0b 34
c201075 7:5aa479fe5d0b 35 //Characteristic Property Setting etc
c201075 7:5aa479fe5d0b 36 GattCharacteristic cmdChar (cmd_uuid, cmdPayload, sizeof(cmdPayload), sizeof(cmdPayload), GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE);
c201075 7:5aa479fe5d0b 37 GattCharacteristic senChar (sen_uuid, senPayload, sizeof(senPayload), sizeof(senPayload), GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
c201075 7:5aa479fe5d0b 38 GattCharacteristic mpuChar (mpu_uuid, mpuPayload, sizeof(mpuPayload), sizeof(mpuPayload), GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
c201075 7:5aa479fe5d0b 39 GattCharacteristic *myChars[] = {&cmdChar, &senChar, &mpuChar};
c201075 7:5aa479fe5d0b 40
c201075 7:5aa479fe5d0b 41 //Service Setting
c201075 7:5aa479fe5d0b 42 GattService myService(base_uuid, myChars, sizeof(myChars) / sizeof(GattCharacteristic *));
c201075 7:5aa479fe5d0b 43
c201075 7:5aa479fe5d0b 44 //======================================================================
c201075 7:5aa479fe5d0b 45 //onDisconnection
c201075 7:5aa479fe5d0b 46 //======================================================================
c201075 8:0268032d5849 47 void onDisconnected(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
c201075 7:5aa479fe5d0b 48 {
c201075 8:0268032d5849 49 wb.control_enable(false);
c201075 7:5aa479fe5d0b 50 wb.stop();
c201075 7:5aa479fe5d0b 51
c201075 7:5aa479fe5d0b 52 ble.startAdvertising();
c201075 7:5aa479fe5d0b 53 wb.set_led2(0);
c201075 7:5aa479fe5d0b 54 #if DBG
c201075 7:5aa479fe5d0b 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 7:5aa479fe5d0b 67 #if DBG
c201075 7:5aa479fe5d0b 68 pc.printf("Connected\n\r");
c201075 7:5aa479fe5d0b 69 #endif
c201075 7:5aa479fe5d0b 70 }
c201075 7:5aa479fe5d0b 71
c201075 7:5aa479fe5d0b 72 //======================================================================
c201075 7:5aa479fe5d0b 73 //onDataWritten
c201075 7:5aa479fe5d0b 74 //======================================================================
c201075 8:0268032d5849 75 void onDataWritten(const GattCharacteristicWriteCBParams *params)
c201075 7:5aa479fe5d0b 76 {
c201075 8:0268032d5849 77 float right_speed;
c201075 8:0268032d5849 78 float left_speed;
c201075 8:0268032d5849 79
c201075 8:0268032d5849 80 memcpy( &right_speed, params->data , sizeof(right_speed));
c201075 8:0268032d5849 81 memcpy( &left_speed, params->data + sizeof(right_speed), sizeof(left_speed));
c201075 8:0268032d5849 82
c201075 8:0268032d5849 83 wb.SetRPM(left_speed,right_speed);
c201075 8:0268032d5849 84 #ifdef DEBUG
c201075 8:0268032d5849 85 pc.printf("SetRPM %f,%f \n\r",left_speed,right_speed);
c201075 8:0268032d5849 86 #endif
c201075 7:5aa479fe5d0b 87 }
jksoft 0:8468a4403fea 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 9:0b048a68de4d 97 int16_t mpudata[6];//ax,ay,az,gx,gy,gz
c201075 8:0268032d5849 98 short line;
c201075 8:0268032d5849 99 float rrpm,lrpm;
c201075 4:6b4563aaee2c 100
c201075 8:0268032d5849 101 uint8_t sen_buf[10];
c201075 8:0268032d5849 102 uint8_t mpu_buf[12];
c201075 8:0268032d5849 103
c201075 8:0268032d5849 104 mpu.initialize();
jksoft 0:8468a4403fea 105
c201075 4:6b4563aaee2c 106 pc.baud(115200);
c201075 4:6b4563aaee2c 107 pc.printf("Start\n\r");
jksoft 0:8468a4403fea 108
jksoft 0:8468a4403fea 109
c201075 7:5aa479fe5d0b 110 ble.init();
c201075 4:6b4563aaee2c 111 //イベント時のコールバック関数
c201075 7:5aa479fe5d0b 112 ble.onConnection(onConnected);
c201075 7:5aa479fe5d0b 113 ble.onDisconnection(onDisconnected);
c201075 7:5aa479fe5d0b 114 ble.onDataWritten(onDataWritten);
c201075 4:6b4563aaee2c 115
jksoft 0:8468a4403fea 116 /* setup advertising */
c201075 4:6b4563aaee2c 117 //クラシックBTはサポートせず、BLEデバイスとして認識してもらう
c201075 7:5aa479fe5d0b 118 ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
c201075 4:6b4563aaee2c 119 //デバイスがセントラルに接続可能であることを設定
c201075 7:5aa479fe5d0b 120 ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
c201075 4:6b4563aaee2c 121 //LOCAL NAMEの設定、BLEでは終端記号は不要なので-1する
c201075 7:5aa479fe5d0b 122 ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,(const uint8_t *)"mbed WallbotBLE", sizeof("mbed WallbotBLE") - 1);
c201075 7:5aa479fe5d0b 123
c201075 4:6b4563aaee2c 124 //16Bit短縮UUIDの設定
c201075 8:0268032d5849 125 //HACK:リバースが必要？「mbed BLE APIで128bit UUIDを含むGATT Serviceを２つ以上登録すると Service UUIDがひっくり返る？」
c201075 7:5aa479fe5d0b 126 ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS,(const uint8_t *)base_uuid, sizeof(base_uuid));
c201075 7:5aa479fe5d0b 127
c201075 4:6b4563aaee2c 128 //Advertiseパケットの送信周期
c201075 7:5aa479fe5d0b 129 ble.setAdvertisingInterval(160); /* 100ms; in multiples of 0.625ms. */
jksoft 0:8468a4403fea 130
c201075 4:6b4563aaee2c 131 //サービスの登録
c201075 7:5aa479fe5d0b 132 ble.addService(myService);
c201075 4:6b4563aaee2c 133
c201075 4:6b4563aaee2c 134 //Advertizeの開始
c201075 7:5aa479fe5d0b 135 ble.startAdvertising();
jksoft 0:8468a4403fea 136
c201075 5:eeabd90b6d62 137 pc.printf("auto calibrate start\n\r");
c201075 5:eeabd90b6d62 138 wb.auto_calibrate();
c201075 6:9fd87d75a24b 139 #ifdef DEBUG
c201075 5:eeabd90b6d62 140 for(int i = 0 ; i < 4 ; i++)
c201075 5:eeabd90b6d62 141 {
c201075 5:eeabd90b6d62 142 pc.printf("(%d,%d) ",wb._calibratedMinimum[i],wb._calibratedMaximum[i]);
c201075 5:eeabd90b6d62 143 }
c201075 5:eeabd90b6d62 144 pc.printf("\n\rauto calibrate end\n\r");
c201075 6:9fd87d75a24b 145 #endif
jksoft 0:8468a4403fea 146
c201075 5:eeabd90b6d62 147 #if 0 //エンコーダキャリブレーション用コード
c201075 4:6b4563aaee2c 148 BusIn enc(P0_8,P0_10,P0_6,P0_7);
c201075 4:6b4563aaee2c 149 enc.mode(PullNone);
c201075 4:6b4563aaee2c 150 while(1){
c201075 4:6b4563aaee2c 151 char c = enc.read();
c201075 4:6b4563aaee2c 152 pc.putc(c);
c201075 4:6b4563aaee2c 153 wait_ms(10);
c201075 4:6b4563aaee2c 154 }
c201075 4:6b4563aaee2c 155 #endif
jksoft 0:8468a4403fea 156 while (true) {
c201075 8:0268032d5849 157 //センサ値の取得
c201075 8:0268032d5849 158 line = wb.GetLinePosition();
c201075 8:0268032d5849 159 rrpm = wb.get_right_rpm();
c201075 8:0268032d5849 160 lrpm = wb.get_left_rpm();
c201075 9:0b048a68de4d 161 mpu.getMotion6(&mpudata[0], &mpudata[1], &mpudata[2], &mpudata[3], &mpudata[4], &mpudata[5]);
c201075 8:0268032d5849 162
c201075 8:0268032d5849 163 //送信の準備
c201075 8:0268032d5849 164 memcpy(sen_buf ,&line,sizeof(line));
c201075 8:0268032d5849 165 memcpy(sen_buf+sizeof(line) ,&rrpm,sizeof(rrpm));
c201075 8:0268032d5849 166 memcpy(sen_buf+sizeof(line)+sizeof(rrpm),&lrpm,sizeof(lrpm));
c201075 8:0268032d5849 167
c201075 9:0b048a68de4d 168 memcpy(mpu_buf,mpudata,sizeof(mpudata));
c201075 8:0268032d5849 169
c201075 8:0268032d5849 170
c201075 8:0268032d5849 171 ble.updateCharacteristicValue(senChar.getValueAttribute().getHandle(), (uint8_t *)sen_buf, sizeof(sen_buf));
c201075 8:0268032d5849 172 ble.updateCharacteristicValue(mpuChar.getValueAttribute().getHandle(), (uint8_t *)mpu_buf, sizeof(mpu_buf));
c201075 8:0268032d5849 173
c201075 8:0268032d5849 174 //
c201075 4:6b4563aaee2c 175 #ifdef DEBUG
c201075 8:0268032d5849 176 pc.printf("Pulse(%d,%d) RPM(%.2f,%.2f) ",wb._left_pulses,wb._right_pulses,rrpm,lrpm);
c201075 8:0268032d5849 177 pc.printf("LinePos:%d ",line);
c201075 6:9fd87d75a24b 178 //pc.printf("calib(%d,%d,%d,%d)",wb.sensor_values[0],wb.sensor_values[1],wb.sensor_values[2],wb.sensor_values[3]);
c201075 9:0b048a68de4d 179 pc.printf("MPU6050(%d;%d;%d;%d;%d;%d)\n\r",mpudata[0], mpudata[1], mpudata[2], mpudata[3], mpudata[4], mpudata[5]);
c201075 5:eeabd90b6d62 180 pc.printf("\n\r");
c201075 4:6b4563aaee2c 181 #endif
c201075 8:0268032d5849 182 wait_ms(10);
jksoft 0:8468a4403fea 183 }
jksoft 0:8468a4403fea 184 }
jksoft 0:8468a4403fea 185