Diff: main.cpp

Revision:

5:a0e50699bfca

Parent:

4:8df0fc5dfd81

diff -r 8df0fc5dfd81 -r a0e50699bfca main.cpp
--- a/main.cpp	Sat Dec 05 05:07:00 2015 +0000
+++ b/main.cpp	Sat Dec 19 13:47:57 2015 +0000
@@ -5,22 +5,22 @@
 #include "Matrix.h"
 #include "Vector.h"
 #include "MPU6050.h"
-#include "HMC5883L.h"
+//#include "HMC5883L.h"
 /********** private define **********/
 /********** private macro **********/
 /********** private typedef **********/
 /********** private variables **********/
 const static float dt = 1.0f;
 
-DigitalOut myled(LED1);                 // デバッグ用LEDのためのデジタル出力
+//DigitalOut myled(LED1);                 // デバッグ用LEDのためのデジタル出力
 Serial pc(USBTX, USBRX);        // PC通信用シリアルポート
-I2C i2c(P0_13, P0_15);                  // mpu6050用I2Cオブジェクト
+I2C i2c(p9,p10);                  // mpu6050用I2Cオブジェクト
 Ticker INS_ticker;                      // 割り込み用タイマー
 MPU6050 mpu6050(&i2c);
-HMC5883L hmc5883l(&i2c);
+//HMC5883L hmc5883l(&i2c);
 
 const int mpu6050_addr = 0xd0;          // mpu6050アドレス
-const int hmc5883l_addr = 0x3C;         // hmc5883lアドレス
+//const int hmc5883l_addr = 0x3C;         // hmc5883lアドレス
 volatile int ret = 0;                   // I2C関数の返り値保存用
 uint8_t cmd[2] = {};                    // I2C送信データ
 uint8_t data[14] = {};                  // I2C受信データ
@@ -36,7 +36,7 @@
 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];                         // デバッグ用文字列
 
@@ -50,7 +50,7 @@
     
     i2c.frequency(400000);                      // mpu6050との通信は400kHz
     mpu6050.init();
-    hmc5883l.init();
+    //hmc5883l.init();
     
     // 0x6bレジスタに0x00を書き込んでmpu6050のスリープモードを解除
     cmd[0] = 0x6b;
@@ -78,10 +78,10 @@
     // 割り込み周期は10ms（10000μs）
     INS_ticker.attach_us(&INS_IntFunc, 1000000 * dt);    
     
-    while(1) {
+    //while(1) {
        // メインループではひたすらLEDチカチカ
-       myled = 0; // LED is ON
-       wait(10); // 50 ms
+      // 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));
@@ -93,10 +93,10 @@
 //            pc.printf("%.3f\t", v.GetComp(2));
 //            pc.printf("%.3f\r\n", v.GetComp(3));
             
-       myled = 0; // LED is OFF
-       wait(10); // 1 sec
+       //myled = 0; // LED is OFF
+  //     wait(10); // 1 sec
            
-    }
+    //}
 }
 
 void INS_IntFunc() {
@@ -107,7 +107,7 @@
         i2c.read(mpu6050_addr | 0x01, (char*)data, 14, false);
         
         mpu6050.read();
-        hmc5883l.read();
+     //   hmc5883l.read();
         
         // 各データを加速度、角速度にそれぞれ突っ込む
         for(int i=0; i<3; i++) {