モータドライバに通信タイムアウトを入れてみた。

 OdriveライクなCAN通信でSimpleFOCのモータドライバと通信しているんだけど、電圧低下などでESP32がフリーズしたりするとモータが前回値でそのまま動いてしまうので通信切断検知を入れてみた。

この前のCAN対応ファームウェアに追加で通信がない状態が1000ms続いたらモータをフリーにするようにしてみた。坂道だとフリーなので走ってしまうかもしれないけど…

アクロバティックな走行をしていると途中でバッテリの電圧降下で回り続けるようになってしまったり。とりあえず通信切断検知を入れたらこの状態で止まってくれた。

とりあえず簡単に実装してみたやつでも動いてそう。

#define WATCHDOG 1000
unsigned long lastUpdate = 0;
boolean watchDogEnable = true;

void inputWatchdog_reset() {
#ifdef WATCHDOG
  lastUpdate = millis();
  motor.enable();
#endif
}

とりあえず追加した変数と関数はこんな感じで通信の場所でinputWatchdog_reset()を読み続ければ通信切断は検知しない感じに。

#ifdef WATCHDOG
  unsigned long watchDogMs = millis();
  if (watchDogMs - lastUpdate > WATCHDOG && watchDogEnable == true) {
    motor.disable();
  }
#endif

切断検知はloop()関数内にこんな感じで追加。

        case 0x00C:  //Set Input Pos
          if (CAN_RX_msg.type == DATA_FRAME) {
            FLOAT_BYTE_UNION input;
            memcpy(input.uint, CAN_RX_msg.data, 8);
            motor.controller = MotionControlType::angle;
            target_angle = input.value[0];
          }
          inputWatchdog_reset();
          break;
        case 0x00D:  //Set Input Vel
          if (CAN_RX_msg.type == DATA_FRAME) {
            FLOAT_BYTE_UNION input;
            memcpy(input.uint, CAN_RX_msg.data, 8);
            motor.controller = MotionControlType::velocity;
            target_angle = input.value[0];
          }
          inputWatchdog_reset();
          break;
        case 0x00E:  //Set Input Torque
          if (CAN_RX_msg.type == DATA_FRAME) {
            FLOAT_BYTE_UNION input;
            memcpy(input.uint, CAN_RX_msg.data, 8);
            motor.controller = MotionControlType::torque;
            target_angle = input.value[0];
          }
          inputWatchdog_reset();
          break;

あとは前回同様CAN通信のswitch case分の中のモータを動かしそうな関数にinputWatchdog_reset()を追加していけばOK。これを追加するとSimpleFOC StudioからいじれなくなるのでそのときはwatchDogEnableをfalseにする関数をコマンドに追加してやろうかなと言う感じ。

使用してるジンバルモータが220kvなので1Sだと90rad/sぐらい回せそうなんだけども48rad/s制限で回してみると結構早い。頑張ればウィリーできるぞ…

電源もブレッドボードからとりあえず取ってるのでウィリーしてリアのケーブルを動かすと接触不良で通信が止まってしまうんだけど、ちゃんとモータが止まるようになった。

ウィリーした瞬間に通信が止まってモータが回り続けているけど1秒ぐらいで停止している感じ。

モータドライバのLEDが付いてるのでモータドライバは動いてそう。通信してるとブレーキが掛かるんだけど、通信していないとフリーになるので通信切断検知が動いてるのかわかりやすい。

結構モータが強いのでキャスターをどうにかしてもうちょっと走行性能を上げたいな。