esp32でロボマスモーター(M2006)を回しました．

初めまして，pebbleです．

最近，ロボマスモーターがロボコン界隈で流行っているようで，卒業研究で使いたかったのでesp32を使って回してみました．

esp32，今までまともに使ったことはなかったのですが，stm32に比べて検索した際の情報量が多くて，簡単にbluetoothが使えてワイヤレスでのデバッグもやりやすいので便利かなと思って選びました． ロボコンでのメイン基板やROS-アクチュエーター間の通信窓口としてならばstm32 H7等と比べてそれほどかさばるものでもないので，ある程度使える気がします．知らんけど

準備

買ったもの

  • esp32
    • wroom32を買ったつもりがwrover32を買っていました．(どちらでもできました．)
  • mcp2562 FD
    • キャントランシーバー(と多分終端抵抗)をつながないとちゃんと送信エラーで停止してくれるので，テストするときもつなぎましょう．
  • ユニバーサル基盤
  • M2006(ロボマスモーター)
  • C610(ESC)

回路図

ちゃんとした回路図作るのが面倒になったのでkicadのグリッドを使ったユニバーサル基盤用の下書きを載せておきます．

回路の図
他のブログの記事でも見られるような普通の接続ですね．

終端抵抗忘れないようにしましょう．(C610についているスライドスイッチをONにすれば十分です．) STBYピンはちゃんとGNDに落とします． たまに聞かれるのですが，CANトランシーバーは別に受信機ではないので，TxはマイコンのTxと，RxはマイコンのRxとつなげばいいです．

送信テスト

Arduino CANというライブラリを使用してサンプルプログラムで送信テストしました． ライブラリマネージャーにいろいろあって，特にパスの設定もしなくて良いのでありがたいですね． ループが回っていれば送信はできていると思います．

ロボマスモーターが1ms毎にデータを送信してくるせいか受信用のサンプルプログラムは落ちてしまいました．(まあ，送信ができればできるでしょ)

CAN.beginの前にCAN.setPins(rx_pin, tx_pin);を加えて通信用のピンをいじれます． 34番ピンを使う予定で基盤を作ってしまったのですが，34,35番ピンが読み取り専用でTxピンに使えませんでした(確認不足) 今回rx pinに19番，tx pinに32番を使用しました．

ボード選択でesp32 wrover module(か，esp32 dev module)を選択しました．

コンパイルエラーが出たり，書き込み時にフラッシュ関連のエラーが出たりしていたのですが，ライブラリを入れたり消したりArduino IDEを入れたり消したりしているとなぜか上手くいきました． 最終的に落ち着いたボードライブラリとライブラリのバージョンをメモしておきます．

  • esp32 by Espressif v2.0.17
    • v3以降だと'esp_intr.h'が無いようで，CANのライブラリを読み込んだ時にコンパイルエラーが出ました．
  • CAN by Sandeep Mistry v0.3.1

ボーレートが半分になるトラブル

新しいesp32で？ボーレートが半分になるトラブルがあるようです． オシロスコープで見ると，信号が時間軸方向へ倍延びてました． 分周周りの設定とかTimeSegmentとかいじってなんとかしようとしたのですが，奇数パルスで1bit送ってるようで無理でした．

優秀な友人に教えてもらったのですが，issueが立っていました．

github.com

Following the thread about the hardware limitation (https://esp32.com/viewtopic.php?t=2142) just recently I read that in Rev2 or up versions of the chip they changed the meaning of bit 4 in IER, from "Wake-up interrupt" to "divide BRP by 2". That explains why the CAN ran in half speed (for some reason bit 4 was set) in newer ESP32.

外国の言葉が書いてあって分かりませんね． とりあえず，Arduino-CANライブラリのESP32SJA1000.cppからbegin()関数を探し出して，修正します．

// writeRegister(REG_IER, 0xFF);   // enable all interrupts
writeRegister(REG_IER, 0b11101111);   // fixed

16進数が分からず，毎回レジスタに書き込む値は10進数か2進数で書いてしまいます．

プログラム

ロボマスモーターを回して，速度とケツのローターの角度を取ってくるプログラムです． 綺麗に，汎用的にクラス化して書こうと思っていたのですが，Arduino IDEを使い慣れておらず，VSCodeのセットアップするのも面倒になって適当に書いてしまいました． githubに上げるまでもないのでここに．

#include <CAN.h>
//#include "c610.hpp"

float vel = 0;
float pos = 0;

int counter = 0;
bool data_size_error_flag_ = false;


constexpr int motor_id = 2;

void send_cur(float cur) {
  constexpr float MAX_CUR = 10;
  constexpr int MAX_CUR_VAL = 10000;

  float val = cur * (MAX_CUR_VAL / MAX_CUR);
  if (val < -MAX_CUR_VAL) val = -MAX_CUR_VAL;
  else if (val > MAX_CUR_VAL) val = MAX_CUR_VAL;
  int16_t transmit_val = val;

  uint8_t send_data[8] = {};

  send_data[(motor_id - 1) * 2] = (transmit_val >> 8) & 0xFF;
  send_data[(motor_id - 1) * 2 + 1] = transmit_val & 0xFF;
  CAN.beginPacket(0x200);
  CAN.write(send_data, 8);
  CAN.endPacket();
}

void can_callback(int packetSize) {
  static std::array<uint8_t, 8> received_data;
  if (CAN.packetId() == 0x200 + motor_id) {
    counter++;
    int data_size_counter = 0;
    while (CAN.available()) {
      received_data[data_size_counter % 8] = CAN.read();
      data_size_counter++;
    }
    data_size_error_flag_ = (data_size_counter != 8);  // 8byte以上のデータが溜まってないか一応チェック

    pos = static_cast<int16_t>((received_data[0] << 8) + received_data[1]) / 8192.; //0~1に
    vel = static_cast<int16_t>((received_data[2] << 8) + received_data[3]) / 60.;//rpmをrpsに
  }
}

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);
  while (!Serial)
    ;

  CAN.setPins(19, 32);
  if (!CAN.begin(1000E3)) {
    Serial.println("Starting CAN failed!");
    while (1)
      ;
  }

  CAN.onReceive(can_callback);
}

void loop() {
  int i = 0;
  for (float cur = -0.5; cur < 0.5; cur += 0.01) {
    send_cur(cur);
    delay(10);
    i++;
    if (!(i % 10)) { // シリアルプロッタを伸ばしすぎないために1/10に制限
      Serial.print("pos:");
      Serial.println(pos);
      Serial.print("vel:");
      Serial.println(vel);
    }
  }
  for (float cur = 0.5; cur > -0.5; cur -= 0.01) {
    send_cur(cur);
    delay(10);
    i++;
    if (!(i % 10)) { // シリアルプロッタを伸ばしすぎないために1/10に制限
      Serial.print("pos:");
      Serial.println(pos);
      Serial.print("vel:");
      Serial.println(vel);
    }
  }

  /*
  //デバッグ
  Serial.print("counter:");
  Serial.print(counter);
  Serial.print(" error:");
  Serial.println(data_size_error_flag_);
  */
}

ほんで，読めてました． 静止摩擦の影響が見て取れる良いグラフですね．

シリアルプロッタ

canのcallback関数内ではどれくらいの処理ができるんでしょうね． 1ms毎にC610からのフィードバックが返ってくるので，Serial.printなんかは避けたいです．

おわり

基板作成含めて2日程度で終わるとおもっていたのですが，結構スケジュールを押してしまいました． 見切り発車で基板を作るのではなく，一度ブレッドボード上で試せばよかったなと毎回思います．

そういえば，初めてブログを完成させました． 読みやすいように真面目に書こうと思っていたのですが，途中で面倒になって気づけばブログとしても備忘録としても微妙になっていました．

それでは．