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@12:96fbb4cb7c48, 2018-05-17 (annotated)

Committer:
c201075
Date:
Thu May 17 23:31:47 2018 +0000
Revision:
12:96fbb4cb7c48
Parent:
11:8142dc8348f4
Child:
13:ed867b8003c2
dekita

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 11:8142dc8348f4 77 float right_cmd;
c201075 11:8142dc8348f4 78 float left_cmd;
c201075 8:0268032d5849 79
c201075 11:8142dc8348f4 80 memcpy( &right_cmd, params->data , sizeof(right_cmd));
c201075 11:8142dc8348f4 81 memcpy( &left_cmd, params->data + sizeof(right_cmd), sizeof(left_cmd));
c201075 8:0268032d5849 82
c201075 11:8142dc8348f4 83 wb.SetRPM(left_cmd,right_cmd);
c201075 8:0268032d5849 84 #ifdef DEBUG
c201075 11:8142dc8348f4 85 pc.printf("SetRPM %f,%f \n\r",left_cmd,right_cmd);
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 12:96fbb4cb7c48 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
c201075 12:96fbb4cb7c48 156
c201075 12:96fbb4cb7c48 157 //wb.control_enable(true);
c201075 12:96fbb4cb7c48 158 //wb.SetRPM(-30,30);
c201075 12:96fbb4cb7c48 159
jksoft 0:8468a4403fea 160 while (true) {
c201075 11:8142dc8348f4 161
c201075 8:0268032d5849 162 //センサ値の取得
c201075 8:0268032d5849 163 line = wb.GetLinePosition();
c201075 8:0268032d5849 164 rrpm = wb.get_right_rpm();
c201075 8:0268032d5849 165 lrpm = wb.get_left_rpm();
c201075 9:0b048a68de4d 166 mpu.getMotion6(&mpudata[0], &mpudata[1], &mpudata[2], &mpudata[3], &mpudata[4], &mpudata[5]);
c201075 8:0268032d5849 167
c201075 8:0268032d5849 168 //送信の準備
c201075 8:0268032d5849 169 memcpy(sen_buf ,&line,sizeof(line));
c201075 8:0268032d5849 170 memcpy(sen_buf+sizeof(line) ,&rrpm,sizeof(rrpm));
c201075 8:0268032d5849 171 memcpy(sen_buf+sizeof(line)+sizeof(rrpm),&lrpm,sizeof(lrpm));
c201075 8:0268032d5849 172
c201075 9:0b048a68de4d 173 memcpy(mpu_buf,mpudata,sizeof(mpudata));
c201075 8:0268032d5849 174
c201075 11:8142dc8348f4 175 //送信
c201075 8:0268032d5849 176 ble.updateCharacteristicValue(senChar.getValueAttribute().getHandle(), (uint8_t *)sen_buf, sizeof(sen_buf));
c201075 8:0268032d5849 177 ble.updateCharacteristicValue(mpuChar.getValueAttribute().getHandle(), (uint8_t *)mpu_buf, sizeof(mpu_buf));
c201075 8:0268032d5849 178
c201075 8:0268032d5849 179 //
c201075 4:6b4563aaee2c 180 #ifdef DEBUG
c201075 11:8142dc8348f4 181 pc.printf("Pulse(%d,%d) RPM(%.2f,%.2f) ",wb._left_pulses,wb._right_pulses,lrpm, rrpm);
c201075 8:0268032d5849 182 pc.printf("LinePos:%d ",line);
c201075 6:9fd87d75a24b 183 //pc.printf("calib(%d,%d,%d,%d)",wb.sensor_values[0],wb.sensor_values[1],wb.sensor_values[2],wb.sensor_values[3]);
c201075 11:8142dc8348f4 184 pc.printf("MPU6050(%d;%d;%d;%d;%d;%d)",mpudata[0], mpudata[1], mpudata[2], mpudata[3], mpudata[4], mpudata[5]);
c201075 5:eeabd90b6d62 185 pc.printf("\n\r");
c201075 4:6b4563aaee2c 186 #endif
c201075 8:0268032d5849 187 wait_ms(10);
jksoft 0:8468a4403fea 188 }
jksoft 0:8468a4403fea 189 }
jksoft 0:8468a4403fea 190