ESP32で自作マウス〜その1〜

はじめに

普段からOS自作とか低レイヤーなことはしていますが、今回は自分でやろうと思ったわけではなく、学校でESP32（一応他のマシンでも可）を使って開発することがあったのがきっかけです。
何を作ろうか色々考えたんですが、あんまり高度なことをやらせようとするとスマホっぽくて、いい感じにセンサー類を活かしつつ小型・少電力で有ることを活かすなら周辺機器かなと思いマウスを自作することにしました。

どんなマウスを作るのか

マウスを作ろうと決めたらどんな機能を実装するか考えなきゃいけません。とりあえず欲しい機能を優先度順で並べると、

1. マウスカーソルの操作・右左のクリック

2. スクロール・中クリック

3. 進む・戻るボタン

4. 空間マウス

5. マルチペアリング

ってとこですかね。機能はこんな感じですが、実装で少し工夫することがあって、本来マウスはLEDを使って移動距離を測定しているんですが、このセンサーの入手方法がわからないのと、これ使ったらわざわざ自作する意味をあまり感じないので、加速度センサを駆使して移動距離を計測しようと思います。
加速度センサーなら、ジャイロセンサーと組み合わせて頑張れば空中でも使えるので、空間マウスも作れるわけです。
というわけで、今回は基本的な処理を実装していこうと思います。

開発環境の整備

まず、ESP32はArduino IDEを使って開発するのが定石なんですが、そのセットアップ手順は大したものではないので他のサイトに譲ります。そしてIDEのセットアップができたら、今回は加速度センサーにMPU6050を使うので、これ用のライブラリ「Adafruit MPU6050」を入れます。そんでもってBLEマウスを作るのに必要なライブラリに「ESP32-BLE-Mouse」があるんですが、これのメインレポジトリはバグを残したまま開発がストップしているので、このレポからDownload ZIPして使用します（追記：Sketch>Include Library>Add .ZIP Libraryで追加できます）。
これでソフトの準備は完了です。一応ESP32の写真も配線を見るために置いときます。左側にあるのがMPU6050加速度センサです。

ESP32の様子

とりあえずコードを組んでいく

というわけで加速度センサーの値を積分して変位を求めて、マウスを動かすプログラムを書いていきます。最初はこんな感じのコードでやってました（開発進めてから書いてるので詳しくは覚えてない）。

#include <Adafruit_MPU6050.h>
#include <Adafruit_Sensor.h>
#include <BleMouse.h>
#include <Wire.h>

struct Motion {
  float accel[3];
  float vel[3];
  signed int dpos[2];
  Motion() {
    accel[0], accel[1], accel[2] = 0.0, 0.0, 0.0;
    vel[0], vel[1], vel[2] = 0.0, 0.0, 0.0;
    dpos[0], dpos[1] = 0, 0;
  }

  void update(float x, float y, float z) {
    // truncate the input if the input value is too small(noise)
    if (-0.05 < x && x < 0.05) {
      x = 0.0;
      vel[0] = 0.0;
      dpos[0] = 0;
    }
    if (-0.05 < y && y < 0.05) {
      y = 0.0;
      vel[1] = 0.0;
      dpos[1] = 0;
    }

    float old_vel[2] = {vel[0], vel[1]};

    // update velocity
    vel[0] += (x+accel[0])*0.4/2;
    vel[1] += (y+accel[1])*0.4/2;

    // assign new acceleration
    accel[0] = x;
    accel[1] = y;
    accel[2] = z;

    // updare position
    dpos[0] = std::round((old_vel[0] + vel[0])/2);
    dpos[1] = std::round((old_vel[1] + vel[1])/2);
  }
};

Adafruit_MPU6050 mpu;
BleMouse bleMouse("HandiClick", "GateHorse", 100);
float ZERO_X;
float ZERO_Y;
float ZERO_Z;
Motion motion;
bool PLANE = true;

void setup(void) {
  // Try to initialize Serial Port
  Serial.begin(115200);
  while (!Serial) {
    delay(10); // will pause Zero, Leonardo, etc until serial console opens
  }

  // Try to initialize MPU6050
  if (!mpu.begin()) {
    Serial.println("Failed to find MPU6050 chip");
    while (1) {
      delay(10);
    }
  }

  // Initialize BLE Mouse
  bleMouse.begin();

  mpu.setAccelerometerRange(MPU6050_RANGE_16_G);
  mpu.setGyroRange(MPU6050_RANGE_250_DEG);
  mpu.setFilterBandwidth(MPU6050_BAND_21_HZ);
  Serial.println();
  delay(100);

  calibration();
}

void loop() {
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);

  motion.update(a.acceleration.x-ZERO_X, a.acceleration.y-ZERO_Y, a.acceleration.z-ZERO_Z);

  Serial.print("AccelX:");
  Serial.print(motion.accel[0]);
  Serial.print(",");
  Serial.print("AccelY:");
  Serial.print(motion.accel[1]);
  Serial.print(",");
  Serial.print("dposX:");
  Serial.print(motion.dpos[0]);
  Serial.print(",");
  Serial.print("dposY:");
  Serial.print(motion.dpos[1]);
  Serial.println();

  if(bleMouse.isConnected()) {
    bleMouse.move(motion.dpos[0], -motion.dpos[1], 0);
  }

  delay(10);
}

void calibration() {
  sensors_event_t a, g, temp;
  mpu.getEvent(&a, &g, &temp);
  ZERO_X = a.acceleration.x;
  ZERO_Y = a.acceleration.y;
  ZERO_Z = a.acceleration.z;
}

これでとりあえずBluetoothを起動して、接続すると一応動くんですが、第一に動きがぎこちなく小さい上に、時々なぜか逆行します。予想はしてましたが、結構これをなめらかにするのは厳しい戦いになりそう。
ここからノイズを除去する工夫を加えていきます（すでにこのコードに書かれてるものもありますが）。

ノイズの除去

加速度のノイズ

キャリブレーションで重力加速度によるズレは取り除いているのですが、加速度センサってのはかなり敏感なセンサーでずっと0.01ぐらいの誤差が出続けてます。だからこれを切るために閾値以下は0にする必要があります。

速度のノイズ

これはあくまで加速度の話で、これを積分して速度にするとまた別の誤差が発生します。それは加速度センサーがあくまで一定間隔ごとに加速度を取得している以上、積分により誤差が生じるのです。これにより単純に加速度を時間で積分しても停止したときに0になりません。
本当は加速度みたいに閾値で解決したいんですが、ズレがかなり大きくなることがあるのですべては解決しきれません。そこで、「マウスが等速直線運動するわけがない」という事実を使って、加速度が0になったら速度も0にしてしまう処理をして帳尻をあわせます。これでかなりまともな動きをするようになりました。

速度の反転を防ぐ

加速度が途中で正負反転するのは当たり前なんですが、なぜか速度が反転してカーソルが逆行することがあります。物理的にありえない話なので混乱しましたが、v-tとa-tをシリアルプロットで表示したら原因がわかりました。

速度と加速度のグラフ

これを見ると、先程速度が0にならないことを防止するために導入した工夫の副作用として、加速度が反転するときに速度まで一度0に戻されていることがわかります。
これは困りました。最初に思いついたのは、加速度の符号を記録しておき、符号が反転したら0にリセットすれば、この変数が0のときのみ速度を0にすれば良いっていう方法なんですが、試すと微妙に加速度が振れたときにバグるのでやめました。
結局、加速度が3回0になったときだけ速度を0にすることにしました。

まとめ

まだ荒削りではありますが、ここまでの工夫でマウスだと主張できるぐらいにはちゃんとカーソルが動くようになりました。次は多分ボタン等の部品を買ってから開発を進めると思います。
コードはGithubで管理しているので、ぜひ覗いてみてください。