DDモータのラジコンを走行テストしてみた。

 前回タミヤのユニバーサルプレートにダイレクトドライブモータとキャスターを取り付けられたので試しにESP32でラジコン化して動かしてみた。

ダイレクトドライブモータとして使用するジンバルモータには自作モータドライバを取り付けてOdriveのようなプロトコルでCANから制御できるようになっている。ESP32にCANトランシーバを取り付けて、Bluetoothで接続したPS3コントローラをつかってラジコン化してみる。

バッテリを共通化しようとしたけどESP32がうまく起動しなかったのでとりあえずESP32の電源はモバイルバッテリを使用することに。昇圧回路挟まないとだめかな。低速でテストするので適当においてあるだけ…

#include <Ps3Controller.h>
#include "driver/twai.h"

#define CAN_RX 35
#define CAN_TX 32
#define canRate 100  //モータステータスリクエスト周期
#define DEBUG
#define DEADZone 8

#define BTMAC "1a:2b:3c:01:01:01"

//コントローラーの値代入用
int lx = 0;
int ly = 0;
int rx = 0;
int ry = 0;
int lt = 0;
int rt = 0;
boolean BTconnected = false;

//モーター出力設定
float MotorGain = 0.5;
uint8_t MotorMode = 0;

//ポーリング用
unsigned long lastMotor = 0;
unsigned long lastupdate = 0;

//CANデータ処理用共用体
typedef union {
  int32_t value;
  byte bytes[4];
} INT32_BYTE_UNION;

typedef union {
  float value;
  byte bytes[4];
} FLOAT_BYTE_UNION;

void notify() {
  lx = Ps3.data.analog.stick.lx;
  ly = Ps3.data.analog.stick.ly;
  rx = Ps3.data.analog.stick.rx;
  ry = Ps3.data.analog.stick.ry;
  if (abs(lx) < DEADZone) {
    lx = 0;
  }
  if (abs(ly) < DEADZone) {
    ly = 0;
  }
  if (abs(rx) < DEADZone) {
    rx = 0;
  }
  if (abs(ry) < DEADZone) {
    ry = 0;
  }
  lt = Ps3.data.analog.button.l2;
  rt = Ps3.data.analog.button.r2;

  if (Ps3.event.button_up.r1) {
    MotorMode += 1;
    if (MotorMode > 4) {
      MotorMode = 4;
    }
    Ps3.setPlayer(MotorMode);
  } else if (Ps3.event.button_up.l1 && (MotorMode > 0)) {
    MotorMode -= 1;
    Ps3.setPlayer(MotorMode);
  }
}

void btdisconn() {
  BTconnected = false;
#ifdef DEBUG
  Serial.println("Controller Disconnected");
#endif
}

void btconn() {
  BTconnected = true;
#ifdef DEBUG
  Serial.println("Controller Connected");
#endif
}

void setup() {
#ifdef DEBUG
  Serial.begin(115200);
#endif

  //DUALSHOCK3初期化
  Ps3.attach(notify);
  Ps3.attachOnDisconnect(btdisconn);
  Ps3.attachOnConnect(btconn);
#ifdef BTMAC
  Ps3.begin(BTMAC);
#else
  Ps3.begin();
#endif

  // Initialize configuration structures using macro initializers
  twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT((gpio_num_t)CAN_TX, (gpio_num_t)CAN_RX, TWAI_MODE_NORMAL);
  twai_timing_config_t t_config = TWAI_TIMING_CONFIG_250KBITS();  //Look in the api-reference for other speed sets.
  twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();

  // Install TWAI driver
  if (twai_driver_install(&g_config, &t_config, &f_config) == ESP_OK) {
    Serial.println("Driver installed");
  } else {
    Serial.println("Failed to install driver");
    return;
  }

  // Start TWAI driver
  if (twai_start() == ESP_OK) {
    Serial.println("Driver started");
  } else {
    Serial.println("Failed to start driver");
    return;
  }

  // Reconfigure alerts to detect TX alerts and Bus-Off errors
  uint32_t alerts_to_enable = TWAI_ALERT_TX_IDLE | TWAI_ALERT_TX_SUCCESS | TWAI_ALERT_TX_FAILED | TWAI_ALERT_ERR_PASS | TWAI_ALERT_BUS_ERROR;
  if (twai_reconfigure_alerts(alerts_to_enable, NULL) == ESP_OK) {
    Serial.println("CAN Alerts reconfigured");
  } else {
    Serial.println("Failed to reconfigure alerts");
    return;
  }
}



void loop() {
  if (lastMotor + 50 <= millis()) {
    lastMotor = millis();  //次の実行のために時間更新
    if (BTconnected == true) {
      setspeed(ly / -127.00, rx / 127.00);
    } else {
      setspeed(0, 0);
    }
  }
}

void setspeed(float y, float x) {  // 前後左右回転それぞれを係数と掛けて足す

  switch (MotorMode) {
    case 0:
      MotorGain = 1.0;
      break;
    case 1:
      MotorGain = 6;
      break;
    case 2:
      MotorGain = 8;
      break;
    case 3:
      MotorGain = 10;
      break;
    case 4:
      MotorGain = 16;
      break;
  }
  float motor_l = (y + x) * MotorGain;
  float motor_r = (y - x) * MotorGain;
  setSpeed(0, motor_l);
  setSpeed(1, motor_r * -1);
}

//CAN通信関連
void canSend(int addr, byte* CanData) {

  twai_message_t message;
  message.identifier = addr;
  message.data_length_code = 8;
  for (int i = 0; i < 8; i++) {
    message.data[i] = CanData[i];
  }

  // Queue message for transmission
  if (twai_transmit(&message, pdMS_TO_TICKS(1000)) == ESP_OK) {
    //printf("Message queued for transmission\n");
  } else {
    //printf("Failed to queue message for transmission\n");
  }
}

void setSpeed(uint8_t mID, float speed) {  //Control motor by Speed (unit 0.01dps/LSB,1rpm = 6dps)
  FLOAT_BYTE_UNION speed_f;
  speed_f.value = speed;
  int addr = mID << 5 | 0x00D;
  byte data[8] = { 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 };
  for (int i = 0; i <= 3; i++) {
    data[i] = speed_f.bytes[i];  //Little endian
  }
  canSend(addr, data);
}

ESP32側のソフトはこんな感じで、以前メカナムラジコンをESP32化したときに使ったスケッチをCAN対応にした感じ。モータドライバ側は前に作ったCAN対応バージョンをそのまま使用している。PIDとかのパラメータはそのまま。

とりあえず低速で動かしてみるとSimpleFOCのキャリブレーションが悪いのか、こんな感じでカクカクしている。無負荷だとスムーズに回っているのでキャリブレーションの問題かも。そういえばvelocity loop PIDはまだいじっていないのでデフォルトだった気も。
通信の負荷の可能性もあったので通信しないで走らせても同じ感じだった。

スピードを上げると少し良くなるのでやっぱりキャリブレーションを見直さないとな…
しかしさすがダイレクトドライブ、タミヤのギアボックスと比べ物にならないぐらい静か。