記事一覧

I put 9 axis acceleration & gyro & magnetic sensor MPU 9250 in my own digital stabilizer Continuation

I put 9 axis acceleration & gyro & magnetic sensor MPU 9250 in my own digital stabilizer Continuation



Hello everyone! I’m RoboTakao.

Last time I tried 9 axis acceleration & gyro & magnetic sensor MPU 9250 in my own digital stabilizer using ArduinoNano. At that time I was using the geomagnetic value only in the horizontal direction. In this case, rotating with 3 axes will not work properly.

So I will try using the geomagnetic sensor values with three axes this time.

digitalStabi115.png

It is this site that I referred to.

Please refer to the previous post of the digital stabilizer here.
http://blog.robotakao.jp/blog-entry-227.html
http://robotakao.jp/digitalstabilizer/e/index.html

Description of the sketch
(Symbol with Ac is acceleration data)

Z axis rotation (rad)
 AcZAngPi = atan(AcX/AcY);

X axis rotation (rad)
 AcXAngPi = atan(-AcZ/(AcX*sin(AcZAngPi)+AcY*cos(AcZAngPi)));

Y axis rotation (deg)
 Upper = (MgY - MgYoff)*sin(AcZAngPi) - (MgX - MgXoff)*cos(AcZAngPi);
Lower = (MgZ - MgZoff)*cos(AcXAngPi) + (MgX - MgXoff)*sin(AcXAngPi)*sin(AcZAngPi) + (MgY - MgYoff)*sin(AcXAngPi)*cos(AcZAngPi);
MgYAng = atan(Upper/Lower)/3.142*180;

Operation check

There is improvement point in responsiveness.
It seems that it operates correctly even if turned.



Sketch


#include
#include

const int MPU_addr=0x68; // I2C address of the MPU-6050
const int AK8963_addr=0x0c; // I2C address of the AK8963
int16_t AcXr,AcYr,AcZr,Tmpr,GyXr,GyYr,GyZr,MgXr,MgYr,MgZr;
int16_t Mg[7];
float AcX,AcY,AcZ,Tmp,GyX,GyY,GyZ,MgX,MgY,MgZ,MgXmin,MgYmin,MgZmin,MgXmax,MgYmax,MgZmax,MgXoff,MgYoff,MgZoff;
float AcXAng,AcZAng,AcXAngPi,AcZAngPi,MgYAng;
float Upper,Lower;
float angX = 0.0;
float angY = 0.0;
float angZ = 0.0;
int angXi = 0;
int angYi = 0;
int angZi = 0;
int angXiLast = 0;
int angYiLast = 0;
int angZiLast = 0;
int aX,aY,aZ;
int angXout, angYout,angZout;
int ZeroX = 93;
int ZeroY = 82;
int ZeroZ = 93;
float dt=30;
int djoy=3;
int joy_1 = 1;
int joy_2 = 2;
int val1,val2;
int angjoy1 =0;
int angjoy2 =0;
int dstep = 50;

Servo myservo2;
Servo myservo3;
Servo myservo4;

int Width_Min = 700; //Minimum pulse width(ms)
int Width_Max = 2300; //Maximum pulse width(ms)


void setup()
{
Wire.begin();
Wire.beginTransmission(MPU_addr);
Wire.write(0x6B); // PWR_MGMT_1 register
Wire.write(0); // set to zero (wakes up the MPU-6050)
Wire.endTransmission(true);

//LPF Setting
Wire.beginTransmission(MPU_addr);
Wire.write(0x1A);
Wire.write(0x01);
Wire.endTransmission();

//AK8963 Setting
Wire.beginTransmission(MPU_addr);
Wire.write(0x37); //Address for setting bypass mode of magnetic sensor
Wire.write(0x02); //bypass mode (Magnetic sensor can be used)
Wire.endTransmission();
Wire.beginTransmission(AK8963_addr);
Wire.write(0x0a); //Address for magnetic sensor setting
Wire.write(0x16); //Output cycle of the magnetic sensor (100Hz) 0x12: Output cycle of the magnetic sensor (8Hz)
Wire.endTransmission();

myservo2.attach(2,Width_Min,Width_Max);
myservo3.attach(3,Width_Min,Width_Max);
myservo4.attach(4,Width_Min,Width_Max);

Serial.begin(9600);

}

void loop()
{
Wire.beginTransmission(MPU_addr);
Wire.write(0x3B); // starting with register 0x3B (ACCEL_XOUT_H)
Wire.endTransmission(false);
Wire.requestFrom(MPU_addr,14,true); // request a total of 14 registers
AcXr=Wire.read()<<8|Wire.read(); // 0x3B (ACCEL_XOUT_H) & 0x3C (ACCEL_XOUT_L)
AcYr=Wire.read()<<8|Wire.read(); // 0x3D (ACCEL_YOUT_H) & 0x3E (ACCEL_YOUT_L)
AcZr=Wire.read()<<8|Wire.read(); // 0x3F (ACCEL_ZOUT_H) & 0x40 (ACCEL_ZOUT_L)
Tmpr=Wire.read()<<8|Wire.read(); // 0x41 (TEMP_OUT_H) & 0x42 (TEMP_OUT_L)
GyXr=Wire.read()<<8|Wire.read(); // 0x43 (GYRO_XOUT_H) & 0x44 (GYRO_XOUT_L)
GyYr=Wire.read()<<8|Wire.read(); // 0x45 (GYRO_YOUT_H) & 0x46 (GYRO_YOUT_L)
GyZr=Wire.read()<<8|Wire.read(); // 0x47 (GYRO_ZOUT_H) & 0x48 (GYRO_ZOUT_L)

Wire.beginTransmission(AK8963_addr);
Wire.write(0x02); // starting with register 0x02
Wire.requestFrom(AK8963_addr,1,true);
Wire.endTransmission(false);

//ST1Bit=Wire.read();

if((Wire.read() & 0x01)){ //Check if reading is ready
Wire.beginTransmission(AK8963_addr);
Wire.write(0x03); // starting with register 0x02
Wire.endTransmission(false);
Wire.requestFrom(AK8963_addr,7,true); // request a total of 7 registers

MgYr=Wire.read()|(Wire.read()<<8);
MgXr=Wire.read()|(Wire.read()<<8);
MgZr=-(Wire.read()|(Wire.read()<<8));
}

// joystic data
val1 = analogRead(joy_1);
val2 = analogRead(joy_2);

if (val1 < 300){
angjoy1 = angjoy1 - djoy;
}
if (val1 > 700){
angjoy1 = angjoy1 + djoy;
}
if (val2 < 300){
angjoy2 = angjoy2 - djoy;
}
if (val2 > 700){
angjoy2 = angjoy2 + djoy;
}

// accel data
// full scale range ±2g LSB sensitivity 16384 LSB/g) -> ±2 x 16384 = ±32768 LSB
AcX = AcXr/16384.0;
AcY = AcYr/16384.0;
AcZ = AcZr/16384.0;

// temperture
Tmp = Tmpr/320.00+36.53;

// gyro data
// full scale range ±250 deg/s LSB sensitivity 131 LSB/deg/s -> ±250 x 131 = ±32750 LSB
GyX = GyXr/131.0;
GyY = GyYr/131.0;
GyZ = GyZr/131.0;

AcZAngPi = atan(AcX/AcY);
AcZAng = AcZAngPi/3.142*180;
AcXAngPi = atan(-AcZ/(AcX*sin(AcZAngPi)+AcY*cos(AcZAngPi)));
AcXAng = AcXAngPi/3.142*180;


// Magnetic data
MgX = MgXr / 32768.0f * 4800.0f;// Conversion to [uT]
MgY = MgYr / 32768.0f * 4800.0f;// Conversion to [uT]
MgZ = MgZr / 32768.0f * 4800.0f;// Conversion to [uT]

//Calculation of offset

if (MgX <= MgXmin) MgXmin = MgX;
if (MgX >= MgXmax) MgXmax = MgX;
if (MgY <= MgYmin) MgYmin = MgY;
if (MgY >= MgYmax) MgYmax = MgY;
if (MgZ <= MgZmin) MgZmin = MgZ;
if (MgZ >= MgZmax) MgZmax = MgZ;

MgXoff = (MgXmax - MgXmin)/2 + MgXmin;
MgYoff = (MgYmax - MgYmin)/2 + MgYmin;
MgZoff = (MgZmax - MgZmin)/2 + MgZmin;

Upper = (MgY - MgYoff)*sin(AcZAngPi) - (MgX - MgXoff)*cos(AcZAngPi);
Lower = (MgZ - MgZoff)*cos(AcXAngPi) + (MgX - MgXoff)*sin(AcXAngPi)*sin(AcZAngPi) + (MgY - MgYoff)*sin(AcXAngPi)*cos(AcZAngPi);

if(Lower > 0.0){
if(Upper > 0.0){
MgYAng = atan(Upper/Lower)/3.142*180;
}else if(Upper < 0.0){
MgYAng = atan(Upper/Lower)/3.142*180;
}
}else if(Lower < 0.0){
if(Upper > 0.0){
MgYAng = atan(Upper/Lower)/3.142*180 + 180;
}else if(Upper < 0.0){
MgYAng = atan(Upper/Lower)/3.142*180 - 180;
}
}


//Complementary filter
angX = 0.9*(angX - GyX*dt*0.001*1.5) - 0.1*AcXAng;
angZ = 0.9*(angZ + GyZ*dt*0.001*1.2) + 0.1*AcZAng;
angY = 0.9*(angY - GyY*dt*0.001) - 0.1*MgYAng;

angXiLast = angXi;
angYiLast = angYi;
angZiLast = angZi;

angXi = (int) angX + angjoy1;
angYi = (int) angY - angjoy2;
angZi = (int) angZ;

aX = angXi - angXiLast;
aY = angYi - angYiLast;
aZ = angZi - angZiLast;

for (int k=1; k <= dstep ; k++){
angXout = aX*k/dstep + angXiLast;
angYout = aY*k/dstep + angYiLast;
angZout = aZ*k/dstep + angZiLast;

if (angXout < -40) angXout = -40;
if (angXout > 20) angXout = 20;
if (angYout < -80) angYout = -80;
if (angYout > 80) angYout = 80;
if (angZout < -70) angZout = -70;
if (angZout > 70) angZout = 70;

myservo2.write(ZeroY - angYout);
myservo3.write(ZeroX - angXout);
myservo4.write(ZeroZ - angZout);

delayMicroseconds(dt/dstep*1000);
}
}


Thank you! Good-bye!

コメント

コメントの投稿

非公開コメント

プロフィール

RoboTakao

Author:RoboTakao
みなさんご訪問ありがとうございます。ロボット作りたいけどお小遣いそんなにないし、簡単でローコストでロボットを作るための私のプロジェクトを紹介します。

ウェブサイトもありますのでそちらもよろしくお願いします。
http://robotakao.jp/