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

11708 ワード
WHILL M5stack M5stack テキストリンク

はじめに

この記事はWHILL Advent Calendar 2018の15日目の記事です。

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

前回の記事はこちら
M5StackでWHILL Model CRの電源をON/OFFしてみた

今回はJoystick moduleでModel CRを動かしてみます。

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

注意点

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

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

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

準備するもの

  • WHILL Model CR
  • M5Stack
  • M5Stack Joystick module
  • M5Stack comm module(WANT)

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

接続するとこのようになります。

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に変換します。

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

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も出来るようにしました。

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

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

最後に

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