メカナムホイールのラジコンをRoverC化してみた

 前回ついにアナログスティックで操作が可能になったメカナムホイールラジコン。メカナムで充電式で2000円ぐらいのラジコンに文句は言えないが、最初からアナログスティックのほうが楽しいと思う…
でも改造楽しいしむしろコントローラなしの開発キットより断然安いのでこれもおもちゃのなせる技。

それはさておきMX1616の調査をするのにPWMの周波数変えたりDuty変えたりいろいろ試してみたけど、データシートをよく読んでみるとPWMモードAとPWMモードBの注意書きが。前回のESP32でのサンプルはPWMモードAで動かしていたんだけど、PWM周波数を高くしたいならPWMモードBのほうが良さそうな。結局データシートで言うモードAがFast DecayでモードBがSlow Decayっぽい。MX1508のライブラリを見てもFast DecayとSlow Decayが切り替えられるようになってたし…

テスト用にESP32にシリアル通信で周波数とDecayモードとDutyを変えられるようなプログラムを作ってみた。これで色々テストしてみたけど20kHzぐらいでもSlow Decayだといい感じにDuty10%ぐらいでも動く。PWM周波数も十分高いのでうるさくないし。とりあえずPWM周波数は14kHzぐらいで行こうと思う。

これでモータの回転数のリニアリティーが高まったので、今度はRaspberry Pi Picoを積んで見ることに。というのもせっかくメカナムのいい感じの車体なのでRaspberry Pi 3Bとかを乗っけてもいいかなと思ったので、ただしPWMを8ch分生成するのはマイコンのほうがいいし。
ということでRaspberry Pi Picoを乗っけて、i2c接続のモータドライバとして動かしてみようかなと。これはM5StackのRoverCも同じ様になっていたので、レジスタやアドレスもRoverC互換にしてみた。GroveコネクタにしてしまえばM5も乗るし…

i2cデバイスをArduinoで作るのは以前作ったものをベースにして、Raspberry Pi PicoからPWMを出力するだけ。Arduino公式のArduino Mbed OS RP2040 Boardsだとレジスタへの書き込みは問題ないから普通に動くんだけど、i2cdumpとか読み出しをするとフリーズしてしまうのでサードパティー製のArduino-Picoを使用した。Mbedとはi2cのポート選択方法が違うのと、pwmのヘッダファイルを手動でインクルードしないといけなかったけど、こっちのほうが快適に動く気がする。動作クロックを125Mhz以外で動かしたいときはclkdiv変更しないとPWM周波数も変わってしまうので注意。

//Raspberry Pi Pico
//Arduino-Pico(C++ SDK)
//CPU Speed 125Mhz

#include <Wire.h>
#include "hardware/pwm.h"

#define SLAVE_ADDRESS 0x38
//MbedI2C i2c_slave(p16, p17); // I2C SDA:GP16, SCL:GP17

#define DEBUG

#define M1p 2
#define M1n 3
#define M2p 4
#define M2n 5
#define M3p 6
#define M3n 7
#define M4p 8
#define M4n 9
#define PWMmax 255
uint SLICE[4];

//使用するレジスタ範囲指定
#define I2C_REG_ROW 1
#define I2C_REG_COL 4
uint8_t I2C_REG[I2C_REG_ROW][I2C_REG_COL];


//読み書きするレジスタアドレス
uint8_t REG_SELECTED;

boolean DATA_RECIEVED = false;

//i2c受信イベント
void receiveEvent(int _length) {
  //1byte目を読み込む(レジスタアドレス指定)
  REG_SELECTED = Wire.read();
  uint8_t startRow = REG_SELECTED >> 4;
  uint8_t startCol = (REG_SELECTED & 0x0F);

#ifdef DEBUG
  Serial.print("REG:");
  Serial.println(REG_SELECTED, HEX);
#endif

  while (Wire.available() > 0) {
    if (startRow < I2C_REG_ROW && startCol < I2C_REG_COL) {
      DATA_RECIEVED = true;
      I2C_REG[startRow][startCol] = Wire.read();
#ifdef DEBUG
      Serial.print("Write:");
      Serial.println(I2C_REG[startRow][startCol], HEX);
#endif
    } else {
      Wire.read();
    }
    ++startCol;
  }
}

//i2c読み込みイベント
void requestEvent() {
  uint8_t startRow = REG_SELECTED >> 4;
  uint8_t startCol = (REG_SELECTED & 0x0F);

  if (startRow < I2C_REG_ROW && startCol < I2C_REG_COL) {
    Wire.write(I2C_REG[startRow][startCol]);
  } else {
    Wire.write(0xff);
  }
}

void setup() {
#ifdef DEBUG
  Serial.begin(115200);
#endif
pinMode(LED_BUILTIN,OUTPUT);
toggle_led();

  //SlaveでWireライブラリを初期化
  Wire.setSDA(16);
  Wire.setSCL(17);
  Wire.begin(SLAVE_ADDRESS);
  //イベント設定
  Wire.onReceive(receiveEvent);
  Wire.onRequest(requestEvent);

  gpio_set_function(M1p, GPIO_FUNC_PWM);
  gpio_set_function(M1n, GPIO_FUNC_PWM);
  gpio_set_function(M2p, GPIO_FUNC_PWM);
  gpio_set_function(M2n, GPIO_FUNC_PWM);
  gpio_set_function(M3p, GPIO_FUNC_PWM);
  gpio_set_function(M3n, GPIO_FUNC_PWM);
  gpio_set_function(M4p, GPIO_FUNC_PWM);
  gpio_set_function(M4n, GPIO_FUNC_PWM);

  SLICE[0] = pwm_gpio_to_slice_num(M1p);
  SLICE[1] = pwm_gpio_to_slice_num(M2p);
  SLICE[2] = pwm_gpio_to_slice_num(M3p);
  SLICE[3] = pwm_gpio_to_slice_num(M4p);

  // PWM周期を設定
  for (int i = 0; i <= 3; i++) {
    pwm_set_clkdiv(SLICE[i], 35);  // 125,000Khz/(255*14Khz)
    pwm_set_wrap(SLICE[i], PWMmax);
    pwm_set_chan_level(SLICE[i], PWM_CHAN_A, 0);
    pwm_set_chan_level(SLICE[i], PWM_CHAN_B, 0);
    pwm_set_enabled(SLICE[i], true);
  }
}

void loop() {
  if (DATA_RECIEVED == true) {
    setspeed();
    DATA_RECIEVED = false;
  }

}

void setspeed() {
  int8_t pwm[4];
  for (int i = 0; i <= 3; i++) {
    pwm[i] = (int8_t)I2C_REG[0][i];
#ifdef DEBUG
    Serial.print("PWM:");
    Serial.println(pwm[i]);
#endif
    if (pwm[i] >= 0) {
      pwm_set_chan_level(SLICE[i], PWM_CHAN_A, (PWMmax - (pwm[i] * 2)));
      pwm_set_chan_level(SLICE[i], PWM_CHAN_B, PWMmax);
    } else {
      pwm_set_chan_level(SLICE[i], PWM_CHAN_A, PWMmax);
      pwm_set_chan_level(SLICE[i], PWM_CHAN_B, (PWMmax - abs(pwm[i] * 2)));
    }
  }
  toggle_led();
}

void toggle_led() {
  static boolean led_state = false;
  led_state = !led_state;
  digitalWrite(LED_BUILTIN, led_state);
}

とりあえずこれでi2cからモータ制御できるようになった。watchdogとかつけるべきかも…
1秒通信しなかったらタイムアウト的なやつ。
とりあえずRaspberry Pi 3Bからのコマンドではモータが動いてるけど回転のリニアリティー確認用にRoverC用のソフトをESP32に乗せてテストしてみるか…
i2cでホストが色々すげ替えられる様になったのでESP32乗っけてラジコンとして遊んでもいいしラズパイ乗っけてDonkey Carみたいなのを乗っけても面白そう。

次回は車体の上を平らにして色々乗っけられるようにしたい。