コウスケ スズキ / Mbed 2 deprecated MPU6050_cansat2

accel.v2

Dependencies:   mbed MPU6050 Math

main.cpp@8:a6a8997a8879, 2022-06-04 (annotated)

Committer:
kosukesuzuki
Date:
Sat Jun 04 10:26:02 2022 +0000
Revision:
8:a6a8997a8879
Parent:
7:8f914ead7fc0 
vertical from accel.v1

Who changed what in which revision?

UserRevisionLine numberNew contents of line
falconsyunya 0:4ba3fa671062 1 #include "mbed.h"
falconsyunya 0:4ba3fa671062 2 #include "MPU6050.h"
falconsyunya 0:4ba3fa671062 3
falconsyunya 3:157b19421485 4 MPU6050 mpu(p9,p10);//(SDA,SCL)のピン配置
falconsyunya 0:4ba3fa671062 5
kosukesuzuki 8:a6a8997a8879 6 const float dt = 0.01;
kosukesuzuki 8:a6a8997a8879 7
kosukesuzuki 8:a6a8997a8879 8 const float K1x = 0.9,K2x = 1 - K1x; //X
kosukesuzuki 8:a6a8997a8879 9 const float K1y = 0.9,K2y = 1 - K1y; //y
kosukesuzuki 8:a6a8997a8879 10 const float K1z = 0.9,K2z = 1 - K1z; //Z
kosukesuzuki 8:a6a8997a8879 11
kosukesuzuki 8:a6a8997a8879 12
falconsyunya 0:4ba3fa671062 13 int main() {
kosukesuzuki 8:a6a8997a8879 14 //float a = 0; //iのデータ
kosukesuzuki 8:a6a8997a8879 15 float xi =0; //i-1のデータ　oldaccel
kosukesuzuki 8:a6a8997a8879 16 float yi =0;
kosukesuzuki 8:a6a8997a8879 17 float zi =0;
kosukesuzuki 8:a6a8997a8879 18
kosukesuzuki 8:a6a8997a8879 19 float Ax = 0; //reading STEINER
kosukesuzuki 8:a6a8997a8879 20 float Ay = 0;
kosukesuzuki 8:a6a8997a8879 21 float Az = 0;
kosukesuzuki 8:a6a8997a8879 22
kosukesuzuki 8:a6a8997a8879 23 float LPFx = 0, HPFx = 0;
kosukesuzuki 8:a6a8997a8879 24 float LPFy = 0, HPFy = 0;
kosukesuzuki 8:a6a8997a8879 25 float LPFz = 0, HPFz = 0;
kosukesuzuki 8:a6a8997a8879 26
kosukesuzuki 8:a6a8997a8879 27 //float HPFa = 0; //使わない
kosukesuzuki 8:a6a8997a8879 28
kosukesuzuki 8:a6a8997a8879 29 float Xspeed = 0;
kosukesuzuki 8:a6a8997a8879 30 float Yspeed = 0;
kosukesuzuki 8:a6a8997a8879 31 float Zspeed = 0;
kosukesuzuki 8:a6a8997a8879 32
kosukesuzuki 8:a6a8997a8879 33 float SPEED = 0;
kosukesuzuki 8:a6a8997a8879 34
kosukesuzuki 8:a6a8997a8879 35 int accel[3];//accelを3つの配列で定義
kosukesuzuki 8:a6a8997a8879 36
falconsyunya 0:4ba3fa671062 37 while(1){
kosukesuzuki 8:a6a8997a8879 38 xi = Ax;
kosukesuzuki 8:a6a8997a8879 39 yi = Ay;
kosukesuzuki 8:a6a8997a8879 40 zi = Az;
kosukesuzuki 8:a6a8997a8879 41
falconsyunya 1:958b581f6d21 42 mpu.readAccelData(accel);//加速度の値をaccel[3]に代入
falconsyunya 7:8f914ead7fc0 43 int x = accel[0]-123;//x軸方向の加速度
falconsyunya 7:8f914ead7fc0 44 int y = accel[1]+60;//y軸方向の加速度
falconsyunya 7:8f914ead7fc0 45 int z = accel[2]+1110 ;//z軸方向の加速度
kosukesuzuki 8:a6a8997a8879 46
kosukesuzuki 8:a6a8997a8879 47 float X = (float)x*0.000597964111328125;
kosukesuzuki 8:a6a8997a8879 48 float Y = (float)y*0.000597964111328125;
kosukesuzuki 8:a6a8997a8879 49 float Z = (float)z*0.000597964111328125;
kosukesuzuki 8:a6a8997a8879 50
kosukesuzuki 8:a6a8997a8879 51 //double a = sqrt(X*X+Y*Y+Z*Z)-9.81;
kosukesuzuki 8:a6a8997a8879 52 //printf("%.2f %.2f %.2f %.2f\r\n",X,Y,Z,a);//速度と変位を表示
kosukesuzuki 8:a6a8997a8879 53
kosukesuzuki 8:a6a8997a8879 54 Ax = X;
kosukesuzuki 8:a6a8997a8879 55 Ay = Y;
kosukesuzuki 8:a6a8997a8879 56 Az = Z;
kosukesuzuki 8:a6a8997a8879 57
kosukesuzuki 8:a6a8997a8879 58 //ローパスフィルタ
kosukesuzuki 8:a6a8997a8879 59 LPFx = LPFx*K1x+K2x*X;
kosukesuzuki 8:a6a8997a8879 60 LPFy = LPFy*K1y+K2y*Y;
kosukesuzuki 8:a6a8997a8879 61 LPFz = LPFz*K1z+K2z*Z;
kosukesuzuki 8:a6a8997a8879 62
kosukesuzuki 8:a6a8997a8879 63 //ハイパスフィルタ
kosukesuzuki 8:a6a8997a8879 64 HPFx = X-LPFx;
kosukesuzuki 8:a6a8997a8879 65 HPFy = Y-LPFy;
kosukesuzuki 8:a6a8997a8879 66 HPFz = Z-LPFz;
kosukesuzuki 8:a6a8997a8879 67
kosukesuzuki 8:a6a8997a8879 68 //HPFa = sqrt(HPFx*HPFx+HPFy*HPFy+HPFz*HPFz); //使わない
kosukesuzuki 8:a6a8997a8879 69
kosukesuzuki 8:a6a8997a8879 70 Xspeed = ((HPFx + xi)*dt)/2 + Xspeed;
kosukesuzuki 8:a6a8997a8879 71 Yspeed = ((HPFy + yi)*dt)/2 + Yspeed;
kosukesuzuki 8:a6a8997a8879 72 Zspeed = ((HPFz + zi)*dt)/2 + Zspeed;
kosukesuzuki 8:a6a8997a8879 73
kosukesuzuki 8:a6a8997a8879 74 //printf("%.2f\r\n",a);//速度と変位を表示
kosukesuzuki 8:a6a8997a8879 75 printf("%.2f %.2f %.2f\r\n",Xspeed,Yspeed,Zspeed); //速度と変位を表示
kosukesuzuki 8:a6a8997a8879 76 SPEED = sqrt(Xspeed*Xspeed+Yspeed*Yspeed+Zspeed*Zspeed-96.2361);
kosukesuzuki 8:a6a8997a8879 77 printf("%.2f \r\n",SPEED);//速度と変位を表示
kosukesuzuki 8:a6a8997a8879 78 wait(dt);
falconsyunya 0:4ba3fa671062 79 }
falconsyunya 0:4ba3fa671062 80 }
kosukesuzuki 8:a6a8997a8879 81