TKS が 2021年05月03日14時41分50秒 に編集
コメント無し
本文の変更
**<目次>** ・概要 ・動作原理 ・部品リスト ・接続図 ・設計仕様 ・動作仕様 **<概要>** 電車やバスなどの乗降車の時の「扉が閉まってからでは遅い！」を解決するためのアプリケーションです。 社員証や重要機器などのセキュリティ事故になるような物品から石焼き芋まで、大切な持ち物の置き忘れを教えてくれるアラート端末です。 市販品には、BLE接続を常時確認し、接続が切れたときに通知してくれるような忘れ物防止端末は安価に販売されており様々なシーンに有用ですが、 この方式のデメリットとして、公共交通機関の乗降時では、BLE接続が切れるタイミングは概ね扉が閉まった後の通知になってしまいます。 そこで、扉が閉まる前に置き忘れを即時通知し、素敵な1日を作るお手伝いをするような端末の作成を目指します。 **<動作原理>** 加速度ジャイロセンサを2セット用いた「活動レベル同期方式(仮称)」で置き忘れを検出します。 前提として、公共交通機関での置き忘れが発生した直後の状況は、本人は「歩行もしくは走行」し、持ち物は「静止」している状況が殆どです。 そこで、本人(ホスト)と持ち物(デバイス)にそれぞれ端末を持たせ、加速度・ジャイロ値から、それぞれの活動レベルを認識し、持ち物が「静止」中に、 本人が「歩行・走行」していれば置き忘れと判断し、通知する仕組みを作成します。   **<部品リスト>** ◇ホスト側 obniz Board 1Y MPU-6050 使用 ３軸ジャイロスコープ・３軸加速度センサー モジュール 【価格：350円】 ブブザーモジュール タイマーArdinuo電子玩具警報装置用【価格：829円】 モバイルバッテリー ５０００ｍＡｈ 品番 4984279860151 【価格：550円（税込）】 ◇デバイス側 LILYGO_T-Higrow ESP32 WiFi + bluetooth + DHT11土壌温度および湿度センサーモジュール 【価格：3,282円】 MPU-6050 使用 ３軸ジャイロスコープ・３軸加速度センサー モジュール 【価格：350円】 モバイルバッテリー ５０００ｍＡｈ 品番 4984279860151 【価格：550円（税込）】 **<接続図>**   **<設計仕様>** ◇ホスト側・デバイス側共通 センサ分解能：加速度 4g ジャイロ 500dps 加速度AD0：Lo(address=0x68) 活動量更新周期：500msec（50msec x 10sampleの平均値を使用） 活動量の計算：abs(Δx)+abs(Δy)+abs(Δz) ※Acc,Gyroで共通の計算式。Δは1sample毎の前回との差分のことです。 ◇ホスト 活動量と活動レベルの関係： Acc活動レベル： 0(静止)：0.0g 1(交通機関移動)：0.2g 2(歩行)：0.4g 3(走行)：1.5g ビープ音：440Hz, duty=50% **<動作仕様>** ホスト、デバイスそれぞれ非同期で設計仕様の通りデータサンプリングを継続的に実施します。 デバイスは活動量を継続的にアドバタイズし、ホストはBLEデータを受信したら活動レベルの判定を行います。 ・ホスト活動レベルが2(歩行)以上で、デバイス活動レベルが0(静止)であれば、置き忘れと判断しビープ音で通知します。 ・デバイスの活動レベルが1(交通機関移動)以上になると、ホストの活動レベルによらずビープ音をストップします。 ・ホストの活動レベルが2(歩行)未満であれば、デバイスの活動レベルによらずビープ音をストップします。 ・デバイスの活動レベルに更新がない場合は「Out of Range」と表示し、ビープ音をストップします。   制御フロー  **<所感>** シンプルな構成ですが、徒歩、走行、信号待ち、電車、バスのシーンで概ね期待通りの活動レベルが出ており、動作仕様通りに通知することができました。 今回実装した活動レベル同期方式(仮称)に加え、従来製品のようなBLE接続状態での判定、またGPSを併用することでより有用な忘れ物防止端末ができるのでは ないかと思いました。 **<ソースコード>** ```arduino:ホスト <html> <head> <meta charset="utf-8" /> <meta name="viewport" content="width=device-width, initial-scale=1" /> <link rel="stylesheet" href="https://stackpath.bootstrapcdn.com/bootstrap/4.3.1/css/bootstrap.min.css" /> <script src="https://code.jquery.com/jquery-3.2.1.min.js"></script> <script src="https://unpkg.com/obniz@3.14.0/obniz.js" crossorigin="anonymous" ></script> </head> <body> <div id="obniz-debug"></div> <h3>Connect From Your Browser</h3> <button class="btn btn-primary" id="on">LED ON</button> <button class="btn btn-primary" id="off">LED OFF</button> <div id="print"></div> <div> <input type="text" id="text" value="Hello World" /> <button class="btn btn-primary" id="showtime">Print on obniz</button> </div> <div>Demo Pin Assign</div> <ul> <li>io0: LED anode</li> <li>io1: LED cathode</li> <li>Switch State will be printed on browser</li> </ul> <script>
var obniz = new Obniz("7062-9865");
var obniz = new Obniz("YOUR_OBNIZ_ID");
obniz.onconnect = async function() { ////////////////////////////////////////////////////////// //デバイス側のAccGyroセンサの設定・データ取得 ////////////////////////////////////////////////////////// //MPU6050 ACC 単位換算[digit/g] const MPU6050_ACC_2G = 32768; const MPU6050_ACC_4G = 16384; const MPU6050_ACC_8G = 8192; const MPU6050_ACC_16G= 4096; //MPU6050 GYRO 単位換算[digit/(deg/sec)] const MPU6050_GYRO_250 = 262; const MPU6050_GYRO_500 = 131; const MPU6050_GYRO_1000= 65; const MPU6050_GYRO_2000= 32; var device_acc_value=0; var device_gyro_value=0; await obniz.ble.initWait(); //デバイス側の加速度・ジャイロ値を取得。（活動量として計算された後の値で取得。） obniz.ble.scan.onfind = async function(ret){ //上位バイトと下位バイトを連結し、単位換算 device_acc_value = ((ret.adv_data[17]*256) + ret.adv_data[18])/MPU6050_ACC_4G; device_gyro_value =((ret.adv_data[19]*256) + ret.adv_data[20])/MPU6050_GYRO_500; }; obniz.ble.scan.onfinish = async function(ret, error){ console.log("scan timeout!"); }; var target = { localName: "ESP32" }; var setting = { duration: null, // 60 sec duplicate: true, // allow duplicate filterOnDevice:true } await obniz.ble.scan.startWait(target, setting); ////////////////////////////////////////////////////////// //ホスト側のAccGyroセンサの設定・データ取得 ////////////////////////////////////////////////////////// //ACCで判断した活動スレッシュ[g] const ACC_ACTIVE_THRESHOLD0 = 0.0; //静止 const ACC_ACTIVE_THRESHOLD1 = 0.2; //交通手段移動中 const ACC_ACTIVE_THRESHOLD2 = 0.4; //徒歩(実測:0.5-1.2g) const ACC_ACTIVE_THRESHOLD3 = 1.5; //走り(実測:1.5-2.0g) //GYROで判断した活動スレッシュ[deg/sec] const GYRO_ACTIVE_THRESHOLD0 = 0; //静止 const GYRO_ACTIVE_THRESHOLD1 = 15; //交通手段移動中 const GYRO_ACTIVE_THRESHOLD2 = 30; //徒歩(40-80deg/sec) const GYRO_ACTIVE_THRESHOLD3 = 50; //走り(80-100deg/sec)※走りと徒歩で差が出ない場合もある //活動レベル const ACTIVE_LEVEL0 = 0; //静止 const ACTIVE_LEVEL1 = 1; //交通手段移動中 const ACTIVE_LEVEL2 = 2; //徒歩 const ACTIVE_LEVEL3 = 3; //走り var data; var acc_value; var gyro_value; var device_acc_value_before=0; var device_gyro_value_before=0; var ret_acc; var ret_gyro; var acc_x = 0; var acc_y = 0; var acc_z = 0; var gyro_x = 0; var gyro_y = 0; var gyro_z = 0; var acc_x_before=0; var acc_y_before=0; var acc_z_before=0; var gyro_x_before; var gyro_y_before; var gyro_z_before; //判定結果（活動レベル 0～4段階） var acc_level=0; var device_acc_level=0; var gyro_level=0; var device_gyro_level=0; const SAMPLE_NUM = 10; const SAMPLE_PERIOD = 50; obniz.io4.output(false); //AD0=Lo var sensor = obniz.wired("MPU6050", {scl: 0, sda: 1 , ad0:0x68}); sensor.setConfig(4, 500); obniz.io7.output(false); //vcc obniz.io9.output(false); //gnd var pwm = obniz.getFreePwm(); // await obniz.wait(5000); //ホストデータ取得・計算、活動レベル判定// // acc = gyro = abs(Δx)+abs(Δy)+abs(Δz) while(obniz.connectionState === 'connected') { try { await obniz.pingWait(); // 通信を確認 ret_acc=0; ret_gyro=0; acc_x = 0; acc_y = 0; acc_z = 0; gyro_x = 0; gyro_y = 0; gyro_z = 0; for (let i = 0; i < SAMPLE_NUM; i++) { data = await sensor.getWait(); //Acc 絶対値変換 if(data.accelerometer.x>0){ acc_x = data.accelerometer.x; }else{ acc_x = -data.accelerometer.x; } if(data.accelerometer.y>0){ acc_y = data.accelerometer.y; }else{ acc_y = -data.accelerometer.y; } if(data.accelerometer.z>0){ acc_z = data.accelerometer.z; }else{ acc_z = -data.accelerometer.z; } //Gyro 絶対値変換 if(data.gyroscope.x>0){ gyro_x = data.gyroscope.x; }else{ gyro_x = -data.gyroscope.x; } if(data.gyroscope.y>0){ gyro_y = data.gyroscope.y; }else{ gyro_y = -data.gyroscope.y; } if(data.gyroscope.z>0){ gyro_z = data.gyroscope.z; }else{ gyro_z = -data.gyroscope.z; } //Acc計算 if(acc_x > acc_x_before){ ret_acc += acc_x - acc_x_before; }else{ ret_acc += acc_x_before - acc_x; } if(acc_y > acc_y_before){ ret_acc += acc_y - acc_y_before; }else{ ret_acc += acc_y_before - acc_y; } if(acc_z > acc_z_before){ ret_acc += acc_z - acc_z_before; }else{ ret_acc += acc_z_before - acc_z; } //Gyro計算 if(gyro_x > gyro_x_before){ ret_gyro += gyro_x - gyro_x_before; }else{ ret_gyro += gyro_x_before - gyro_x; } if(gyro_y > gyro_y_before){ ret_gyro += gyro_y - gyro_y_before; }else{ ret_gyro += gyro_y_before - gyro_y; } if(gyro_z > gyro_z_before){ ret_gyro += gyro_z - gyro_z_before; }else{ ret_gyro += gyro_z_before - gyro_z; } acc_x_before = acc_x; acc_y_before = acc_y; acc_z_before = acc_z; gyro_x_before = gyro_x; gyro_y_before = gyro_y; gyro_z_before = gyro_z; await obniz.wait(SAMPLE_PERIOD); } acc_value = ret_acc / SAMPLE_NUM; gyro_value = ret_gyro / SAMPLE_NUM; // console.log('acc_value: ',acc_value); // console.log('gyro_value: ', gyro_value); // console.log(data); //判定処理本体 //デバイス側のデータが更新された場合に実施する。 //デバイス側とホスト側で活動レベルが異なった場合はブザー等で通知する if((device_acc_value!=device_acc_value_before)||(device_gyro_value!=device_gyro_value_before)){ //Accの判定 if(acc_value < ACC_ACTIVE_THRESHOLD1){ acc_level = ACTIVE_LEVEL0; //静止 }else if(acc_value < ACC_ACTIVE_THRESHOLD2){ acc_level = ACTIVE_LEVEL1; //交通手段移動中 }else if(acc_value < ACC_ACTIVE_THRESHOLD3){ acc_level = ACTIVE_LEVEL2; //徒歩 }else if(acc_value >= ACC_ACTIVE_THRESHOLD3){ acc_level = ACTIVE_LEVEL3; //走り } if(device_acc_value < ACC_ACTIVE_THRESHOLD1){ device_acc_level = ACTIVE_LEVEL0; //静止 }else if(device_acc_value < ACC_ACTIVE_THRESHOLD2){ device_acc_level = ACTIVE_LEVEL1; //交通手段移動中 }else if(device_acc_value < ACC_ACTIVE_THRESHOLD3){ device_acc_level = ACTIVE_LEVEL2; //徒歩 }else if(device_acc_value >= ACC_ACTIVE_THRESHOLD3){ device_acc_level = ACTIVE_LEVEL3; //走り } //Gyroの判定 if(gyro_value < GYRO_ACTIVE_THRESHOLD1){ gyro_level = ACTIVE_LEVEL0; //静止 }else if(gyro_value < GYRO_ACTIVE_THRESHOLD2){ gyro_level = ACTIVE_LEVEL1; //交通手段移動中 }else if(gyro_value < GYRO_ACTIVE_THRESHOLD3){ gyro_level = ACTIVE_LEVEL2; //徒歩 }else if(gyro_value >= GYRO_ACTIVE_THRESHOLD3){ gyro_level = ACTIVE_LEVEL3; //走り } if(device_gyro_value < GYRO_ACTIVE_THRESHOLD1){ device_gyro_level = ACTIVE_LEVEL0; //静止 }else if(device_gyro_value < GYRO_ACTIVE_THRESHOLD2){ device_gyro_level = ACTIVE_LEVEL1; //交通手段移動中 }else if(device_gyro_value < GYRO_ACTIVE_THRESHOLD3){ device_gyro_level = ACTIVE_LEVEL2; //徒歩 }else if(device_gyro_value >= GYRO_ACTIVE_THRESHOLD3){ device_gyro_level = ACTIVE_LEVEL3; //走り } obniz.display.clear(); // obniz.display.print(acc_value); // obniz.display.print(device_acc_value); // obniz.display.print(gyro_value); // obniz.display.print(device_gyro_value); obniz.display.print(acc_level); obniz.display.print(device_acc_level); obniz.display.print(gyro_level); obniz.display.print(device_gyro_level); //ホスト側が徒歩以上の活動レベルである場合 if((acc_level >= ACTIVE_LEVEL2)&&(gyro_level >= ACTIVE_LEVEL2)){ if((device_acc_level < ACTIVE_LEVEL1)&&(device_gyro_level < ACTIVE_LEVEL1)){ //デバイス側が交通手段移動中未満の活動レベルである場合 //置き忘れ obniz.io7.output(true); //vcc obniz.io9.output(false); //gnd pwm.start({io:8}); pwm.freq(440); // 440Hz pwm.duty(50) // 50% }else{ //ホストとデバイスの活動レベルが同期している状態 //徒歩 //走行 obniz.io8.output(false); //signal obniz.io7.output(false); //vcc obniz.io9.output(false); //gnd } console.log('acc_level: ',acc_level); console.log('device_acc_level: ',device_acc_level); }else{ //ホスト側が徒歩以下の活動レベルである場合 //デバイス・ホスト両方放置 //信号待ち //交通手段移動中 obniz.io8.output(false); //signal obniz.io7.output(false); //vcc obniz.io9.output(false); //gnd } device_acc_value_before = device_acc_value; device_gyro_value_before= device_gyro_value; } else { //デバイス側のBLEが受信失敗状態 obniz.display.clear(); obniz.display.print("Out of Range"); obniz.io8.output(false); //signal obniz.io7.output(false); //vcc obniz.io9.output(false); //gnd } } catch(e) { // break or throw a error } ////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////// } var led = obniz.wired("LED", { anode: 0, cathode: 1 }); }; </script> </body> </html> ``` ```arduino:デバイス #include <Arduino.h> #include <Wire.h> #include <BLEDevice.h> #include <BLEUtils.h> #include <BLEServer.h> // ******************************************************************************************************************************* // START userdefined data // ******************************************************************************************************************************* #define DEVICE_NAME "ESP32" // デバイス名 #define DEVICE_NUMBER 1 // デバイス識別番号（1～8） RTC_DATA_ATTR static uint8_t seq_number; // RTCメモリー上のシーケンス番号 // MPU-6050 Accelerometer + Gyro // レジスタアドレス #define MPU6050_I2C_SDA 22 #define MPU6050_I2C_SCL 21 #define MPU6050_ACCEL_XOUT_H 0x3B // R #define MPU6050_WHO_AM_I 0x75 // R #define MPU6050_PWR_MGMT_1 0x6B // R/W #define MPU6050_I2C_ADDRESS 0x68 // 構造体定義 typedef union accel_t_gyro_union { struct { uint8_t x_accel_h; uint8_t x_accel_l; uint8_t y_accel_h; uint8_t y_accel_l; uint8_t z_accel_h; uint8_t z_accel_l; uint8_t t_h; uint8_t t_l; uint8_t x_gyro_h; uint8_t x_gyro_l; uint8_t y_gyro_h; uint8_t y_gyro_l; uint8_t z_gyro_h; uint8_t z_gyro_l; } reg; struct { int16_t x_accel; int16_t y_accel; int16_t z_accel; int16_t temperature; int16_t x_gyro; int16_t y_gyro; int16_t z_gyro; } value; }; // MPU-6050 Accelerometer + Gyro /////////////////////////////////////// // ******************************************************************************************************************************* // END userdefined data // ******************************************************************************************************************************* void setup() { Serial.begin(115200); Serial.println("Void Setup"); // MPU6050動作開始 Wire.begin(MPU6050_I2C_SDA, MPU6050_I2C_SCL); // obnizサンプルコードの通りシーケンスを実行 MPU6050_write_reg(0x6B, 0x00); delay(2); MPU6050_write_reg(0x1A, 0x01); delay(2); MPU6050_write_reg(0x19, 0x05); delay(2); MPU6050_write_reg(0x38, 0x00); delay(2); MPU6050_write_reg(0x6A, 0x00); delay(2); MPU6050_write_reg(0x23, 0x00); delay(4); MPU6050_write_reg(0x37, 0x22); delay(2); MPU6050_write_reg(0x38, 0x01); delay(2); MPU6050_write_reg(0x1C, 0x00); MPU6050_write_reg(0x1B, 0x00); MPU6050_write_reg(0x1C, 0x00); MPU6050_write_reg(0x1B, 0x00); delay(100); BLEDevice::init(DEVICE_NAME); // BLEデバイスを初期化する // BLEサーバーを作成してアドバタイズオブジェクトを取得する BLEServer *pServer = BLEDevice::createServer(); BLEAdvertising *pAdvertising = pServer->getAdvertising(); // アドバタイズ開始 pAdvertising->start(); Serial.println("Advertising started!"); while(1){ // 送信情報を設定してシーケンス番号をインクリメントする setAdvertisementData(pAdvertising); //センサ値取得・活動レベル計算、データ送信 seq_number++; } } void loop() { } void setAdvertisementData(BLEAdvertising* pAdvertising) { float acc_tmp=0; float gyro_tmp=0; float acc_value=0; float gyro_value=0; uint16_t acc_value_16 = 0; uint16_t gyro_value_16 = 0; const uint8_t sample_num = 10; //加速度ジャイロの単純移動平均を取得する acc_tmp = 0; gyro_tmp = 0; acc_value = 0; gyro_value = 0; for(uint8_t i=0; i<sample_num; i++){ acc_gyro_process(&acc_tmp,&gyro_tmp); acc_value += acc_tmp; gyro_value += gyro_tmp; delay(50); } acc_value = acc_value/sample_num; gyro_value = gyro_value/sample_num; acc_value_16 = (uint16_t)acc_value; gyro_value_16 = (uint16_t)gyro_value; Serial.print("acc_value:"); Serial.print(acc_value/16384, 2); Serial.print(", gyro_value:"); Serial.print(gyro_value/131, 2); Serial.println(""); Serial.print("acc_value_16:"); Serial.print(acc_value_16, 2); Serial.print(", gyro_value_16:"); Serial.print(gyro_value_16, 2); Serial.println(""); // string領域に送信情報を連結する std::string strData = ""; strData += (char)0xff; // Manufacturer specific data strData += (char)0xff; // manufacturer ID low byte strData += (char)0xff; // manufacturer ID high byte strData += (char)DEVICE_NUMBER; // サーバー識別番号 strData += (char)0xAA; // 異常状態判定 strData += (char)seq_number; // シーケンス番号 strData += (char)((acc_value_16 >> 8) & 0xff); // acc活動量の上位バイト strData += (char)(acc_value_16 & 0xff); // acc活動量の下位バイト strData += (char)((gyro_value_16 >> 8) & 0xff); // gyro活動量の上位バイト strData += (char)(gyro_value_16 & 0xff); // gyro活動量の下位バイト strData = (char)strData.length() + strData; // 先頭にLengthを設定 // デバイス名とフラグをセットし、送信情報を組み込んでアドバタイズオブジェクトに設定する BLEAdvertisementData oAdvertisementData = BLEAdvertisementData(); oAdvertisementData.setName(DEVICE_NAME); oAdvertisementData.setFlags(0x06); // LE General Discoverable Mode | BR_EDR_NOT_SUPPORTED oAdvertisementData.addData(strData); pAdvertising->setAdvertisementData(oAdvertisementData); } /* * 加速度センサ値を取得し、活動レベルを返す * 活動レベル= ( abs(Δx)+abs(Δy)+abs(Δz)+GyrX+GyrY+GyrZ )のTBD秒間の移動平均 */ void acc_gyro_process(float *acc_value, float *gyro_value){ int error; float ret_acc; float ret_gyro; static float acc_x_before=0; static float acc_y_before=0; static float acc_z_before=0; static float gyro_x_before=0; static float gyro_y_before=0; static float gyro_z_before=0; accel_t_gyro_union accel_t_gyro; Wire.begin(MPU6050_I2C_SDA, MPU6050_I2C_SCL); // 加速度、角速度の読み出し // accel_t_gyroは読み出した値を保存する構造体、その後ろの引数は取り出すバイト数 error = MPU6050_read(MPU6050_ACCEL_XOUT_H, (uint8_t *)&accel_t_gyro, sizeof(accel_t_gyro)); // 取得できるデータはビッグエンディアンなので上位バイトと下位バイトの入れ替え（AVRはリトルエンディアン） uint8_t swap; #define SWAP(x,y) swap = x; x = y; y = swap SWAP (accel_t_gyro.reg.x_accel_h, accel_t_gyro.reg.x_accel_l); SWAP (accel_t_gyro.reg.y_accel_h, accel_t_gyro.reg.y_accel_l); SWAP (accel_t_gyro.reg.z_accel_h, accel_t_gyro.reg.z_accel_l); SWAP (accel_t_gyro.reg.t_h, accel_t_gyro.reg.t_l); SWAP (accel_t_gyro.reg.x_gyro_h, accel_t_gyro.reg.x_gyro_l); SWAP (accel_t_gyro.reg.y_gyro_h, accel_t_gyro.reg.y_gyro_l); SWAP (accel_t_gyro.reg.z_gyro_h, accel_t_gyro.reg.z_gyro_l); // （未実施。ホスト側で実施する。）取得した加速度値を分解能で割って加速度(G)に変換する float acc_x = accel_t_gyro.value.x_accel;// / 16384.0; //FS_SEL_0 16,384 LSB / g float acc_y = accel_t_gyro.value.y_accel;// / 16384.0; float acc_z = accel_t_gyro.value.z_accel;// / 16384.0; // （未実施。ホスト側で実施する。）取得した角速度値を分解能で割って角速度(degrees per sec)に変換する float gyro_x = accel_t_gyro.value.x_gyro;// / 131.0;//FS_SEL_0 131 LSB / (°/s) float gyro_y = accel_t_gyro.value.y_gyro;// / 131.0; float gyro_z = accel_t_gyro.value.z_gyro;// / 131.0; //デバイスデータ計算// // acc = gyro = abs(Δx)+abs(Δy)+abs(Δz) //絶対値変換 if(acc_x < 0){ acc_x = -acc_x; } if(acc_y < 0){ acc_y = -acc_y; } if(acc_z < 0){ acc_z = -acc_z; } if(gyro_x < 0){ gyro_x = -gyro_x; } if(gyro_y < 0){ gyro_y = -gyro_y; } if(gyro_z < 0){ gyro_z = -gyro_z; } //Acc計算 if(acc_x > acc_x_before){ ret_acc += acc_x - acc_x_before; }else{ ret_acc += acc_x_before - acc_x; } if(acc_y > acc_y_before){ ret_acc += acc_y - acc_y_before; }else{ ret_acc += acc_y_before - acc_y; } if(acc_z > acc_z_before){ ret_acc += acc_z - acc_z_before; }else{ ret_acc += acc_z_before - acc_z; } //Gyr計算 if(gyro_x > gyro_x_before){ ret_gyro += gyro_x - gyro_x_before; }else{ ret_gyro += gyro_x_before - gyro_x; } if(gyro_y > gyro_y_before){ ret_gyro += gyro_y - gyro_y_before; }else{ ret_gyro += gyro_y_before - gyro_y; } if(gyro_z > gyro_z_before){ ret_gyro += gyro_z - gyro_z_before; }else{ ret_gyro += gyro_z_before - gyro_z; } acc_x_before = acc_x; acc_y_before = acc_y; acc_z_before = acc_z; gyro_x_before = gyro_x; gyro_y_before = gyro_y; gyro_z_before = gyro_z; *acc_value = ret_acc;//(uint16_t)ret_acc; *gyro_value = ret_gyro;//(uint16_t)ret_gyro; } // MPU6050_read int MPU6050_read(int start, uint8_t *buffer, int size) { int i, n, error; Wire.beginTransmission(MPU6050_I2C_ADDRESS); n = Wire.write(start); if (n != 1) { return (-10); } n = Wire.endTransmission(false);// hold the I2C-bus if (n != 0) { return (n); } // Third parameter is true: relase I2C-bus after data is read. Wire.requestFrom(MPU6050_I2C_ADDRESS, size, true); i = 0; while (Wire.available() && i < size) { buffer[i++] = Wire.read(); } if ( i != size) { return (-11); } return (0); // return : no error } // MPU6050_write int MPU6050_write(int start, const uint8_t *pData, int size) { int n, error; Wire.beginTransmission(MPU6050_I2C_ADDRESS); n = Wire.write(start);// write the start address if (n != 1) { return (-20); } n = Wire.write(pData, size);// write data bytes if (n != size) { return (-21); } error = Wire.endTransmission(true); // release the I2C-bus if (error != 0) { return (error); } return (0);// return : no error } // MPU6050_write_reg int MPU6050_write_reg(int reg, uint8_t data) { int error; error = MPU6050_write(reg, &data, 1); Serial.print("error = "); Serial.println(error); return (error); }; ```