LoginSignup
2
2

More than 3 years have passed since last update.

@MuAuan(ムー ウワン)

【arduino nano入門】ServoとMPU9250センサーとMadgwickFilterで追跡カメラを制御する♪

Last updated at Posted at 2021-05-16

いよいよこのシリーズ「ServoとMPU9250（MPU6050）とマイコンで追跡カメラを制御する」も最後に近づいてきた。
今回は、Aruduino nanoを利用してみようと思う。
特に、フィルターとして有名なMadgwickFilterを利用する。
※理論は次回以降に予定する

やったこと

・arduino nanoのハンダ付け
・ボードpin図と結線
・通常の平均化の限界
・MPU9250の測定とMadgwickFilter
・追跡カメラを制御する

・arduino nanoのハンダ付け

ピンヘッドを半田つけ（２０Wコテ）したが、最初いも半田に気づかずに原因不明の測定不能で苦労した。
久しぶりだったので、言い訳はたくさんあるが、原因は温度が不十分だったと思われる。
やはり、ピンヘッドの場合、コテをあてて５ー１０秒程度立ってから半田を流すのがよさそうだ。
※検証は、ピンヘッドを押してみると抜けるかどうかで見極められる

・ボードpin図と結線

arduinonano_pin.jpg

Arduino nano mpu9250 servo Led
GND GND GND GND
3.3V 3.3v - -
PILO(4.2v) - + -
SCL SCL - -
SDA SDA - -
myservo1.attach(10) - Servo1(yellow) -
myservo2.attach(11) - Servo2(yellow) -
pinMode(2, OUTPUT) - - led(+)

・通常の平均化の限界

測定誤差を抑えるために以下二点の対策を実施した。
①yaw計算の時間間隔を測定して正しくした
　dt = (micros()-preTime)/1000000;
　gyro_z_ += gyro_z*dt;
②pitchの時系列を以下の関数で26回平均にした
　averageStream(float val)
この改善で、yawつまり回転方向の精度はつかえるレベルになったように思うがpitchのほうは振動が収まらない。

平均化処理のコードは参考のコードを利用させていただいた。
【参考】
analogRead()用の関数作ってみた

コード
#include "Wire.h"
#include <Servo.h>          // サーボモータを動かすライブラリ
Servo myservo1;            // 
Servo myservo2;             
#define PI 3.141592653589793

float power1 = 0;            // サーボの出力　sg９０
float power2 = 0;            

int16_t axRaw, ayRaw, azRaw, gxRaw, gyRaw, gzRaw, Temperature;
float rawX, rawY, rawZ;
float acc_x0=0, acc_y0=0, acc_z0=0;
float acc_x=0, acc_y=0, acc_z=0;
float gyro_x=0, gyro_y=0, gyro_z=0;
float pitch, roll;
float gyro_z_=0;
int sk = 0;
float dt, preTime;

void setup() {
  Serial.begin(115200);     //シリアル通信を115200bpsに設定
  myservo1.attach(10);       // 10番ピンに接続
  myservo2.attach(11);       // 11番ピンに接続
  pinMode(2, OUTPUT);      // 2番ピンでLED（確認用）

  Wire.begin();
  setupMPU9250();
  readData();
  acc_x0=acc_x;
  acc_y0=acc_y;
  acc_z0=acc_z-1;
  preTime = micros();
}

void loop() {
  readData();
  Serial.println("#accelerometer#");
  Serial.print(acc_x-acc_x0); Serial.print(",");
  Serial.print(acc_y-acc_y0); Serial.print(",");
  Serial.print(acc_z-acc_z0);Serial.print("\n");
  Serial.print("#gyroscope#");
  Serial.print(gyro_x);Serial.print(",");
  Serial.print(gyro_y); Serial.print(",");
  Serial.print(gyro_z);Serial.print("\n");

  // 出力を計算する
  get_angle();
  dt = (micros()-preTime)/1000000;
  preTime = micros();
  gyro_z_ += gyro_z*dt;
  Serial.print(pitch); Serial.print(",");
  Serial.print(roll); Serial.print(",");
  Serial.println(gyro_z_);Serial.print("\n");

  pitch = averageStream(pitch);

  power2 = pitch;
  power1 = gyro_z_;
  Serial.print(sk); Serial.print("\n");
  //power1 = constrain(power1, -120, 120);  // 制限
  //power2 = constrain(power2, -120, 120);  // 制限
  // 
  Serial.print("power1 =\t");  Serial.print(power1);  Serial.print("\n");
  Serial.print("power2 =\t");  Serial.print(power2); Serial.print("\n");
  myservo1.write(50-power1);
  myservo2.write(50-power2);   

  digitalWrite(2, HIGH); 
  delay(100);             
  digitalWrite(2, LOW);   
  delay(100);
  sk += 1;
}

void get_angle(){
  rawX = acc_x-acc_x0;
  rawY = acc_y-acc_y0;
  rawZ = acc_z-acc_z0;
  pitch = atan2(rawX, sqrt(rawY*rawY+rawZ*rawZ))* 180.0 / PI;
  roll = atan2(rawY, rawZ)* 180.0 / PI;
}

float averageStream(float val){
    static const int MODULUS = 26;
    static int val_hist[MODULUS];
    static float sum=0;
    static byte i_mod=0;

    sum += ( val - val_hist[i_mod] );
    val_hist[i_mod] = val;
    i_mod = ++i_mod % MODULUS;
    return sum / MODULUS;
}

int16_t u2s(int16_t unsigneddata){
  int16_t usd = unsigneddata;
  if (usd & (0x01 << 15)){ 
      usd = -1 * ((unsigneddata ^ 0xffff) + 1);
  }
  return usd;
}

void readData() {
  Wire.beginTransmission(0x68);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(0x68, 14, true);
  while (Wire.available() < 14);

  axRaw = u2s(Wire.read() << 8 | Wire.read());
  ayRaw = u2s(Wire.read() << 8 | Wire.read());
  azRaw = u2s(Wire.read() << 8 | Wire.read());
  Temperature = u2s(Wire.read() << 8 | Wire.read());
  gxRaw = u2s(Wire.read() << 8 | Wire.read());
  gyRaw = u2s(Wire.read() << 8 | Wire.read());
  gzRaw = u2s(Wire.read() << 8 | Wire.read());

  // 加速度値を分解能で割って加速度(G)に変換する
  acc_x = (2. / 32768.)*axRaw ;  //FS_SEL_0 16,384 LSB / g
  acc_y = (2. / 32768.)*ayRaw ;
  acc_z = (2. / 32768.)*azRaw ;

  // 角速度値を分解能で割って角速度(degrees per sec)に変換する
  gyro_x = (250. / 32768.)*gxRaw ;  // (度/s)
  gyro_y = (250. / 32768.)*gyRaw ;
  gyro_z = (250. / 32768.)*gzRaw ;
}

//初期化関数は、参考の解説みてください
void setupMPU9250() {
  Wire.beginTransmission(0x68);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission();

  Wire.beginTransmission(0x68);
  Wire.write(0x1A);
  Wire.write(0x05);
  Wire.endTransmission();

  Wire.beginTransmission(0x68);
  Wire.write(0x37);
  Wire.write(0x02);
  Wire.endTransmission();

  Wire.beginTransmission(0x0C);
  Wire.write(0x0A);
  Wire.write(0x16);
  Wire.endTransmission();
  delay(500);
}

・MPU9250の測定とMadgwickFilter

Madgwickのつかいかたは、参考のコードを参考とした。

【参考】
Arduino nanoと6軸センサMPU6050と連続回転サーボFS90Rで倒立振子を作った

MPU9250の測定とMadgwickFilterのコード
#include "Wire.h"
int16_t axRaw, ayRaw, azRaw, gxRaw, gyRaw, gzRaw, Temperature;
float acc_x0=0, acc_y0=0, acc_z0=0;
float acc_x=0, acc_y=0, acc_z=0;
float gyro_x=0, gyro_y=0, gyro_z=0;
float mx_, my_, mz_;
int16_t mx,my,mz;
float temp;
int sk = 0;
#include <MadgwickAHRS.h>   // 角度と角速度にMadgwickフィルタをかけるライブラリ
Madgwick MadgwickFilter;    // Madgwickフィルタのオブジェクトを設定

float preTime;

void setup() {
  Wire.begin();
  Serial.begin(115200);
  setupMPU9250();
  readData();
  acc_x0=acc_x;
  acc_y0=acc_y;
  acc_z0=acc_z-1;
  MadgwickFilter.begin(100);  // Madgwickフィルタの周波数を100Hzに設定。
  preTime = micros();                   // 処理時間を記録
}

void loop() {

  readCompass();
  //Serial.print(sk); Serial.print("\n");
  //Serial.print("#magnetic#");
  //Serial.print(mx_); Serial.print(",");
  //Serial.print(my_); Serial.print(",");
  //Serial.println(mz_);Serial.print("#accelerometer#");
  readData();
  //Serial.print(acc_x-acc_x0); Serial.print(",");
  //Serial.print(acc_y-acc_y0); Serial.print(",");
  //Serial.println(acc_z-acc_z0);Serial.print("#gyroscope#");
  //Serial.print(gyro_x);Serial.print(",");
  //Serial.print(gyro_y); Serial.print(",");
  //Serial.println(gyro_z);Serial.print("\n");

  // Madgwickフィルターを用いて、PRY（pitch, roll, yaw）を計算
  MadgwickFilter.updateIMU(gyro_x, gyro_y, gyro_z, acc_x-acc_x0, acc_y-acc_y0, acc_z-acc_z0);

  // rollの計算結果のみを取得する。pitchとyawは使わない。このrollが現在の角度になる。
  // 前に傾いたら90+x°、後ろに傾いたら90-x°の値をとる。
  float pitch  = MadgwickFilter.getPitch();
  float roll  = MadgwickFilter.getRoll();  
  float yaw   = MadgwickFilter.getYaw();

  Serial.print("#pitch, roll, yaw#");
  Serial.print(pitch);  Serial.print(",");
  Serial.print(roll);  Serial.print(",");
  Serial.print(yaw-180); Serial.print("\n"); 
  Serial.print((micros()-preTime)/1000);  Serial.print("\n");
  preTime = micros(); 

  delay(4.5); //測定間隔１０msecに調整
  sk += 1;
}

int16_t u2s(int16_t unsigneddata){
  int16_t usd = unsigneddata;
  if (usd & (0x01 << 15)){ 
      usd = -1 * ((unsigneddata ^ 0xffff) + 1);
  }
  return usd;
}

void readData() {
  Wire.beginTransmission(0x68);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(0x68, 14, true);
  while (Wire.available() < 14);

  axRaw = u2s(Wire.read() << 8 | Wire.read());
  ayRaw = u2s(Wire.read() << 8 | Wire.read());
  azRaw = u2s(Wire.read() << 8 | Wire.read());
  Temperature = u2s(Wire.read() << 8 | Wire.read());
  gxRaw = u2s(Wire.read() << 8 | Wire.read());
  gyRaw = u2s(Wire.read() << 8 | Wire.read());
  gzRaw = u2s(Wire.read() << 8 | Wire.read());

  // 加速度値を分解能で割って加速度(G)に変換する
  acc_x = (2. / 32768.)*axRaw ;  //FS_SEL_0 16,384 LSB / g
  acc_y = (2. / 32768.)*ayRaw ;
  acc_z = (2. / 32768.)*azRaw ;

  // 角速度値を分解能で割って角速度(degrees per sec)に変換する
  gyro_x = (250. / 32768.)*gxRaw ;  // (度/s)
  gyro_y = (250. / 32768.)*gyRaw ;
  gyro_z = (250. / 32768.)*gzRaw ;
  //temperature
  temp= (Temperature-4040)/340. + 29.0; //室温29.0Cで校正
}

void readCompass() {
  float magcof = 4912/32760.0; //
  Wire.beginTransmission(0x0C);
  Wire.write(0x02);
  Wire.endTransmission();
  Wire.requestFrom(0x0C, 1);

  uint8_t ST1 = Wire.read();
  if (ST1 & 0x01) {
    Wire.beginTransmission(0x0C);
    Wire.write(0x03);
    Wire.endTransmission();
    Wire.requestFrom(0x0C, 7);
    uint8_t i = 0;
    uint8_t buf[7];
    while (Wire.available()) {
      buf[i++] = Wire.read();
    }
    if (!(buf[6] & 0x08)) {
      mx = ((int16_t)buf[1] << 8) | buf[0];
      my = ((int16_t)buf[3] << 8) | buf[2];
      mz = ((int16_t)buf[5] << 8) | buf[4];
      mx_ = mx*magcof;
      my_ = my*magcof;
      mz_ = mz*magcof;                                                      
    }
  }
}

//以下の数字の初期化の意味は参考③で解説されている
void setupMPU9250() {
  Wire.beginTransmission(0x68);
  Wire.write(0x6B);
  Wire.write(0x00);  // 0; 内部クロックを設定
  Wire.endTransmission();

  Wire.beginTransmission(0x68);
  Wire.write(0x1A);
  Wire.write(0x05);  // 
  Wire.endTransmission();

  Wire.beginTransmission(0x68);
  Wire.write(0x37);
  Wire.write(0x02);
  Wire.endTransmission();

  Wire.beginTransmission(0x0C);
  Wire.write(0x0A);
  Wire.write(0x16);
  Wire.endTransmission();
  delay(500);
}

・追跡カメラを制御する

以下のコードで正常の振る舞いとなった。
しかし、yawのモーターは安定して動くもののpitchは上と同様なノイズが残っている．
※別の理由があるかもしれないが、本日時点の事実として記載しておく

追跡カメラのコード
#include "Wire.h"
#include <Servo.h>          // サーボモータを動かすライブラリ
Servo myservo1;            // 
Servo myservo2;             
#define PI 3.141592653589793

float power1 = 0;            // サーボの出力　sg９０
float power2 = 0;            

int16_t axRaw, ayRaw, azRaw, gxRaw, gyRaw, gzRaw, Temperature;
float rawX, rawY, rawZ;
float acc_x0=0, acc_y0=0, acc_z0=0;
float acc_x=0, acc_y=0, acc_z=0;
float gyro_x=0, gyro_y=0, gyro_z=0;
float pitch, roll;
float gyro_z_=0;
int sk = 0;
float dt, preTime;
#include <MadgwickAHRS.h>   // 角度と角速度にMadgwickフィルタをかけるライブラリ
Madgwick MadgwickFilter;    // Madgwickフィルタのオブジェクトを設定

void setup() {
  Serial.begin(115200);     //シリアル通信を115200bpsに設定
  myservo1.attach(10);       // 10番ピンに接続
  myservo2.attach(11);       // 11番ピンに接続
  pinMode(2, OUTPUT);      // 2番ピンでLED（確認用）

  Wire.begin();
  setupMPU9250();
  readData();
  acc_x0=acc_x;
  acc_y0=acc_y;
  acc_z0=acc_z-1;
  preTime = micros();
  MadgwickFilter.begin(100);  // Madgwickフィルタの周波数を100Hzに設定。
}

void loop() {
  readData();

  // 出力を計算する
  // Madgwickフィルターを用いて、PRY（pitch, roll, yaw）を計算
  MadgwickFilter.updateIMU(gyro_x, gyro_y, gyro_z, acc_x-acc_x0, acc_y-acc_y0, acc_z-acc_z0);

  float pitch  = MadgwickFilter.getPitch();
  float roll  = MadgwickFilter.getRoll();  
  float yaw   = MadgwickFilter.getYaw();

  Serial.print("#pitch, roll, yaw#");
  Serial.print(pitch);  Serial.print(",");
  Serial.print(roll);  Serial.print(",");
  Serial.print(yaw-180); Serial.print("\n"); 
  Serial.print((micros()-preTime)/1000);  Serial.print("\n");
  preTime = micros();

  power2 = pitch;
  power1 = yaw-180;
  Serial.print(sk); Serial.print("\n");

  Serial.print("power1 =\t");  Serial.print(power1);  Serial.print("\n");
  Serial.print("power2 =\t");  Serial.print(power2); Serial.print("\n");
  myservo1.write(50-power1);
  myservo2.write(50-power2);
  delay(4);

  sk += 1;
}

int16_t u2s(int16_t unsigneddata){
  int16_t usd = unsigneddata;
  if (usd & (0x01 << 15)){ 
      usd = -1 * ((unsigneddata ^ 0xffff) + 1);
  }
  return usd;
}

void readData() {
  Wire.beginTransmission(0x68);
  Wire.write(0x3B);
  Wire.endTransmission(false);
  Wire.requestFrom(0x68, 14, true);
  while (Wire.available() < 14);

  axRaw = u2s(Wire.read() << 8 | Wire.read());
  ayRaw = u2s(Wire.read() << 8 | Wire.read());
  azRaw = u2s(Wire.read() << 8 | Wire.read());
  Temperature = u2s(Wire.read() << 8 | Wire.read());
  gxRaw = u2s(Wire.read() << 8 | Wire.read());
  gyRaw = u2s(Wire.read() << 8 | Wire.read());
  gzRaw = u2s(Wire.read() << 8 | Wire.read());

  // 加速度値を分解能で割って加速度(G)に変換する
  acc_x = (2. / 32768.)*axRaw ;  //FS_SEL_0 16,384 LSB / g
  acc_y = (2. / 32768.)*ayRaw ;
  acc_z = (2. / 32768.)*azRaw ;

  // 角速度値を分解能で割って角速度(degrees per sec)に変換する
  gyro_x = (250. / 32768.)*gxRaw ;  // (度/s)
  gyro_y = (250. / 32768.)*gyRaw ;
  gyro_z = (250. / 32768.)*gzRaw ;
}

//初期化関数は、参考の解説みてください
void setupMPU9250() {
  Wire.beginTransmission(0x68);
  Wire.write(0x6B);
  Wire.write(0x00);
  Wire.endTransmission();

  Wire.beginTransmission(0x68);
  Wire.write(0x1A);
  Wire.write(0x05);
  Wire.endTransmission();

  Wire.beginTransmission(0x68);
  Wire.write(0x37);
  Wire.write(0x02);
  Wire.endTransmission();

  Wire.beginTransmission(0x0C);
  Wire.write(0x0A);
  Wire.write(0x16);
  Wire.endTransmission();
  delay(500);
}

出力例10.50msec程度で動いている．
pitchの値は、他の値と比較して大きなノイズが乗っているわけではなさそうである．

追跡カメラの出力例
24908
power1 =    19.47
power2 =    -4.95
#pitch, roll, yaw#-4.98,2.77,19.41
10.50
24909
power1 =    19.41
power2 =    -4.98
#pitch, roll, yaw#-4.91,2.77,19.31
10.53
24910
power1 =    19.31
power2 =    -4.91
#pitch, roll, yaw#-4.84,2.82,19.16
10.56
24911
power1 =    19.16
power2 =    -4.84
#pitch, roll, yaw#-4.77,2.85,18.99
10.56
24912
power1 =    18.99
power2 =    -4.77
#pitch, roll, yaw#-4.69,2.86,18.83
10.53
24913
power1 =    18.83
power2 =    -4.69
#pitch, roll, yaw#-4.61,2.79,18.72
10.56
24914
power1 =    18.72
power2 =    -4.61
#pitch, roll, yaw#-4.54,2.69,18.67
10.53
24915
power1 =    18.67
power2 =    -4.54
#pitch, roll, yaw#-4.51,2.59,18.67
10.53
24916
power1 =    18.67
power2 =    -4.51
#pitch, roll, yaw#-4.50,2.52,18.70
10.50

まとめ

・Arduinonanoで追跡カメラを制御した
・制御にangleの直接計算で実施し、平均化処理を導入したがノイズが残留している
・MadgwickFilterを導入したが、上記とほぼ同様な振る舞いとなっている

・MadgwickFilterの理論をやろうと思う
・他の系に対して応用しようと思う

2
2
0

Register as a new user and use Qiita more conveniently

  1. You get articles that match your needs
  2. You can efficiently read back useful information
  3. You can use dark theme
What you can do with signing up
Sign upLogin
2
2