M5StackのJoystickでWHILL Model CRを動かしてみた

# はじめに
この記事は[WHILL Advent Calendar 2018](https://adventar.org/calendars/3238#list-2018-12-03)の15日目の記事です。

---
前回同様に、大好きなM5StackでWHILLを動かしてみたシリーズです。

前回の記事はこちら
[M5StackでWHILL Model CRの電源をON/OFFしてみた](https://qiita.com/KatsuShun89/items/69222ce96ff3debcba11)

今回はJoystick moduleでModel CRを動かしてみます。
![Joystick_module.jpg](https://qiita-image-store.s3.amazonaws.com/0/149063/19da7b4e-8cf1-c2ec-0bbb-5c4469b6a02e.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
- M5Stack Joystick module
- M5Stack comm module(WANT)

comm moduleはJoystickを接続するのにあれば便利ですが、なくてもI2Cのポートに接続すれば使えます。

接続するとこのようになります。
![接続.jpg](https://qiita-image-store.s3.amazonaws.com/0/149063/ffbd04ca-83a8-d6ae-f5c5-a398d44003aa.jpeg)



# Joystick制御のコーディング
Joystick moduleのセンサー値の取得はM5Stack exampleのJoystickのコードを参考にしました。

Model CR側は、Arduino SDKの`setJoystick(joy_x, joy_y)`関数を使用すれば実際に動きます。
joy_x, joy_yには-100 ~ 100の値を設定してください。
前進後進：joy_y
左右：joy_x
となります。

joystick moduleの値は中心 128程度の0 ~ 256程度の範囲を取るuint8_tの値になっていましたので、
適当に-100 ~ 100に変換します。

具体的にはコードはこのようになります。
このコードも動作補償をしたものではありませんですので、参考にされる方は自己責任でご使用ください。

```cpp:M5Stack_whill_joystick_controller.ino
#include <M5Stack.h>
#include "WHILL.h"
#include "Wire.h"

#define JOY_ADDR 0x52

uint8_t x_data;
uint8_t y_data;
uint8_t button_data;
char data[100];

const uint8_t NEUTRAL = 120;
const int8_t MAX_TH = 100;
const int8_t MIN_TH = 10;

WHILL whill(&Serial2);

bool is_power_on = false;

void setup()
{
  Serial.begin(115200);
  // Power ON Stabilizing...
  delay(500);
  M5.begin();

  Wire.begin(21, 22, 400000);
}

int8_t convertJoystick(uint8_t ext_joy)
{
  int8_t conv_joy = ext_joy - NEUTRAL;

  if(conv_joy >= 0){
    if(abs(conv_joy) < MIN_TH) return 0;
    conv_joy -= MIN_TH; //offset;
    if(abs(conv_joy) >= MAX_TH) return MAX_TH;
    return conv_joy;
  }else{
    if(abs(conv_joy) < MIN_TH) return 0;
    conv_joy += MIN_TH; //offset;
    if(abs(conv_joy) >= MAX_TH) return -MAX_TH;  
    return conv_joy;
  }
}

void loop() {
  Wire.requestFrom(JOY_ADDR, 3);
  if (Wire.available()) {
    x_data = Wire.read();
    y_data = Wire.read();
    int8_t conv_y = convertJoystick(y_data);
    int8_t conv_x = convertJoystick(x_data);
    button_data = Wire.read();

    sprintf(data, "x:%d y:%d button:%d\n", conv_x, conv_y, button_data);
    Serial.print(data);

    if(button_data == 1){
      if(is_power_on){
        whill.setPower(false);
        is_power_on = false;
      }else{
        whill.setPower(true);
        is_power_on = true;
      }
    }else{
      whill.setJoystick(conv_x, conv_y);
    }
  }
   delay(10);
}
```

ついでに、joystick押し込むとボタンになっていたので、それで電源ON/OFFも出来るようにしました。

実際に動かしている様子です。
乗りながら撮ったからずっと見てると酔いますので注意。

<blockquote class="twitter-tweet" data-lang="ja"><p lang="ja" dir="ltr">M5Stackのjoystick moduleでWHILL Model CRを動かしてみた。<a href="https://twitter.com/hashtag/M5Stack?src=hash&amp;ref_src=twsrc%5Etfw">#M5Stack</a> <a href="https://twitter.com/hashtag/WHILL?src=hash&amp;ref_src=twsrc%5Etfw">#WHILL</a> <a href="https://t.co/igMAd2uzi8">pic.twitter.com/igMAd2uzi8</a></p>&mdash; Katsushun89 (@katsushun89) <a href="https://twitter.com/katsushun89/status/1073581212873089030?ref_src=twsrc%5Etfw">2018年12月14日</a></blockquote>
<script async src="https://platform.twitter.com/widgets.js" charset="utf-8"></script>

Joystickが引っかかるところがあってたまに中心に戻ってこないので、ちょっと怖かったです。(笑）

# 最後に
前回同様にM5StackでModel CRを動かしてみました。
SDKが用意してあるので動かすのもかなり簡単に出来ました。
コードも添付したもの＋SDKだけです。
是非試してみてください。