クラシックマウス 作ってみた その4 Gyro

---

はじめに

• 作成したクラシックマウスの各パーツの解説。
• 今回は「Gyroセンサー　MPU6050」。
• 機体写真、動画などは「その1」をご参照。

Gyroセンサー　MPU6050

ステッピングモーターを使っていても、進むにつれて車体は斜めになっていき、いずれ壁に激突してしまう。距離センサーは精度が足りず、なかなか機体を制御するのが難しかった。
Gyroセンサーは、安価なのに精度が高く、機体の向きの制御が格段に容易になりました。

• BNO055との比較
  • BNO055も試してみましたが、MPU6050の方が精度が高い気がしました。
  • BNO055は9軸なので、向き(yaw)の絶対値が求まります。一方MPU6050は6軸なので、相対的な向きが出てきます。迷路探索においては、相対的な向きで十分なので、MPU6050を使っています。
• 取付位置
  • 機体の写真で上に飛び出しているのがMPU6050です。マイコンやモーターからのノイズを減らす目的で離して取り付けています。ミニブレボ上に取り付けるより良くなった気がします。
• 使い方
  • スタート時に向きを測定し、その測定数字をもとに機体の向きを修正しています。
  • 90度回転　→　向き測定　→　ずれた分を微調整　のような感じで利用しています。　
• "yaw" 4元数？
  • 本モジュールはマイコン(DMP)を内蔵していて、いろいろな数字を出力してくれますが、ここでは"yaw"しか使っていません。Sampleスケッチによると、"yaw"は4元数から計算されているようなので、精度は十分と考えました。
  • 正直、4元数は難しすぎます。ドローン制御には必須なのでしょうか…
• 精度、調整
  • 精度はかなり高いように思います。いろいろ回転させてみても、1~2°程度の誤差に収まっているようなので、機体制御には十分です。
  • 自作の4×4迷路では、移動を繰り返した後でも、十分な精度を保っているように見えます。16x16ではどうなるでしょうか。
  • 電源を入れてから、しばらく数字が安定しません。10秒程度（？）経つと安定するので、それから利用しています。
• ライブラリ
  • ElectronicCatsのMPU6050を使いました。
  • MPU6050_DMP6_using_DMP_V6v12.ino を適宜修正、削除して、"yaw"を利用しています。
  • このSampleスケッチは割込みピンを利用する形になっていますが、ここでは利用していません。計測頻度が高くないので大丈夫なのでしょう。
  • Calibrationは良くわからないので、Sampleスケッチにおまかせです。間違いがあるかもしれません。

#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps612.h"
MPU6050 mpu;
#define OUTPUT_READABLE_YAWPITCHROLL

// MPU control/status vars
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorFloat gravity;    // [x, y, z]            gravity vector
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

/////  以下SETUP内  ////////////
//6050初期化
// initialize device
  mpu.initialize();
  // load and configure the DMP
  devStatus = mpu.dmpInitialize();
  // supply your own gyro offsets here, scaled for min sensitivity
  mpu.setXGyroOffset(51);
  mpu.setYGyroOffset(8);
  mpu.setZGyroOffset(21);
  mpu.setXAccelOffset(1150);
  mpu.setYAccelOffset(-50);
  mpu.setZAccelOffset(1060);
  // make sure it worked (returns 0 if so)
  if (devStatus == 0) {
    // Calibration Time: generate offsets and calibrate our MPU6050
    mpu.CalibrateAccel(6);
    mpu.CalibrateGyro(6);
    mpu.PrintActiveOffsets();
    // turn on the DMP, now that it's ready
    mpu.setDMPEnabled(true);
    // get expected DMP packet size for later comparison
    packetSize = mpu.dmpGetFIFOPacketSize();
  } else {
    // ERROR!
    // 1 = initial memory load failed
    // 2 = DMP configuration updates failed
    // (if it's going to break, usually the code will be 1)
    oled.print(F("DMP Initialization failed (code "));
    oled.print(devStatus);
  }

/////  以下yaw取得のための関数  ////////////

float get_y_6050(){
    // read a packet from FIFO
    mpu.dmpGetCurrentFIFOPacket(fifoBuffer);
    // display Euler angles in degrees
    mpu.dmpGetQuaternion(&q, fifoBuffer);
    mpu.dmpGetGravity(&gravity, &q);
    mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);

    return ypr[0] * 180 / M_PI;
}

以上