Diff: main.cpp

Revision:

1:21ba826811d6

Parent:

0:ae23b58fc2d4

Child:

2:8ba36609face

Child:

3:7644f7b86ae0

--- a/main.cpp	Sun Aug 12 02:16:58 2018 +0000
+++ b/main.cpp	Sun Aug 12 06:34:00 2018 +0000
@@ -39,40 +39,64 @@
 
 int16_t mx, my, mz;
 double magX, magY, magZ, mag;
-float rad;
-float degree;
+int16_t ax, ay, az;
+float accX, accY, accZ, acc;
+double rad;
+double degree;
 float ID;
+double roll;
+double pitch;
+
 int main() {
     i2cWrite(MPU9250_ADDRESS, PWR, 0x00);  //スリープモードの解除
-    i2cWrite(MPU9250_ADDRESS, MAG_OPN, 0x02); //磁気センサの起動
-    i2cWrite(AK8963_ADDRESS, MAG_CONFIG, MAG_100HZ);//磁気センサの測定レンジの設定
     double maveX = 0, maveY = 0, maveZ = 0;
     while(1) {
-        for(int i = 0;i < 100;i++){
+        //加速度の値を取得し、落下判定
+        Ac_Read(&ax,&ay,&az);
+        accX = ax * accRange / 32768.0;//[G]に変換
+        accY = ay * accRange / 32768.0;//[G]に変換
+        accZ = az * accRange / 32768.0;//[G]に変換
+        roll = atan2(accY, accZ);
+        pitch = atan2(-accX, sin(accY) + cos(accZ));
+        i2cWrite(MPU9250_ADDRESS, MAG_OPN, 0x02); //磁気センサの起動
+        i2cWrite(AK8963_ADDRESS, MAG_CONFIG, MAG_100HZ);//磁気センサの測定レンジの設定
+        for(int i = 0;i < 150;i++){
             Mag_Read(&mx, &my, &mz);
             maveX += mx;
             maveY += my;
             maveZ += mz;
-            wait_ms(15);
+            wait_ms(10);
         }          
-        maveX /= 100;
-        maveY /= 100;
-        maveZ /= 100;
+        maveX /= 150;
+        maveY /= 150;
+        maveZ /= 150;
         magX = (maveX - 168.75) / 32768.0f * 4800.0f;//[uT]に変換
         magY = (maveY - 18.75) / 32768.0f * 4800.0f;//[uT]に変換
+        magZ = mz / 32768.0f * 4800.0f;//[uT]に変換
         pc.printf("%f,%f\n\r", magX, magY);
-        ID = IDcheck();
+        //ID = IDcheck();
         //pc.printf("%f\n\r", ID);
-        //magZ = mz / 32768.0f * 4800.0f;//[uT]に変換
-        rad = atan2(magY, magX);
-        degree = (180.0 * rad / 3.14);
+        rad = atan2(magZ * sin(roll) - magY * cos(roll), magX * cos(pitch) + magY * sin(pitch) * sin(roll) + magZ * sin(pitch) * cos(roll));
+        degree = -((int)(rad * 180.0 / 3.141592 + 270.0 - 7.5) % 360 - 360.0);
+        /*
         if(degree < 0){
             degree += 360;    
         }
-        //pc.printf("%f\n\r",degree);
+        */
+        pc.printf("%d\n\r",(int)degree);
     }
 }
 
+//mpu9250から加速度センサのみ引っ張ってくる
+void Ac_Read(int16_t* ax, int16_t* ay, int16_t* az){
+  i2cWrite(MPU9250_ADDRESS, PWR, 0x00);  //スリープモードの解除
+  i2cWrite(MPU9250_ADDRESS, ACC_CONFIG, ACC_16G);//加速度センサの測定レンジの設定
+  i2cRead(MPU9250_ADDRESS, 0x3b, 14, accgyrodata);
+  *ax = (accgyrodata[0] << 8) | accgyrodata[1];//accGyroTempData[0]を左に8シフトし(<<)，accGyroTempData[1]を足し合わせる(|) |は論理和
+  *ay = (accgyrodata[2] << 8) | accgyrodata[3];//accGyroTempData[2]を左に8シフトし(<<)，accGyroTempData[3]を足し合わせる(|)
+  *az = (accgyrodata[4] << 8) | accgyrodata[5];//accGyroTempData[4]を左に8シフトし(<<)，accGyroTempData[5]を足し合わせる(|)
+}
+
 //mpu9250から磁気センサのみ引っ張ってくる
 void Mag_Read(int16_t* mx, int16_t* my, int16_t* mz){
   i2cRead(AK8963_ADDRESS, ST2, 1, &ST2_Bit);//読み出し準備ができたか確認