M5Stack+Nintendo Laboのバイクキット(ダンボールのみ)でWHILL Model CRを走らせてみた

# はじめに
この記事は[WHILL Advent Calendar 2018](https://adventar.org/calendars/3238#list-2018-12-03)の17日目の記事兼、
[M5Stack Advent Calendar 2018](https://qiita.com/advent-calendar/2018/m5stack)の19日目の記事です。

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

今までの記事はこちら
[M5StackでWHILL Model CRの電源をON/OFFしてみた](https://qiita.com/s-katsu/items/f98861eebb519e6b7d00)
[M5StackのJoystickでWHILL Model CRを動かしてみた](https://qiita.com/s-katsu/items/0e1bf1eb5909b6b663ee)

WHILL Model CRはRS232C経由で外部から制御可能なパーソナルモビリティです。
<img width="400" alt=”model_cr.png" src="https://qiita-image-store.s3.amazonaws.com/0/149063/63ff38bf-bc36-65ce-af84-d81a96eefd04.png">


WHILL Model CRの説明も[最初の記事](https://qiita.com/s-katsu/items/f98861eebb519e6b7d00)にありますので詳しくはそちらご参照ください。

今回はNitendo LaboのバイクコントローラーにM5Stackを取り付けて、Model CRを動かしてみました。
<img width="600" alt=”bike_control.jpeg" src="https://qiita-image-store.s3.amazonaws.com/0/149063/36a7074a-157d-a327-0abc-875d4f0e3618.jpeg">

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

接続方法や[Model CR Arduino SDK(esp32)](https://github.com/WHILL/whill-sdk-arduino/tree/esp32)の使用方法は[以前の記事](https://qiita.com/s-katsu/items/f98861eebb519e6b7d00)をご参照ください。

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

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

もし危険だと思った際は、
- 電源OFFする
- バッテリーを抜く
- RS232Cケーブルを外す
- 機体を持ち上げて後輪を浮かせる
など即時に対応してください。

また、最初お試しで動かす際は、後輪を浮かせた状態で実験するのがオススメです。
<img width="400" alt=”jack_up.jpeg" src="https://qiita-image-store.s3.amazonaws.com/0/149063/68d7f02f-972a-2d45-e9d2-c5d0a17105d7.jpeg">


#準備するもの
- WHILL Model CR
- M5Stack Gray(MPU9250が乗っていればOK)
- [Nintendo Laboのバイクキット](https://www.nintendo.co.jp/labo/play/index.html)

バイクコントローラーの右ハンドルの付け根にM5Stackを両面テープで取り付けます。
<img width="400" alt=”m5_pos.jpeg" src="https://qiita-image-store.s3.amazonaws.com/0/149063/4c395a0c-09a3-c796-334c-e8a3cba52c66.jpeg">


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

## MPU9250の検出値
MPU9250でのセンサー値からRoll, Pitch, Yawへの変換までは、M5Stackのexampleをほぼそのまま流用しています。
[MPU9250BasicAHRS.ino](https://github.com/m5stack/M5Stack/blob/master/examples/Modules/MPU9250/MPU9250BasicAHRS/MPU9250BasicAHRS.ino)

M5Stackの各軸の検出はこのようになっています。
![M5 rotate.png](https://qiita-image-store.s3.amazonaws.com/0/149063/f62013c3-fe3d-cdb9-40fe-c468edefef68.png)

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

```cpp: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](http://x-io.co.uk/open-source-imu-and-ahrs-algorithms/)

また、こちらの解説にアルゴリズムの説明が書かれていました。
[Madgwick Filterを読んでみた](https://qiita.com/fumiya_sato/items/df28dff5ccc653183235)

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

```cpp: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方向の値を増加させます。

```cpp: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方向の制御量に変換します。

```cpp: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()`に渡してあげるだけです。

```cpp: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ファイル全体はこのようになっています。

```cpp: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の制御値に変換しただけのわりには期待通りに動いて、なかなか楽しかったです！

<blockquote class="twitter-tweet" data-lang="ja"><p lang="ja" dir="ltr">M5StackをNintendo Laboのバイク型コントローラーに取り付けて、WHILL Model CRを走らせてみた。アドベントカレンダーネタです。<a href="https://twitter.com/hashtag/WHILL?src=hash&amp;ref_src=twsrc%5Etfw">#WHILL</a> <a href="https://twitter.com/hashtag/M5Stack?src=hash&amp;ref_src=twsrc%5Etfw">#M5Stack</a> <a href="https://t.co/SIpOF7Q3Lm">pic.twitter.com/SIpOF7Q3Lm</a></p>&mdash; Katsushun89 (@katsushun89) <a href="https://twitter.com/katsushun89/status/1075027734831673344?ref_src=twsrc%5Etfw">2018年12月18日</a></blockquote>
<script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

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