Diff: main.cpp

Revision:

4:8df0fc5dfd81

Parent:

3:40559ebef0f1

Child:

5:a0e50699bfca

--- a/main.cpp	Wed May 13 04:02:27 2015 +0000
+++ b/main.cpp	Sat Dec 05 05:07:00 2015 +0000
@@ -10,33 +10,34 @@
 /********** private macro **********/
 /********** private typedef **********/
 /********** private variables **********/
-const static float dt = 0.01f;
+const static float dt = 1.0f;
 
 DigitalOut myled(LED1);                 // デバッグ用LEDのためのデジタル出力
-Serial pc(SERIAL_TX, SERIAL_RX);        // PC通信用シリアルポート
-I2C i2c(D14, D15);                  // mpu6050用I2Cオブジェクト
+Serial pc(USBTX, USBRX);        // PC通信用シリアルポート
+I2C i2c(P0_13, P0_15);                  // mpu6050用I2Cオブジェクト
 Ticker INS_ticker;                      // 割り込み用タイマー
 MPU6050 mpu6050(&i2c);
 HMC5883L hmc5883l(&i2c);
 
-//const int mpu6050_addr = 0xd0;          // mpu6050アドレス
-//const int hmc5883l_addr = 0x3C;         // hmc5883lアドレス
+const int mpu6050_addr = 0xd0;          // mpu6050アドレス
+const int hmc5883l_addr = 0x3C;         // hmc5883lアドレス
 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] = {};                   // 角速度
-//int16_t mag[3] = {};                    // 地磁気
-Vector acc(3);
-Vector gyro(3);
-Vector mag(3);
+int16_t acc[3] = {};                    // 加速度
+int16_t gyro[3] = {};                   // 角速度
+int16_t mag[3] = {};     
+               // 地磁気
+Vector Vacc(3);
+Vector Vgyro(3);
+Vector Vmag(3);
 Vector g(3);
 Vector n(3);
 Vector v_acc(3);
 Vector v(3);
-float theta[2] = {};                    // ロール、ピッチ角
-
+//float theta[2] = {};                    // ロール、ピッチ角
+float pitch = 0.0, roll = 0.0;
 char text[256];                         // デバッグ用文字列
 
 /********** private functions **********/
@@ -52,18 +53,18 @@
     hmc5883l.init();
     
     // 0x6bレジスタに0x00を書き込んでmpu6050のスリープモードを解除
-    /*cmd[0] = 0x6b;
+    cmd[0] = 0x6b;
     cmd[1] = 0x00;
     ret = i2c.write(mpu6050_addr, (char*)cmd, 2);
-    pc.printf("ret = %d ", ret);*/
+    //pc.printf("ret = %d ", ret);
     
     // 地磁気センサの初期値を取得
-    hmc5883l.read();
-    mag.SetComp(1, (float)hmc5883l.data.value[0]);
-    mag.SetComp(2, (float)hmc5883l.data.value[1]);
-    mag.SetComp(3, (float)hmc5883l.data.value[2]);
-    
-    mag = mag.Normalize();
+    //hmc5883l.read();
+//    mag.SetComp(1, (float)hmc5883l.data.value[0]);
+//    mag.SetComp(2, (float)hmc5883l.data.value[1]);
+//    mag.SetComp(3, (float)hmc5883l.data.value[2]);
+//    
+//    mag = mag.Normalize();
     
     v.SetComp(1, 0.0f);
     v.SetComp(2, 0.0f);
@@ -79,21 +80,21 @@
     
     while(1) {
        // メインループではひたすらLEDチカチカ
-       myled = 1; // LED is ON
-       wait(0.05); // 50 ms
-       
-            pc.printf("%.3f\t", g.GetComp(1));
-            pc.printf("%.3f\t", g.GetComp(2));
-            pc.printf("%.3f\t", g.GetComp(3));
-            pc.printf("%.3f\t", n.GetComp(1));
-            pc.printf("%.3f\t", n.GetComp(2));
-            pc.printf("%.3f\t", n.GetComp(3));
-            pc.printf("%.3f\t", v.GetComp(1));
-            pc.printf("%.3f\t", v.GetComp(2));
-            pc.printf("%.3f\r\n", v.GetComp(3));
+       myled = 0; // LED is ON
+       wait(10); // 50 ms
+            //pc.printf("testtest\n");
+            //pc.printf("%.3f\t", g.GetComp(1));
+//            pc.printf("%.3f\t", g.GetComp(2));
+//            pc.printf("%.3f\t", g.GetComp(3));
+//            pc.printf("%.3f\t", n.GetComp(1));
+//            pc.printf("%.3f\t", n.GetComp(2));
+//            pc.printf("%.3f\t", n.GetComp(3));
+//            pc.printf("%.3f\t", v.GetComp(1));
+//            pc.printf("%.3f\t", v.GetComp(2));
+//            pc.printf("%.3f\r\n", v.GetComp(3));
             
        myled = 0; // LED is OFF
-       wait(0.05); // 1 sec
+       wait(10); // 1 sec
            
     }
 }
@@ -101,94 +102,106 @@
 void INS_IntFunc() {
         
         // 0x3bレジスタからデータの読み取りを行う
-        /*cmd[0] = 0x3b;
+        cmd[0] = 0x3b;
         ret = i2c.write(mpu6050_addr, (char*)cmd, 1, true);
         i2c.read(mpu6050_addr | 0x01, (char*)data, 14, false);
         
+        mpu6050.read();
+        hmc5883l.read();
+        
         // 各データを加速度、角速度にそれぞれ突っ込む
         for(int i=0; i<3; i++) {
             int16_t acc_temp = 0;
             acc_temp = ((int16_t)data[i*2])<<8 | ((int16_t)data[i*2+1]);
-            acc.SetComp(i+1, -(float)acc_temp * ACC_LSB_TO_G);
+            //Vacc.SetComp(i+1, -(float)acc_temp * ACC_LSB_TO_G);
+            Vacc.SetComp(i+1, -(float)acc_temp);
         }
         Tempr = ((int16_t)data[6])<<8 | ((int16_t)data[7]);
         for(int i=4; i<7; i++) {
             int16_t gyro_temp = 0;
             gyro_temp = ((int16_t)data[i*2])<<8 | ((int16_t)data[i*2+1]);
-            gyro.SetComp(i-3, (float)gyro_temp * GYRO_LSB_TO_DEG * DEG_TO_RAD);
-        }*/
+            Vgyro.SetComp(i-3, (float)gyro_temp * GYRO_LSB_TO_DEG * DEG_TO_RAD);
+        }
         
-        mpu6050.read();
-        hmc5883l.read();
+        //mpu6050.read();
+//        hmc5883l.read();
         
         for(int i=0; i<3; i++) {
-            acc.SetComp(i+1, (float)mpu6050.data.value.acc[i] * ACC_LSB_TO_G);
-            gyro.SetComp(i+1, (float)mpu6050.data.value.gyro[i] * GYRO_LSB_TO_DEG * DEG_TO_RAD);
-            mag.SetComp(i+1, (float)hmc5883l.data.value[i]);
+            //Vacc.SetComp(i+1, (float)acc[i]);
+            Vacc.SetComp(i+1, (float)acc[i] * ACC_LSB_TO_G);
+            Vgyro.SetComp(i+1, (float)gyro[i] * GYRO_LSB_TO_DEG * DEG_TO_RAD);
+            //Vmag.SetComp(i+1, (float)hmc5883l.data.value[i]);
         }
         
-        //acc = acc.Normalize();      // 欲しいのは方向のみなので単位ベクトル化
-        //mag = mag.Normalize();      // 欲しいのは方向のみなので単位ベクトル化
+        Vacc = Vacc.Normalize();      // 欲しいのは方向のみなので単位ベクトル化
+        Vmag = Vmag.Normalize();      // 欲しいのは方向のみなので単位ベクトル化
         
-        /*pc.printf("%.4f\t", acc.GetComp(1));
-        pc.printf("%.4f\t", acc.GetComp(2));
-        pc.printf("%.4f\t", acc.GetComp(3));
-        pc.printf("%.4f\t", gyro.GetComp(1));
-        pc.printf("%.4f\t", gyro.GetComp(2));
-        pc.printf("%.4f\t", gyro.GetComp(3));
-        pc.printf("%.4f\t", mag.GetComp(1));
-        pc.printf("%.4f\t", mag.GetComp(2));
-        pc.printf("%.4f\r\n", mag.GetComp(3));
-        */
+       // pc.printf("%.4f\t", Vacc.GetComp(1));
+//        pc.printf("%.4f\t", Vacc.GetComp(2));
+//        pc.printf("%.4f\t", Vacc.GetComp(3));
+//        pc.printf("%.4f\t", Vgyro.GetComp(1));
+//        pc.printf("%.4f\t", Vgyro.GetComp(2));
+//        pc.printf("%.4f\t", Vgyro.GetComp(3));
+//        pc.printf("%.4f\t", Vmag.GetComp(1));
+//        pc.printf("%.4f\t", Vmag.GetComp(2));
+//        pc.printf("%.4f\r\n", Vmag.GetComp(3));
+//        
         // 重力ベクトルを推定
-        {
-            Vector delta_g = Cross(gyro, g);  // Δg = ω × g
-            Vector delta_n = Cross(gyro, n);  // Δf = ω × f
+        
+            Vector delta_g = Cross(Vgyro, g);  // Δg = ω × g
+            Vector delta_n = Cross(Vgyro, n);  // Δf = ω × f
             
             
             // 相補フィルタを使ってみる
             //g = g + delta * dt;
-            g = 0.9f * (g + delta_g * dt) + 0.1f * acc.Normalize();
+            g = 0.9f * (g + delta_g * dt) + 0.1f * Vacc.Normalize();
             g = g.Normalize();
-            n = 0.9f * (n + delta_n * dt) + 0.1f * mag.Normalize();
+            n = 0.9f * (n + delta_n * dt) + 0.1f * Vmag.Normalize();
             n = n.Normalize();
             
-            v_acc = G_TO_MPSS * (acc - (acc * g) * g);
+            v_acc = G_TO_MPSS * (Vacc - (Vacc * g) * g);
             
             v += v_acc * dt;
             
             // 推定結果をPCに送信
-            /*pc.printf("%.3f\t", g.GetComp(1));
-            pc.printf("%.3f\t", g.GetComp(2));
-            pc.printf("%.3f\t", g.GetComp(3));
-            pc.printf("%.3f\t", n.GetComp(1));
-            pc.printf("%.3f\t", n.GetComp(2));
-            pc.printf("%.3f\t", n.GetComp(3));
-            pc.printf("%.3f\t", v.GetComp(1));
-            pc.printf("%.3f\t", v.GetComp(2));
-            pc.printf("%.3f\r\n", v.GetComp(3));*/
+           // pc.printf("%.3f\t", g.GetComp(1));
+//            pc.printf("%.3f\t", g.GetComp(2));
+//            pc.printf("%.3f\t", g.GetComp(3));
+//            pc.printf("%.3f\t", n.GetComp(1));
+//            pc.printf("%.3f\t", n.GetComp(2));
+//            pc.printf("%.3f\t", n.GetComp(3));
+//            pc.printf("%.3f\t", v.GetComp(1));
+//            pc.printf("%.3f\t", v.GetComp(2));
+//            pc.printf("%.3f\r\n", v.GetComp(3));
             
             
             
             //pc.printf("%f\t", (float)mpu6050.data.value.gyro[0] * GYRO_LSB_TO_DEG);
-            //pc.printf("%f\t", (float)mpu6050.data.value.gyro[1] * GYRO_LSB_TO_DEG);
-            //pc.printf("%f\r\n", (float)mpu6050.data.value.gyro[2] * GYRO_LSB_TO_DEG);
-            
-        }
+//            pc.printf("%f\t", (float)mpu6050.data.value.gyro[1] * GYRO_LSB_TO_DEG);
+//            pc.printf("%f\r\n", (float)mpu6050.data.value.gyro[2] * GYRO_LSB_TO_DEG);
+//            
         
-        /*
+        
+        
         // 各センサー値からセンサーの姿勢・角速度を計算
+        
         float roll_acc = (atan2((float)acc[0], (float)acc[2]) * RAD_TO_DEG);
         float roll_ratio_gyro = (float)gyro[1] / 65.5f;
         float pitch_acc = (atan2((float)acc[1], (float)acc[2]) * RAD_TO_DEG);
         float pitch_ratio_gyro = (float)gyro[0] / 65.5f;
         
+       // for(int i = 0; i < 3; i++){
+//            g.
         // 相補フィルタを用いて角度を更新
-        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;
-        
+        roll = 0.98f * (theta[0] - roll_ratio_gyro * 0.01f) + 0.02f * roll_acc;
+        pitch = 0.98f * (theta[1] + pitch_ratio_gyro * 0.01f) + 0.02f * pitch_acc;
+
+        //pitch = 0.98f * atan((float)g.GetComp(1) / sqrt((float)g.GetComp(2)^2 + (float)g.GetComp(3)^2));
+//        roll = 0.98f * atan((float)g.GetComp(2) / sqrt((float)g.GetComp(1)^2 + (float)g.GetComp(3)^2));        
         // 推定された角度をパソコンに送信
+        //pc.printf("%.4f\t", Vacc.GetComp(1));
+       // pc.printf("%.4f\t", atan2((float)acc[0], (float)acc[2]));
         pc.printf("%.4f\t", theta[0]);
         pc.printf("%.4f\r\n", theta[1]);
-        */
+//        
 }
\ No newline at end of file