Revision 1:279026544175, committed 2022-12-03

Comitter:

kosukesuzuki

Date:

Sat Dec 03 03:54:51 2022 +0000

Parent:

0:0ed2c0a9b8d4

Commit message:

angle

Changed in this revision

--- a/main.cpp	Sun Jan 23 03:59:50 2022 +0000
+++ b/main.cpp	Sat Dec 03 03:54:51 2022 +0000
@@ -1,36 +1,160 @@
 #include "mbed.h"
 #include "MPU6050.h"
 
-DigitalOut myled(LED1);
- 
-//SDFileSystem sd(p5, p6, p7, p8, "sd"); // the pinout on the mbed Cool Components workshop board
 MPU6050 mpu(p9,p10);
 Serial pc(USBTX,USBRX,9600);
-int accel[3];
+#define PI 3.14159265358979323846f
+
+int Gyro[3];
+int Accel[3];
+
+float dt = 0.01;
 
 int main() {
-    //mkdir("/sd/mydir", 0777);
-    //FILE *fp = fopen("/sd/mydir/sdtest.txt", "w");
-    for(int i =0;i<10;i++){
-        myled=1;
-        mpu.readAccelData(accel);//加速度の値をaccel[3]に代入
-        int x = accel[0]-123;//x軸方向の加速度
-        int y = accel[1]+60;//y軸方向の加速度
-        int z = accel[2]+1110 ;//z軸方向の加速度
-        float X = x*0.000597964111328125;
-        float Y = y*0.000597964111328125;
-        float Z = z*0.000597964111328125; 
-        double a = X*X+Y*Y+Z*Z-95.982071137936;
-        printf("%.2f %.2f %.2f %.2f\r\n",X,Y,Z,a);//速度と変位を表示
-        wait(1.0);
-        myled=0;
-        wait(1.0);
+    int i;//回数
+    int ax,ay,az,sax,say,saz;//加速度補正
+    sax=0,say=0,saz=0;
+    int gx,gy,gz,sgx,sgy,sgz;//角速度補正
+    sgx=0,sgy=0,sgz=0;
+    
+    double x,y,z,x1,y1,z1;//台形法
+    double ax1,ay1,az1,ox,oy;//オイラー
+    x=0,y=0,z=0;
+    x1=0,y1=0,z1=0;
+    
+    double angleX,angleY,ox1,oy1;//相補
+    ox1=0,oy1=0;
+    
+    //カルマンフィルタ
+    double newAngle,newRate;
+    double Q_angle  =  0.001;
+    double Q_gyro   =  0.003;
+    double R_angle  =  0.03;
+    double x_angle;
+    double x_bias = 0;
+    double P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
+    double  y11, S;
+    double K_0, K_1;
+    double dt=0.005;
+    
+    //相補とカルマンフィルタの平均
+    double AnX;
+    
+    //補正
+    /*//なぜかおかしくなる
+     for(i=0;i<100;i++){
+        mpu.readAccelData(Accel);
+        ax = Accel[0];
+        ay = Accel[1];
+        az = Accel[2];
+        
+        sax = sax + ax;
+        say = say + ay;
+        saz = saz + az;
+        
+        wait(dt);
+        }
+    
+    sax = sax/100;
+    say = say/100;
+    saz = saz/100;
+    */
+    
+    for(i=0;i<100;i++){
+        mpu.readGyroData(Gyro);
+        gx = Gyro[0];//x軸方向
+        gy = Gyro[1];//y軸方向
+        gz = Gyro[2];//z軸方向
+        
+        sgx = sgx + gx;
+        sgy = sgy + gy;
+        sgz = sgz + gz;
+        
+        wait(dt);
+        }
+    
+    sgx = sgx/100;
+    sgy = sgy/100;
+    sgz = sgz/100;
+    
+    while(1){
+        //加速度計測
+        mpu.readAccelData(Accel);
+        ax = Accel[0]-123;
+        ay = Accel[1]+60;
+        az = Accel[2]+110;
+        //printf("%d,%d,%d\r\n",ax,ay,az);
         
-    //if(fp == NULL) {
-        //error("Could not open file for write\n");
-    //}
-    //fprintf(fp,"%.2f %.2f %.2f %.2f\r\n",X,Y,Z,a);
-    //fprintf(fp,"%.2f \r\n",Y);
-    //}
-    //fclose(fp);
-}}
\ No newline at end of file
+        ax1 = (double)ax*0.000597964111328125;
+        ay1 = (double)ay*0.000597964111328125;
+        az1 = (double)az*0.000597964111328125;
+        //printf("%.2f,%.2f,%.2f\r\n",ax1,ay1,az1); 
+        
+        //オイラー角
+        ox = atan(ay1/az1)*180/PI;
+        oy = atan(ax1/sqrt(ay1*ay1+az1*az1))*180/PI;
+        //printf("%f,%f\r\n",ox,oy);
+        
+        //角速度計測
+        mpu.readGyroData(Gyro);
+        gx = Gyro[0]-sgx;//x軸方向
+        gy = Gyro[1]-sgy;//y軸方向*0.291796151
+        gz = Gyro[2]-sgz;//z軸方向
+        //printf("%d,%d,%d\r\n",gx,gy,gz);
+        
+        double gX = (double)gx*250.0/32768.0;
+        double gY = (double)gy*250.0/32768.0;
+        double gZ = (double)gz*250.0/32768.0;
+        //printf("%.2f,%.2f,%.2f\r\n",gX,gY,gZ);//角速度
+        
+        //台形法(これいらんな)
+        /*
+        x = x + (x1+(gX *180/ PI))*dt/2;
+        y = y + (y1+(gY *180/ PI))*dt/2;
+        z = z + (z1+(gZ *180/ PI))*dt/2;
+        
+        x1 = gX* 180/ PI;
+        y1 = gY* 180/ PI;
+        z1 = gZ* 180/ PI;
+        //pc.printf("%.2f,%.2f,%.2f\r\n",x,y,z);
+        */
+        
+        //相補フィルタ
+        angleX = 0.95*(ox1+(gX*dt*180/PI))+0.05*ox;
+        angleY = 0.95*(oy1+(gY*dt*180/PI))+0.05*oy;
+        ox1 = ox;
+        oy1 = oy; 
+        //pc.printf("%f,%f\r\n",angleX,angleY);
+        
+        //カルマンフィルタ(？http://meerstern.seesaa.net/article/417550787.html)
+        newAngle = ox;
+        newRate = gx*dt;
+        
+        x_angle = x_angle + dt * (newRate - x_bias);
+      
+        P_00 = P_00 + dt * (dt * P_11 - P_01 - P_10 + Q_angle);
+        P_01 = P_01 - dt * P_11;
+        P_10 = P_10 - dt * P_11;
+        P_11 = P_11 - Q_gyro * dt;
+
+        y11 = newAngle - x_angle;
+        S = P_00 + R_angle;
+        K_0 = P_00 / S;
+        K_1 = P_10 / S;
+
+        x_angle = x_angle +  K_0 * y11;
+        x_bias  = x_bias +  K_1 * y11;
+        P_00 = P_00 - K_0 * P_00;
+        P_01 = P_01 - K_0 * P_01;
+        P_10 = P_10 - K_1 * P_00;
+        P_11 = P_11 - K_1 * P_01;
+        //printf("%f\r\n",x_angle);
+        
+        //xの平均角度
+        AnX = (x_angle+angleX)/2;
+        printf("%f\r\n",AnX);
+        
+        
+        wait(dt);
+        }
+}