M5Stack ATOMS3でDDT M06ダイレクトドライブモーターを動かしてみた(C++編)

www.sato-susumu.com

前回はUIFlowでDDT M06ダイレクトドライブモーターを動かしたけど、最終的にはROS2で動かしたいと考えているので、その一歩手前として次はC++で動かしてみました。
もっとコンパクトにしたいので、前回と違って、M5Stack ATOMS3＋ATOMIC RS485キットを使ってます。

環境

DDT M0601C_111 2つ
Windows 11
PlatformIO
M5Stack ATOMS3
ATOMIC RS485キット

プログラム

ゆうもやさんがM06/M15ダイレクトドライブモーターに対応したライブラリを公開してくれているので、かなり楽ができます。
なるべくM5Stackの種類に依存しないように作ったけど、GPIOの番号だけはATOMS3に依存しています。

platformio.ini

[env:m5stack-atoms3]
platform = espressif32
board = m5stack-atoms3
framework = arduino
build_flags =
    '-D RX_GPIO=5'
    '-D TX_GPIO=6'
lib_deps =
    m5stack/M5Unified@^0.1.7
    https://github.com/takex5g/M5_DDTMotor_M15M06

main.cpp

#include <M5Unified.h>
#include <DDT_Motor_M15M06.h>

#define MOTOR_SPEED_RPM 50

MotorHandler motor_handler = MotorHandler(RX_GPIO, TX_GPIO);
const uint8_t motor_ids[] = {1, 2}; // ID of motors
Receiver received_data[sizeof(motor_ids) / sizeof(motor_ids[0])]; // Receiver objects for each motor
int motor_speeds[sizeof(motor_ids) / sizeof(motor_ids[0])]; // Speeds for each motor

void control_motors();
void stop_motors();
void move_forward();
void move_backward();
void turn_left();
void turn_right();
void display_status(const char* action);

void setup() {
  M5.begin();
  M5.Lcd.setTextSize(2);
  M5.Lcd.println("DDTM Test");

  stop_motors(); // Initialize all motors to stopped state
}

void loop() {
  static int mode = 0;

  M5.update();

  if (M5.BtnA.wasPressed()) {
    mode = (mode + 1) % 5; // Cycle through modes 0 to 4

    switch (mode) {
      case 0: stop_motors(); display_status("Stop"); break;
      case 1: move_forward(); display_status("Forward"); break;
      case 2: move_backward(); display_status("Backward"); break;
      case 3: turn_left(); display_status("Turn Left"); break;
      case 4: turn_right(); display_status("Turn Right"); break;
    }
  }

  delay(10);
}


void control_motors()
{
  for (size_t i = 0; i < sizeof(motor_ids) / sizeof(motor_ids[0]); ++i) {
    motor_handler.Control_Motor(motor_speeds[i], motor_ids[i], 0, 0, &received_data[i]);
  }
}

void stop_motors()
{
  motor_speeds[0] = 0;
  motor_speeds[1] = 0;
  control_motors();
}

void move_forward()
{
  motor_speeds[0] = -MOTOR_SPEED_RPM;
  motor_speeds[1] = MOTOR_SPEED_RPM;
  control_motors();
}

void move_backward()
{
  motor_speeds[0] = MOTOR_SPEED_RPM;
  motor_speeds[1] = -MOTOR_SPEED_RPM;
  control_motors();
}

void turn_left()
{
  motor_speeds[0] = -MOTOR_SPEED_RPM;
  motor_speeds[1] = -MOTOR_SPEED_RPM;
  control_motors();
}

void turn_right()
{
  motor_speeds[0] = MOTOR_SPEED_RPM;
  motor_speeds[1] = MOTOR_SPEED_RPM;
  control_motors();
}

void display_status(const char* action)
{
  M5.Lcd.fillScreen(BLACK);
  M5.Lcd.setCursor(0, 0);
  M5.Lcd.printf("%s\n", action);
  M5.Lcd.printf("T-RPM %d %d", motor_speeds[0], motor_speeds[1]);
}