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@8:0268032d5849, 2018-05-17 (annotated)

Committer:
c201075
Date:
Thu May 17 04:51:49 2018 +0000
Revision:
8:0268032d5849
Parent:
7:5aa479fe5d0b
Child:
9:0b048a68de4d
ble;

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