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

accel gyro

Dependencies:   MPU6050 mbed

main.cpp@1:279026544175, 19 months ago (annotated)

Committer:
kosukesuzuki
Date:
Sat Dec 03 03:54:51 2022 +0000
Revision:
1:279026544175
Parent:
0:0ed2c0a9b8d4 
angle

Who changed what in which revision?

UserRevisionLine numberNew contents of line
kosukesuzuki 0:0ed2c0a9b8d4 1 #include "mbed.h"
kosukesuzuki 0:0ed2c0a9b8d4 2 #include "MPU6050.h"
kosukesuzuki 0:0ed2c0a9b8d4 3
kosukesuzuki 0:0ed2c0a9b8d4 4 MPU6050 mpu(p9,p10);
kosukesuzuki 0:0ed2c0a9b8d4 5 Serial pc(USBTX,USBRX,9600);
kosukesuzuki 1:279026544175 6 #define PI 3.14159265358979323846f
kosukesuzuki 1:279026544175 7
kosukesuzuki 1:279026544175 8 int Gyro[3];
kosukesuzuki 1:279026544175 9 int Accel[3];
kosukesuzuki 1:279026544175 10
kosukesuzuki 1:279026544175 11 float dt = 0.01;
kosukesuzuki 0:0ed2c0a9b8d4 12
kosukesuzuki 0:0ed2c0a9b8d4 13 int main() {
kosukesuzuki 1:279026544175 14 int i;//回数
kosukesuzuki 1:279026544175 15 int ax,ay,az,sax,say,saz;//加速度補正
kosukesuzuki 1:279026544175 16 sax=0,say=0,saz=0;
kosukesuzuki 1:279026544175 17 int gx,gy,gz,sgx,sgy,sgz;//角速度補正
kosukesuzuki 1:279026544175 18 sgx=0,sgy=0,sgz=0;
kosukesuzuki 1:279026544175 19
kosukesuzuki 1:279026544175 20 double x,y,z,x1,y1,z1;//台形法
kosukesuzuki 1:279026544175 21 double ax1,ay1,az1,ox,oy;//オイラー
kosukesuzuki 1:279026544175 22 x=0,y=0,z=0;
kosukesuzuki 1:279026544175 23 x1=0,y1=0,z1=0;
kosukesuzuki 1:279026544175 24
kosukesuzuki 1:279026544175 25 double angleX,angleY,ox1,oy1;//相補
kosukesuzuki 1:279026544175 26 ox1=0,oy1=0;
kosukesuzuki 1:279026544175 27
kosukesuzuki 1:279026544175 28 //カルマンフィルタ
kosukesuzuki 1:279026544175 29 double newAngle,newRate;
kosukesuzuki 1:279026544175 30 double Q_angle = 0.001;
kosukesuzuki 1:279026544175 31 double Q_gyro = 0.003;
kosukesuzuki 1:279026544175 32 double R_angle = 0.03;
kosukesuzuki 1:279026544175 33 double x_angle;
kosukesuzuki 1:279026544175 34 double x_bias = 0;
kosukesuzuki 1:279026544175 35 double P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
kosukesuzuki 1:279026544175 36 double y11, S;
kosukesuzuki 1:279026544175 37 double K_0, K_1;
kosukesuzuki 1:279026544175 38 double dt=0.005;
kosukesuzuki 1:279026544175 39
kosukesuzuki 1:279026544175 40 //相補とカルマンフィルタの平均
kosukesuzuki 1:279026544175 41 double AnX;
kosukesuzuki 1:279026544175 42
kosukesuzuki 1:279026544175 43 //補正
kosukesuzuki 1:279026544175 44 /*//なぜかおかしくなる
kosukesuzuki 1:279026544175 45 for(i=0;i<100;i++){
kosukesuzuki 1:279026544175 46 mpu.readAccelData(Accel);
kosukesuzuki 1:279026544175 47 ax = Accel[0];
kosukesuzuki 1:279026544175 48 ay = Accel[1];
kosukesuzuki 1:279026544175 49 az = Accel[2];
kosukesuzuki 1:279026544175 50
kosukesuzuki 1:279026544175 51 sax = sax + ax;
kosukesuzuki 1:279026544175 52 say = say + ay;
kosukesuzuki 1:279026544175 53 saz = saz + az;
kosukesuzuki 1:279026544175 54
kosukesuzuki 1:279026544175 55 wait(dt);
kosukesuzuki 1:279026544175 56 }
kosukesuzuki 1:279026544175 57
kosukesuzuki 1:279026544175 58 sax = sax/100;
kosukesuzuki 1:279026544175 59 say = say/100;
kosukesuzuki 1:279026544175 60 saz = saz/100;
kosukesuzuki 1:279026544175 61 */
kosukesuzuki 1:279026544175 62
kosukesuzuki 1:279026544175 63 for(i=0;i<100;i++){
kosukesuzuki 1:279026544175 64 mpu.readGyroData(Gyro);
kosukesuzuki 1:279026544175 65 gx = Gyro[0];//x軸方向
kosukesuzuki 1:279026544175 66 gy = Gyro[1];//y軸方向
kosukesuzuki 1:279026544175 67 gz = Gyro[2];//z軸方向
kosukesuzuki 1:279026544175 68
kosukesuzuki 1:279026544175 69 sgx = sgx + gx;
kosukesuzuki 1:279026544175 70 sgy = sgy + gy;
kosukesuzuki 1:279026544175 71 sgz = sgz + gz;
kosukesuzuki 1:279026544175 72
kosukesuzuki 1:279026544175 73 wait(dt);
kosukesuzuki 1:279026544175 74 }
kosukesuzuki 1:279026544175 75
kosukesuzuki 1:279026544175 76 sgx = sgx/100;
kosukesuzuki 1:279026544175 77 sgy = sgy/100;
kosukesuzuki 1:279026544175 78 sgz = sgz/100;
kosukesuzuki 1:279026544175 79
kosukesuzuki 1:279026544175 80 while(1){
kosukesuzuki 1:279026544175 81 //加速度計測
kosukesuzuki 1:279026544175 82 mpu.readAccelData(Accel);
kosukesuzuki 1:279026544175 83 ax = Accel[0]-123;
kosukesuzuki 1:279026544175 84 ay = Accel[1]+60;
kosukesuzuki 1:279026544175 85 az = Accel[2]+110;
kosukesuzuki 1:279026544175 86 //printf("%d,%d,%d\r\n",ax,ay,az);
kosukesuzuki 0:0ed2c0a9b8d4 87
kosukesuzuki 1:279026544175 88 ax1 = (double)ax*0.000597964111328125;
kosukesuzuki 1:279026544175 89 ay1 = (double)ay*0.000597964111328125;
kosukesuzuki 1:279026544175 90 az1 = (double)az*0.000597964111328125;
kosukesuzuki 1:279026544175 91 //printf("%.2f,%.2f,%.2f\r\n",ax1,ay1,az1);
kosukesuzuki 1:279026544175 92
kosukesuzuki 1:279026544175 93 //オイラー角
kosukesuzuki 1:279026544175 94 ox = atan(ay1/az1)*180/PI;
kosukesuzuki 1:279026544175 95 oy = atan(ax1/sqrt(ay1*ay1+az1*az1))*180/PI;
kosukesuzuki 1:279026544175 96 //printf("%f,%f\r\n",ox,oy);
kosukesuzuki 1:279026544175 97
kosukesuzuki 1:279026544175 98 //角速度計測
kosukesuzuki 1:279026544175 99 mpu.readGyroData(Gyro);
kosukesuzuki 1:279026544175 100 gx = Gyro[0]-sgx;//x軸方向
kosukesuzuki 1:279026544175 101 gy = Gyro[1]-sgy;//y軸方向*0.291796151
kosukesuzuki 1:279026544175 102 gz = Gyro[2]-sgz;//z軸方向
kosukesuzuki 1:279026544175 103 //printf("%d,%d,%d\r\n",gx,gy,gz);
kosukesuzuki 1:279026544175 104
kosukesuzuki 1:279026544175 105 double gX = (double)gx*250.0/32768.0;
kosukesuzuki 1:279026544175 106 double gY = (double)gy*250.0/32768.0;
kosukesuzuki 1:279026544175 107 double gZ = (double)gz*250.0/32768.0;
kosukesuzuki 1:279026544175 108 //printf("%.2f,%.2f,%.2f\r\n",gX,gY,gZ);//角速度
kosukesuzuki 1:279026544175 109
kosukesuzuki 1:279026544175 110 //台形法(これいらんな)
kosukesuzuki 1:279026544175 111 /*
kosukesuzuki 1:279026544175 112 x = x + (x1+(gX *180/ PI))*dt/2;
kosukesuzuki 1:279026544175 113 y = y + (y1+(gY *180/ PI))*dt/2;
kosukesuzuki 1:279026544175 114 z = z + (z1+(gZ *180/ PI))*dt/2;
kosukesuzuki 1:279026544175 115
kosukesuzuki 1:279026544175 116 x1 = gX* 180/ PI;
kosukesuzuki 1:279026544175 117 y1 = gY* 180/ PI;
kosukesuzuki 1:279026544175 118 z1 = gZ* 180/ PI;
kosukesuzuki 1:279026544175 119 //pc.printf("%.2f,%.2f,%.2f\r\n",x,y,z);
kosukesuzuki 1:279026544175 120 */
kosukesuzuki 1:279026544175 121
kosukesuzuki 1:279026544175 122 //相補フィルタ
kosukesuzuki 1:279026544175 123 angleX = 0.95*(ox1+(gX*dt*180/PI))+0.05*ox;
kosukesuzuki 1:279026544175 124 angleY = 0.95*(oy1+(gY*dt*180/PI))+0.05*oy;
kosukesuzuki 1:279026544175 125 ox1 = ox;
kosukesuzuki 1:279026544175 126 oy1 = oy;
kosukesuzuki 1:279026544175 127 //pc.printf("%f,%f\r\n",angleX,angleY);
kosukesuzuki 1:279026544175 128
kosukesuzuki 1:279026544175 129 //カルマンフィルタ(？http://meerstern.seesaa.net/article/417550787.html)
kosukesuzuki 1:279026544175 130 newAngle = ox;
kosukesuzuki 1:279026544175 131 newRate = gx*dt;
kosukesuzuki 1:279026544175 132
kosukesuzuki 1:279026544175 133 x_angle = x_angle + dt * (newRate - x_bias);
kosukesuzuki 1:279026544175 134
kosukesuzuki 1:279026544175 135 P_00 = P_00 + dt * (dt * P_11 - P_01 - P_10 + Q_angle);
kosukesuzuki 1:279026544175 136 P_01 = P_01 - dt * P_11;
kosukesuzuki 1:279026544175 137 P_10 = P_10 - dt * P_11;
kosukesuzuki 1:279026544175 138 P_11 = P_11 - Q_gyro * dt;
kosukesuzuki 1:279026544175 139
kosukesuzuki 1:279026544175 140 y11 = newAngle - x_angle;
kosukesuzuki 1:279026544175 141 S = P_00 + R_angle;
kosukesuzuki 1:279026544175 142 K_0 = P_00 / S;
kosukesuzuki 1:279026544175 143 K_1 = P_10 / S;
kosukesuzuki 1:279026544175 144
kosukesuzuki 1:279026544175 145 x_angle = x_angle + K_0 * y11;
kosukesuzuki 1:279026544175 146 x_bias = x_bias + K_1 * y11;
kosukesuzuki 1:279026544175 147 P_00 = P_00 - K_0 * P_00;
kosukesuzuki 1:279026544175 148 P_01 = P_01 - K_0 * P_01;
kosukesuzuki 1:279026544175 149 P_10 = P_10 - K_1 * P_00;
kosukesuzuki 1:279026544175 150 P_11 = P_11 - K_1 * P_01;
kosukesuzuki 1:279026544175 151 //printf("%f\r\n",x_angle);
kosukesuzuki 1:279026544175 152
kosukesuzuki 1:279026544175 153 //xの平均角度
kosukesuzuki 1:279026544175 154 AnX = (x_angle+angleX)/2;
kosukesuzuki 1:279026544175 155 printf("%f\r\n",AnX);
kosukesuzuki 1:279026544175 156
kosukesuzuki 1:279026544175 157
kosukesuzuki 1:279026544175 158 wait(dt);
kosukesuzuki 1:279026544175 159 }
kosukesuzuki 1:279026544175 160 }