アナログ式メカナムホイール(RoverC)制御法

まずこの記事はRoverCがコロナの影響で発送が2か月近くかかったため自分の頭の中で考えてた制御がたまたまうまくいったため参考にした記事などはなくもっといい制御法があるかもしれないことをお断りしておきます。

今回使用したRoverCというのはM5StickCを接続して使用するメカナムホイールユニットです。
(ピン配置を合わせれば他でも使えるかもしれませんが)

RoverC(W/O M5StickC) – m5stack-store
https://m5stack.com/products/rovercw-o-m5stickc?variant=31185881792602

まずRoverCのサンプルスケッチを確認しますが

JoyCで操作する前提となっているようなので重要なところだけを抜き出すと

int8_t speed_sendbuff[4] = {0};

I2CWritebuff(0x00, (uint8_t *)speed_sendbuff, 4);

speed_sendbuff[4]に-100～100のintを設定して
I2CWritebuff(0x00, (uint8_t *)speed_sendbuff, 4);
すると車輪が回ります。

配列と車輪の対応は
speed_sendbuff[0]→左前
speed_sendbuff[1]→右前
speed_sendbuff[2]→左後
speed_sendbuff[3]→右後
となります。

###動作の定義###

メカナムホイールは4つの車輪を制御することで自由な走行を実現できます

画像で見ると複雑そうですが、前進、左進、左斜め前進を抜き出してみると

int FORWARD[4] = {50, 50, 50, 50};
int LEFT[4] = {-50, 50, 50, -50};
int FandL[4] = {0, 100, 100, 0};

という風に定義できます
そしてこれは
FORWARD + LEFT = FandL　( {0, 100, 100, 0} )
となりますので各要素の足し算によって決定できることが分かります

###実際に制御してみる###

アナログスティック2本のコントローラーで操作すると仮定します

メカナムホイール完全に理解したら(理解したとは言ってない)#M5StickC #RoverC pic.twitter.com/XE4UE0SnsK

— もけ@ムギ㌠ (@coppercele) March 23, 2020

すべての動作を実現するには前進、後退、左進、右進、左回転、右回転が必要なので配列で定義します
int FORWARD[4] = {50, 50, 50, 50};
int LEFT[4] = {-50, 50, 50, -50};
int BACKWARD[4] = {-50, -50, -50, -50};
int RIGHT[4] = {50, -50, -50, 50};
int ROTATE_L[4] = {-30, 30, -30, 30};
int ROTATE_R[4] = {30, -30, 30, -30};

そしてスティックの倒れ具合をそれぞれ変数に対応させると
以下のように出力を決定できます(それぞれ0.0~1.0)
左スティック前→f
左スティック後→b
左スティック左→l
左スティック右→r
右スティック左→rl(rotate l)
右スティック右→rr(rotate r)

speed_sendbuff = FORWARD * f + LEFT * l + BACKWARD * b + RIGHT * r + ROTATE_L * rl + ROTATE_R * rr

というわけで実際にソースにするとこんな感じになります

#include <M5StickC.h>

int8_t speed_sendbuff[4] = {0};

int8_t FORWARD[4]     = {50, 50, 50, 50};
int8_t LEFT[4]        = {-50, 50, 50, -50};
int8_t BACKWARD[4]    = {-50, -50, -50, -50};
int8_t RIGHT[4]       = {50, -50, -50, 50};
int8_t ROTATE_R[4]    = {-25, 25, -25, 25};
int8_t ROTATE_L[4]    = {25, -25, 25, -25};

float f, b, l, r, rr, rl = 0.0;

void SetChargingCurrent(uint8_t CurrentLevel)
{
    Wire1.beginTransmission(0x34);
    Wire1.write(0x33);
    Wire1.write(0xC0 | (CurrentLevel & 0x0f));
    Wire1.endTransmission();
}

int8_t I2CWrite1Byte(uint8_t Addr, uint8_t Data)
{
    Wire.beginTransmission(0x38);
    Wire.write(Addr);
    Wire.write(Data);
    return Wire.endTransmission();
}

uint8_t I2CWritebuff(uint8_t Addr, uint8_t *Data, uint16_t Length)
{
    Wire.beginTransmission(0x38);
    Wire.write(Addr);
    for (int i = 0; i < Length; i++)
    {
        Wire.write(Data[i]);
    }
    return Wire.endTransmission();
}

uint8_t setspeed() {  // 前後左右回転それぞれを係数と掛けて足す
  for (int i = 0; i < 4; i++) {
    speed_sendbuff[i] = FORWARD[i] * f;
    speed_sendbuff[i] += BACKWARD[i] * b;
    speed_sendbuff[i] += RIGHT[i] * r;
    speed_sendbuff[i] += LEFT[i] * l;
    speed_sendbuff[i] += ROTATE_L[i] * rl;
    speed_sendbuff[i] += ROTATE_R[i] * rr;
  }
  float limit = 0.0;
  for (int i = 0; i < 4; i++) {
    // speedが100を超えないようにリミッターをかける
    limit = 100.0 / max(
                         abs(speed_sendbuff[3]), max(
                           abs(speed_sendbuff[2]), max(
                             abs(speed_sendbuff[1]),abs(speed_sendbuff[0])
                           )
                         )
                       );
  }
  //    printf("limit = %f\n", limit);
  if (limit < 1.0) {
    for (int i = 0; i < 4; i++) {
      speed_sendbuff[i] = speed_sendbuff[i] * limit;
    }
  }

  return I2CWritebuff(0x00, (uint8_t *)speed_sendbuff, 4);
}


void setup()
{
  Serial.begin(115200);

  M5.begin();
  M5.update();
  Wire.begin(0, 26, 10000);
  SetChargingCurrent(4);
  M5.Axp.ScreenBreath(7); 
  M5.Lcd.fillScreen(BLACK);
  pinMode(GPIO_NUM_10, OUTPUT);
  digitalWrite(GPIO_NUM_10, HIGH);
}

void loop()
{
  M5.update();
    if ( M5.BtnB.wasReleased() ) {
    esp_restart();
  }
  // 前進
  f = 1.0;
  setspeed();
  delay(500);
  // 左平行移動
  f = 0.0;
  l = 1.0;
  setspeed();
  delay(500);
  // 後退
  l = 0.0;
  b = 1.0;
  setspeed();
  delay(500);
  // 右平行移動
  b = 0.0;
  r = 1.0;
  setspeed();
  delay(500);
  // 停止
  r = 0.0;
  setspeed();
  delay(500);
  // 右ナナメ前移動
  f = 0.5;
  r = 0.5;
  setspeed();
  delay(500);
  // 右斜め後ろ移動
  f = 0.0;
  r = 0.5;
  b = 0.5;
  setspeed();
  delay(500);
  // 左斜め後ろ移動
  r = 0.0;
  l = 0.5;
  b = 0.5;
  setspeed();
  delay(500);
  // 左斜め前移動
  f = 0.5;
  l = 0.5;
  b = 0.0;
  setspeed();
  delay(500);
  // 右回転
  f = 0.0;
  l = 0.0;
  rr = 1.0;
  setspeed();
  delay(500);
  // 左回転
  rr = 0.0;
  rl = 1.0;
  setspeed();
  delay(500);
  // 停止
  rl = 0.0;
  setspeed();
  delay(1000);


}

なぜこのようにしたかというと将来的にUnitVなどで操作するときにfなどの係数をいじるだけで簡単に動かせるだろうなと思ったからです
この記事を書いた人はC++に不自由(メインはJava)なのでもっとエレガントな書き方があったら教えてください(´・ω・`)

こちらの記事で実際にPS3コントローラーで操作した記事を書いてあります

ESP32(M5StickC)をPS3コントローラで操作する - Qiita
https://qiita.com/coppercele/items/0724b0b951868044223b

#RoverC (メカナムホイール)をPS3のアナログスティックで操作すると奇っ怪な動きが出来るから楽しい＼(^o^)／#M5StickC pic.twitter.com/wt0Yvg9gK0

— もけ@ムギ㌠ (@coppercele) September 19, 2020