自作デジタルスタビライザーに９軸 加速度＆ジャイロ＆磁気センサ MPU9250を入れてみた　続き

こんにちはRoboTakaoです。

前回、９軸 加速度＆ジャイロ＆磁気センサ MPU9250をArduinoNanoを使った自作のデジタルスタビライザーに入れてみました。その時には地磁気の値を水平方向のみ反映させるようにしていました。この場合3軸で回転させると正しく動作しません。

そこで今回は地磁気センサーの値を三軸使って動作させてみます。



参考にしたのはこちらのサイトです。

以前のデジタルスタビライザーの投稿はこちらを参考にして下さい。
http://blog.robotakao.jp/blog-entry-227.html
http://robotakao.jp/digitalstabilizer/index.html

スケッチの説明
（Acが付いているのは加速度データ）

Z軸周り　（rad）
　AcZAngPi = atan(AcX/AcY);

X軸周り　（rad）
　AcXAngPi = atan(-AcZ/(AcX*sin(AcZAngPi)+AcY*cos(AcZAngPi)));

Y軸周り　（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;

動作確認

　応答性に改善の余地がありますが、ぐるぐる回しても正しく動作しているようです。



スケッチ

#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;    //最小パルス幅（ms）
int Width_Max = 2300;   //最大パルス幅（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設定
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x1A);
  Wire.write(0x01);
  Wire.endTransmission();

  //AK8963設定
  Wire.beginTransmission(MPU_addr);
  Wire.write(0x37);  //磁気センサのバイパスモード設定用のアドレス
  Wire.write(0x02);  //bypass mode(磁気センサが使用出来るようになる)
  Wire.endTransmission();
  Wire.beginTransmission(AK8963_addr);
  Wire.write(0x0a);  //磁気センサ設定用のアドレス
  Wire.write(0x16);  //磁気センサの出力周期(100Hz) 0x12:磁気センサの出力周期(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)){   //読み出し準備ができたか確認
    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;//[uT]に変換
  MgY = MgYr / 32768.0f * 4800.0f;//[uT]に変換
  MgZ = MgZr / 32768.0f * 4800.0f;//[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;
      }
    }
  
//相補フィルター
  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);
  }
}

それではこの辺で失礼します。

ありがとうございました。