ماژول imu شش محوره mpu6050 با فیلتر کالمن

[litron]

الحمدلله تونستم برنامش رو واسه آردو بنویسم
#include <SoftwareSerial.h>

SoftwareSerial imu(10, 11); // RX, TX (TX not used)
const int sentenceSize = 80;
float a[3],w[3],angle[3],T;
unsigned char Re_buf[11],counter;
void setup()
{
Serial.begin(9600);
imu.begin(115200);
imu.print(0x64 , HEX);
}

void loop()
{


Re_buf[0]=imu.read();

if(Re_buf[0]==0x55)
{
Re_buf[1]=imu.read();
Re_buf[2]=imu.read();
Re_buf[3]=imu.read();
Re_buf[4]=imu.read();
Re_buf[5]=imu.read();
Re_buf[6]=imu.read();
Re_buf[7]=imu.read();
Re_buf[8]=imu.read();
Re_buf[9]=imu.read();
Re_buf[10]=imu.read();
Re_buf[11]=imu.read();


switch(Re_buf [1])
{
case 0x51:
a[0] = (short(Re_buf [3]<<8| Re_buf [2]))/32768.0*16;
a[1] = (short(Re_buf [5]<<8| Re_buf [4]))/32768.0*16;
a[2] = (short(Re_buf [7]<<8| Re_buf [6]))/32768.0*16;
T = (short(Re_buf [9]<<8| Re_buf [8]))/340.0+36.25;
break;
case 0x52:
w[0] = (short(Re_buf [3]<<8| Re_buf [2]))/32768.0*2000;
w[1] = (short(Re_buf [5]<<8| Re_buf [4]))/32768.0*2000;
w[2] = (short(Re_buf [7]<<8| Re_buf [6]))/32768.0*2000;
T = (short(Re_buf [9]<<8| Re_buf [8]))/340.0+36.25;
break;
case 0x53:
angle[0] = (short(Re_buf [3]<<8| Re_buf [2]))/32768.0*180;
angle[1] = (short(Re_buf [5]<<8| Re_buf [4]))/32768.0*180;
angle[2] = (short(Re_buf [7]<<8| Re_buf [6]))/32768.0*180;
T = (short(Re_buf [9]<<8| Re_buf [8]))/340.0+36.25;
break;
}

}
}
می تونید داده ها رو که 100 بار در ثانیه آپدیت میشه پرینت بگیرید یا تو محاسباتتون تو خود آردو استفاده کنید