Revision 40:4188d55e62fc, committed 2015-12-25

Comitter:

YusukeWakuta

Date:

Fri Dec 25 18:22:15 2015 +0000

Parent:

39:9ca388ce902f

Child:

41:8fd7cdca0891

Commit message:

12/26??

Changed in this revision

--- a/Vector/Vector.h	Wed Dec 23 09:59:35 2015 +0000
+++ b/Vector/Vector.h	Fri Dec 25 18:22:15 2015 +0000
@@ -4,6 +4,9 @@
 class Matrix;
 
 class Vector {
+    private:
+    int dim;
+    float* components;
 public:
     Vector(int dim);
     ~Vector();
@@ -39,9 +42,6 @@
 
     void CleanUp();
 
-private:
-    int dim;
-    float* components;
 
     Vector& operator*=(const Matrix& m);
     Vector& operator*=(const Vector& m);

--- a/main.cpp	Wed Dec 23 09:59:35 2015 +0000
+++ b/main.cpp	Fri Dec 25 18:22:15 2015 +0000
@@ -19,7 +19,7 @@
 /****************************** private typedef   ******************************/
 /****************************** private variables ******************************/
 DigitalOut      myled(LED1);                        // デバッグ用LEDのためのデジタル出力
-I2C             i2c(p9,p10);                    // I2Cポート
+I2C             i2c(p28,p27);                    // I2Cポート
 MPU6050         mpu(&i2c);                          // 加速度・角速度センサ
 //HMC5883L        hmc(&i2c);                          // 地磁気センサ
 LPS25H          lps(&i2c);                          // 気圧センサ
@@ -139,6 +139,7 @@
 {
     //↓mpuのスリープモードを解除するコードを含んだ関数（SensorsInit()）が宣言だけされて実装されてなかったので追加(w)
     SensorsInit();
+    //↓i2c.startたしたけど変化なし 
     i2c.start();
 pc.printf("HELLO");
     i2c.frequency(400000);                  // I2Cの通信速度を400kHzに設定
@@ -149,12 +150,12 @@
 
     /* ------------------------------ ↓↓↓ ここからメインループ ↓↓↓ ------------------------------ */
     while(1) {
+       pc.printf("roll:%7f,pitch:%7f,yaw:%7f\n\r",roll,pitch,yaw);
        // pc.printf("HELLO");
         timer.stop();
         timer.reset();
         timer.start();
         myled = 1;                                          // LED is ON
-
         INT_flag = false;                               // 割り込みによる変数書き換えを阻止
         /******************************* データ処理 *******************************/
         DataProcessing();
@@ -166,6 +167,7 @@
         boolcheck = 0;
         pc.printf("%d",boolcheck);
         //==================================================== */
+        wait_ms(10);
     }
     /* ------------------------------ ↑↑↑ ここまでメインループ ↑↑↑ ------------------------------ */
 }
@@ -213,9 +215,12 @@
 
     if(yaw > 180.0f) yaw -= 360.0f;                 // ヨー角を[-π, π]に補正
     else if(yaw < -180.0f) yaw += 360.0f;           // ヨー角を[-π, π]に補正
-   
+    //下のroll,pitch,yawを表示させるprintfでnanと出るので、変数の中に何も値が代入されていない可能性を調べた。→値は入ってるらしい。(w)========================
+       pc.printf("%p",&roll);
+       }
+       //=======================================================================================================================
+       
    //pc.printf("roll:%7f,pitch:%7f,yaw:%7f\n\r",roll,pitch,yaw);
-}
 
 
 /** 各種センサーの初期化を行う関数
@@ -409,14 +414,25 @@
         raw_gyro.SetComp(i+1, (float)mpu.data.value.gyro[i] * GYRO_LSB_TO_DEG * DEG_TO_RAD);
      //   raw_geomag.SetComp(i+1, (float)hmc.data.value[i] * MAG_LSB_TO_GAUSS);
     }
-
+   /* for(int i = 0;i < 3;i++){
+        pc.printf("acc:%18fgyro:%18f",raw_acc.getter[i](),raw_gyro.getter[i]());
+        }
+        pc.printf("\n\r\n\r");
+     */   
     Vector delta_g = Cross(raw_gyro, raw_g);                              // Δg = ω × g
     raw_g = 0.9f * (raw_g - delta_g * dt) + 0.1f * raw_acc.Normalize();   // 相補フィルタ
     raw_g = raw_g.Normalize();
 
     KalmanUpdate();
-
-    if(INT_flag) {
+    /*//check INT_flag true or false(w)=================
+        if(INT_flag)
+        boolcheck = 1;
+        else if(!INT_flag)
+        boolcheck = 0;
+        pc.printf("%d",boolcheck);
+        //====================================================*/
+    if(1) {
+        
         g = raw_g;
         for(int i=0; i<3; i++) {
             geomag.SetComp(i+1, post_x1.GetComp(i+1));