はじめに

この記事はWHILL Advent Calendar 2018の17日目の記事兼、
M5Stack Advent Calendar 2018の19日目の記事です。

大好きなM5StackでWHILLを動かしてみたシリーズです。

今までの記事はこちら
M5StackでWHILL Model CRの電源をON/OFFしてみた
 M5StackのJoystickでWHILL Model CRを動かしてみた

WHILL Model CRはRS232C経由で外部から制御可能なパーソナルモビリティです。

WHILL Model CRの説明も最初の記事にありますので詳しくはそちらご参照ください。

今回はNitendo LaboのバイクコントローラーにM5Stackを取り付けて、Model CRを動かしてみました。

実は本当はこの記事の内容がやりたくて、その準備段階として前回、前々回の記事の内容を用意しています。

接続方法やModel CR Arduino SDK(esp32)の使用方法は以前の記事をご参照ください。

注意点

前回も書きましたが、大事なことなので今回も書きます。

今回の記事は実際に機体を走行させるものになります。
Model CRは人も乗れるモビリティですので、誤った操作でケガや物損する可能性があります。
実際に実施する場合は自己責任かつ、事故などに十分注意して行ってください。
本記事の内容も動作を補償したものではありませんので、承知の上で参考にしていただければと思います。

もし危険だと思った際は、

電源OFFする
バッテリーを抜く
RS232Cケーブルを外す
機体を持ち上げて後輪を浮かせる
など即時に対応してください。

また、最初お試しで動かす際は、後輪を浮かせた状態で実験するのがオススメです。

準備するもの

WHILL Model CR
M5Stack Gray(MPU9250が乗っていればOK)
Nintendo Laboのバイクキット

バイクコントローラーの右ハンドルの付け根にM5Stackを両面テープで取り付けます。

制御方法

9軸IMUのMPU9250のRoll,Pitchの傾きをJoystickの操作量に変換して制御します。
ハンドルを手前に回転させると加速、ハンドル自体を左右に倒すと旋回するように制御します。

MPU9250の検出値

MPU9250でのセンサー値からRoll, Pitch, Yawへの変換までは、M5Stackのexampleをほぼそのまま流用しています。
MPU9250BasicAHRS.ino

M5Stackの各軸の検出はこのようになっています。

フィルタ処理

IMUのセンサーのRaw Dataはそのままだと扱うのが難しく、通常は何かしらのフィルタ処理をかけますが、（相補フィルタやカルマンフィルタなど）M5Stackのソースコードには、Madgwick Filterというフィルタの関数が既に用意されています。

imu.cpp

MahonyQuaternionUpdate(imu->ax, imu->ay, imu->az,
  imu->gx * DEG_TO_RAD, imu->gy * DEG_TO_RAD, imu->gz * DEG_TO_RAD,
  imu->mx, imu->my, imu->mz, imu->deltat);

内部ではQuaternionを更新する作りになっています。
サンプルですでに使用されているのでそれをそのまま使えばフィルタ処理がかかった値が利用できます。便利ですね。

Madgwick Filter自体は理解せずとも利用出来ますが、論文とソースコードも公開されています。
Open source IMU and AHRS algorithms

また、こちらの解説にアルゴリズムの説明が書かれていました。
Madgwick Filterを読んでみた

QuaternionからRoll, Pitch, Yawへの変換もexampleの通りです。

imu.cpp

      imu->yaw   = atan2(2.0f * (*(getQ()+1) * *(getQ()+2) + *getQ() *
                    *(getQ()+3)), *getQ() * *getQ() + *(getQ()+1) * *(getQ()+1)
                    - *(getQ()+2) * *(getQ()+2) - *(getQ()+3) * *(getQ()+3));
      imu->pitch = -asin(2.0f * (*(getQ()+1) * *(getQ()+3) - *getQ() *
                    *(getQ()+2)));
      imu->roll  = atan2(2.0f * (*getQ() * *(getQ()+1) + *(getQ()+2) *
                    *(getQ()+3)), *getQ() * *getQ() - *(getQ()+1) * *(getQ()+1)
                    - *(getQ()+2) * *(getQ()+2) + *(getQ()+3) * *(getQ()+3));
      imu->pitch *= RAD_TO_DEG;
      imu->yaw   *= RAD_TO_DEG;
      // Declination of SparkFun Electronics (40°05'26.6"N 105°11'05.9"W) is
      // 	8° 30' E  ± 0° 21' (or 8.5°) on 2016-07-19
      // - http://www.ngdc.noaa.gov/geomag-web/#declination
      imu->yaw   -= 8.5;
      imu->roll  *= RAD_TO_DEG;

Roll, Pitch角 -> Joystick制御量への変換

Model CRのJoystickの制御値はX(旋回）、Y(前後進)方向に対して、-100 ~ 100の値を設定します。
中心位置は0で、X:0, Y:0で停止し、Y:100,X:0で前進します。
Xの値が正:右旋回、負：左旋回します。

基準位置をM5Stackの画面を上向きで水平とすると、バイクキットのハンドルをアクセルさせる（手前に引くような）方向に回転させるとRoll角度が増加します。
その回転量に応じてJoystickのY方向の値を増加させます。

RPY_Controller.cpp

MAX_ROLL_ = 70.0;
MIN_ROLL_ = 10.0;

int8_t RPYController::setAccel(MPU9250 *imu)
{
    if(imu->roll <= MIN_ROLL_){
        return 0;
    }
    if(imu->roll >= MAX_ROLL_){
       return MAX_ROLL_;
    }

    //normalize
    return ((imu->roll - MIN_ROLL_) / (MAX_ROLL_ - MIN_ROLL_) * MAX_ROLL_);
}

setAccelを通すことで、水平位置でY:0、アクセルを回転させると最大でY:70となります。
本来は100まで設定出来ますが、まずは小さめに設定しました。

同様に左右もpitchの値をJoystick X方向の制御量に変換します。

RPY_Controller.cpp


MAX_PITCH_ = 40.0;
MIN_PITCH_ = 10.0;

int8_t RPYController::setTurn(MPU9250 *imu)
{
  if(imu->pitch < 0){
    if(abs(imu->pitch) <= MIN_PITCH_){
        return 0;
    }
    if(abs(imu->pitch) >= MAX_PITCH_){
       return -MAX_PITCH_;
    }
    //normalize
    
    return ((imu->pitch + MIN_PITCH_) / (MAX_PITCH_ - MIN_PITCH_) * MAX_PITCH_);
  }else{
    if(abs(imu->pitch) <= MIN_PITCH_){
        return 0;
    }
    if(abs(imu->pitch) >= MAX_PITCH_){
       return MAX_PITCH_;
    }
    //normalize
    return ((imu->pitch - MIN_PITCH_) / (MAX_PITCH_ - MIN_PITCH_) * MAX_PITCH_);    
  }
}

あとは、このRoll, Pitchの値をJoystick制御関数setJoystick()に渡してあげるだけです。

RPY_Controller.cpp

void RPYController::setRPY(MPU9250 *imu)
{
    joy_y_ = setAccel(imu);
    joy_x_ = setTurn(imu);
}
    
void RPYController::updateDirectionControl(WHILL *whill)
{
    whill->setJoystick(joy_x_, joy_y_);
}

安全機能：ボタンを押したときだけ走行可能

常にIMUの傾き値によって機体が走行すると危険なので、Cボタンを押したときだけ先程の制御を実行するようにしました。
これで危なくなってもCボタンを離せばOKです。

if(M5.BtnC.isPressed()){
  rpyController.setRPY(&IMU);
  rpyController.updateDirectionControl(&whill);
}

あと、説明していなかったですが、前進、左右旋回しか出来ないので後進は出来ないです。（まあバイクキットなので。。）
メインのinoファイル全体はこのようになっています。

M5Stack_whill_rpy_controller.ino

# include <M5Stack.h>
# include "utility/MPU9250.h"
# include "WHILL.h"
# include "imu.h"
# include "RPYController.h"

// Devices
MPU9250 IMU;
WHILL whill(&Serial2);
RPYController rpyController;

void setup()
{
  Serial.begin(115200);
  // Power ON Stabilizing...
  delay(500);
  M5.begin();
  Wire.begin();
  
  initIMU(&IMU);
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setTextColor(GREEN);
  M5.Lcd.setTextSize(2);
}

void loop() {
  
  if(M5.BtnA.wasPressed())
  {
    whill.setPower(true);  // Turn WHILL on
    resetLCD();
    M5.Lcd.println("POWER:ON");
  }
  if(M5.BtnB.wasPressed())
  {
    whill.setPower(false);  // Turn WHILL off
    resetLCD();
    M5.Lcd.println("POWER:OFF");
  }
  
  if(calcIMU(&IMU)){
      {
        Serial.print("Yaw, Pitch, Roll: ");
        Serial.print(IMU.yaw, 2);
        Serial.print(", ");
        Serial.print(IMU.pitch, 2);
        Serial.print(", ");
        Serial.print(IMU.roll, 2);
      }
      if(M5.BtnC.isPressed()){
        rpyController.setRPY(&IMU);
        rpyController.updateDirectionControl(&whill);
        Serial.print(", ");
        Serial.print("X:");
        Serial.print(String(rpyController.getJoyX(), DEC));
        Serial.print(", ");
        Serial.print("Y:");
        Serial.print(String(rpyController.getJoyY(), DEC));
        Serial.println("");
      }else{
        Serial.println("");
      }
  }
  delay(100);
  M5.update();
}

デモ

実際に走行させてみた様子はこちらです。
ちょっとX方向のMAXが40になっているので曲がりにくいです。
そこは要調整ですが、単純にRoll,Pitchの値をJoystickの制御値に変換しただけのわりには期待通りに動いて、なかなか楽しかったです！

M5StackをNintendo Laboのバイク型コントローラーに取り付けて、WHILL Model CRを走らせてみた。アドベントカレンダーネタです。#WHILL #M5Stack pic.twitter.com/SIpOF7Q3Lm
— Katsushun89 (@katsushun89) 2018年12月18日

最後に

M5StackでModel CRを動かす系の記事を3つほど書きました。
M5Stackはボタン・LCD・IMUなどなどついていて、こういったプロトタイピングが簡単に出来て本当に便利ですね！
もっと発展していろいろ出来ると思いますので是非試してみてください。