Diff: main.cpp

Revision:

0:5e220b09d315

Child:

1:2eca9b376580

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Apr 13 08:10:05 2015 +0000
@@ -0,0 +1,83 @@
+#include "mbed.h"
+#include "toString.h"
+/********** private typedef **********/
+/********** public variables **********/
+/********** public functions **********/
+/********** private variables **********/
+
+DigitalOut myled(LED1);                 // デバッグ用LEDのためのデジタル出力
+Serial pc(SERIAL_TX, SERIAL_RX);        // PC通信用シリアルポート
+I2C mpu6050(D14, D15);                  // mpu6050用I2Cオブジェクト
+Ticker INS_ticker;                      // 割り込み用タイマー
+
+const int addr = 0xd0;                  // mpu6050アドレス
+volatile int ret = 0;                   // I2C関数の返り値保存用
+uint8_t cmd[2] = {};                    // I2C送信データ
+uint8_t data[14] = {};                  // I2C受信データ
+uint16_t Tempr = 0;                     // 温度
+int16_t acc[3] = {};                    // 加速度
+int16_t gyro[3] = {};                   // 角速度
+float theta[2] = {};                    // ロール、ピッチ角
+
+char text[256];                         // デバッグ用文字列
+
+/********** private functions **********/
+
+void INS_IntFunc();                     // センサー値取得用割り込み関数
+
+/********** main function **********/
+
+int main() {
+    
+    mpu6050.frequency(400000);                      // mpu6050との通信は400kHz
+    
+    // 0x6bレジスタに0x00を書き込んでmpu6050のスリープモードを解除
+    cmd[0] = 0x6b;
+    cmd[1] = 0x00;
+    ret = mpu6050.write(addr, (char*)cmd, 2);
+    pc.printf("ret = %d ", ret);
+    
+    // センサー値の取得・計算は割り込み関数内で行う。
+    // 割り込み周期は10ms（10000μs）
+    INS_ticker.attach_us(&INS_IntFunc, 10000);
+    
+    while(1) {
+       // メインループではひたすらLEDチカチカ
+       myled = 1; // LED is ON
+       wait(0.2); // 200 ms
+       myled = 0; // LED is OFF
+       wait(1.0); // 1 sec
+           
+    }
+}
+
+void INS_IntFunc() {
+        
+        // 0x3bレジスタからデータの読み取りを行う
+        cmd[0] = 0x3b;
+        ret = mpu6050.write(addr, (char*)cmd, 1, true);
+        mpu6050.read(addr | 0x01, (char*)data, 14, false);
+        
+        // 各データを加速度、角速度にそれぞれ突っ込む
+        for(int i=0; i<3; i++) {
+            acc[i] = ((int16_t)data[i*2])<<8 | ((int16_t)data[i*2+1]);
+        }
+        Tempr = ((int16_t)data[6])<<8 | ((int16_t)data[7]);
+        for(int i=4; i<7; i++) {
+            gyro[i-4] = ((int16_t)data[i*2])<<8 | ((int16_t)data[i*2+1]);
+        }
+        
+        // 各センサー値からセンサーの姿勢・角速度を計算
+        float roll_acc = (180.0f * atan2((float)acc[0], (float)acc[2]) / 3.1415f);
+        float roll_ratio_gyro = (float)gyro[1] / 65.5f;
+        float pitch_acc = (180.0f * atan2((float)acc[1], (float)acc[2]) / 3.1415f);
+        float pitch_ratio_gyro = (float)gyro[0] / 65.5f;
+        
+        // 相補フィルタを用いて角度を更新
+        theta[0] = 0.98f * (theta[0] + roll_ratio_gyro * 0.01f) + 0.02f * roll_acc;
+        theta[1] = 0.98f * (theta[1] + pitch_ratio_gyro * 0.01f) + 0.02f * pitch_acc;
+        
+        // 推定された角度をパソコンに送信
+        pc.printf("%.4f\t", theta[0]);
+        pc.printf("%.4f\r\n", theta[1]);
+}
\ No newline at end of file