ついにHomeMadeGarbageがガチのロボットモータを購入しましたよ。

いろいろおいじりしたので報告します。

ROBSTRIDE 05

ROBSTRIDE 05 は遊星ギアとコントローラが一体となったロボット向けQDD (quasi-direct drive) モータです。

この手のモータもだいぶ手ごろとなり既存のロボット用サーボとの価格差が縮まっています。
ホビーユースへの敷居がかなり低くなってきております。

そこであまり裕福でない HomeMadeGarbage もこの度購入させていただきました。

おいじり

モータのほかに簡易な説明書とコネクタが同梱されておりました。

CAN通信

早速 CAN通信で ROBSTRIDE 05 のIDでも覗き込んでやろうと思います。

CAN通信はコントローラボードを介してM5Stack ATOMで実施 (電源は15V 印加)

しかし、見慣れないビット数の信号が帰ってきて調べてみたところ ROBSTRIDE 05 の CAN 通信は
拡張フォーマット (29bit) というものを使っていることが分かりました。

途方に暮れつつネットをさまようとマニュアルを発見できました。

Product_Information/Product Literature/RS05/RS05使用说明书250625.pdf at main · RobStride/Product_Information

GitHub

こちらにプロトコルも明記されていましたので、以後これをもとに解析を進めました。

CAN ID

早速マニュアルのプロトコルをもとにIDをゲットしてかつIDを変更できるコードを作成。

といってもマニュアルをChatGPTにつっ込んでコードを生成してもらっただけです。

// M5 ATOM (ESP32) + TWAI(CAN) 1Mbps
// RS05: 通信タイプ0でスキャン / 通信タイプ7でID書き換え（Arduino-ESP32 2.x対応）
// 配線: TX=GPIO25, RX=GPIO21

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

#define CAN_TX_PIN GPIO_NUM_25
#define CAN_RX_PIN GPIO_NUM_21

static const twai_timing_config_t t_config = TWAI_TIMING_CONFIG_1MBITS();
static const twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();
static twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT(CAN_TX_PIN, CAN_RX_PIN, TWAI_MODE_NORMAL);

static inline uint32_t build_ext_id(uint8_t mode, uint16_t data16, uint8_t id8) {
  return ( (uint32_t)(mode & 0x1F) << 24 ) | ( (uint32_t)data16 << 8 ) | (uint32_t)id8;
}

static const uint8_t COMM_GET_ID = 0x00; // モード0: GET_ID
static const uint8_t COMM_SET_ID = 0x07; // モード7: SET_ID
static const uint8_t RESP_MARKER = 0xFE; // 応答ID下位8bit
static const uint8_t MASTER_ID   = 0xFD; // 任意の上位機ID（慣例値）

uint8_t last_found_id = 0xFF;

bool send_get_id(uint8_t target_can_id) {
  twai_message_t tx = {};
  tx.identifier = build_ext_id(COMM_GET_ID, ((uint16_t)MASTER_ID << 8) | 0x00, target_can_id);
  tx.flags = TWAI_MSG_FLAG_EXTD | TWAI_MSG_FLAG_SS;
  tx.data_length_code = 8;
  memset(tx.data, 0, 8);
  return twai_transmit(&tx, pdMS_TO_TICKS(5)) == ESP_OK;
}

bool send_set_id(uint8_t current_id, uint8_t new_id) {
  // data16: [23..16]=new_id, [15..8]=MASTER_ID
  uint16_t data16 = ((uint16_t)new_id << 8) | MASTER_ID;
  twai_message_t tx = {};
  tx.identifier = build_ext_id(COMM_SET_ID, data16, current_id);
  tx.flags = TWAI_MSG_FLAG_EXTD | TWAI_MSG_FLAG_SS;
  tx.data_length_code = 8;
  memset(tx.data, 0, 8);
  return twai_transmit(&tx, pdMS_TO_TICKS(5)) == ESP_OK;
}

void scan_ids(uint32_t wait_ms=1500) {
  Serial.println(F("全IDにGET_ID(タイプ0)を送信..."));
  for (uint16_t id = 0; id <= 127; ++id) {
    send_get_id((uint8_t)id);
    delayMicroseconds(200);
  }
  Serial.println(F("応答待ち..."));
  uint32_t t0 = millis();
  last_found_id = 0xFF;
  while (millis() - t0 < wait_ms) {
    twai_message_t rx = {};
    if (twai_receive(&rx, pdMS_TO_TICKS(10)) != ESP_OK) continue;
    if ((rx.flags & TWAI_MSG_FLAG_EXTD) == 0) continue;
    if (rx.data_length_code != 8) continue;

    uint32_t extid = rx.identifier;
    uint8_t  mode  = (extid >> 24) & 0x1F;
    uint16_t data16= (extid >> 8)  & 0xFFFF;
    uint8_t  id8   =  extid        & 0xFF;
    if (mode == 0x00 && id8 == RESP_MARKER) {
      uint8_t motor_can_id = (uint8_t)(data16 & 0x00FF); // 応答中位16bitの下位8bit=CAN ID
      last_found_id = motor_can_id;
      Serial.printf("[RX] EXTID=0x%08lX mode=%u data16=0x%04X id8=0x%02X => CAN_ID=0x%02X\n",
                    extid, mode, data16, id8, motor_can_id);
    }
  }
  if (last_found_id == 0xFF) Serial.println(F("応答なし"));
}

void setup() {
  Serial.begin(115200);
  delay(200);
  Serial.println(F("\nRS05 IDツール: G=スキャン / S <hex>=ID変更"));
  if (twai_driver_install(&g_config, &t_config, &f_config) != ESP_OK) { Serial.println(F("driver_install失敗")); while(1) delay(1000); }
  if (twai_start() != ESP_OK) { Serial.println(F("twai_start失敗")); while(1) delay(1000); }
  Serial.printf("CAN start OK (TX=%d RX=%d @1Mbps)\n", CAN_TX_PIN, CAN_RX_PIN);

  // 起動時に一発スキャン
  scan_ids();
}

void loop() {
  // シリアルコマンド:
  //   G           -> 再スキャン
  //   S xx        -> 現在見えてるID(last_found_id)を 0xXX に変更
  if (Serial.available()) {
    String cmd = Serial.readStringUntil('\n');
    cmd.trim();
    if (cmd.length() == 0) return;

    if (cmd.equalsIgnoreCase("G")) {
      scan_ids();
    } else if (cmd.length() >= 2 && (cmd[0] == 'S' || cmd[0] == 's')) {
      // 例: "S 3B" で 0x3B に変更
      int sp = cmd.indexOf(' ');
      if (sp > 0) {
        String hexStr = cmd.substring(sp + 1);
        hexStr.trim();
        long v = strtol(hexStr.c_str(), nullptr, 16);
        if (v >= 0 && v <= 0x7F) {
          if (last_found_id == 0xFF) {
            Serial.println(F("現在見えてるIDが無い。先にGでスキャンしろ。"));
          } else {
            uint8_t new_id = (uint8_t)v;
            Serial.printf("SET_ID: 0x%02X -> 0x%02X ...\n", last_found_id, new_id);
            if (send_set_id(last_found_id, new_id)) {
              delay(50);
              // 変更直後にブロードキャスト応答が来る仕様。ついでに再スキャンで確定させる。
              scan_ids();
            } else {
              Serial.println(F("送信失敗"));
            }
          }
        } else {
          Serial.println(F("ID範囲外。0x00..0x7Fだけや。"));
        }
      } else {
        Serial.println(F("使い方: S <hex> 例) S 3B"));
      }
    } else {
      Serial.println(F("コマンド: G / S <hex>"));
    }
  }
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

// M5 ATOM (ESP32) + TWAI(CAN) 1Mbps

// RS05: 通信タイプ0でスキャン / 通信タイプ7でID書き換え（Arduino-ESP32 2.x対応）

// 配線: TX=GPIO25, RX=GPIO21

#include <Arduino.h>

#include "driver/twai.h"

#define CAN_TX_PIN GPIO_NUM_25

#define CAN_RX_PIN GPIO_NUM_21

static const twai_timing_config_t t_config = TWAI_TIMING_CONFIG_1MBITS();

static const twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();

static twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT(CAN_TX_PIN, CAN_RX_PIN, TWAI_MODE_NORMAL);

static inline uint32_t build_ext_id(uint8_t mode, uint16_t data16, uint8_t id8) {

return ( (uint32_t)(mode & 0x1F) << 24 ) | ( (uint32_t)data16 << 8 ) | (uint32_t)id8;

}

static const uint8_t COMM_GET_ID = 0x00; // モード0: GET_ID

static const uint8_t COMM_SET_ID = 0x07; // モード7: SET_ID

static const uint8_t RESP_MARKER = 0xFE; // 応答ID下位8bit

static const uint8_t MASTER_ID = 0xFD; // 任意の上位機ID（慣例値）

uint8_t last_found_id = 0xFF;

bool send_get_id(uint8_t target_can_id) {

twai_message_t tx = {};

tx.identifier = build_ext_id(COMM_GET_ID, ((uint16_t)MASTER_ID << 8) | 0x00, target_can_id);

tx.flags = TWAI_MSG_FLAG_EXTD | TWAI_MSG_FLAG_SS;

tx.data_length_code = 8;

memset(tx.data, 0, 8);

return twai_transmit(&tx, pdMS_TO_TICKS(5)) == ESP_OK;

}

bool send_set_id(uint8_t current_id, uint8_t new_id) {

// data16: [23..16]=new_id, [15..8]=MASTER_ID

uint16_t data16 = ((uint16_t)new_id << 8) | MASTER_ID;

twai_message_t tx = {};

tx.identifier = build_ext_id(COMM_SET_ID, data16, current_id);

tx.flags = TWAI_MSG_FLAG_EXTD | TWAI_MSG_FLAG_SS;

tx.data_length_code = 8;

memset(tx.data, 0, 8);

return twai_transmit(&tx, pdMS_TO_TICKS(5)) == ESP_OK;

}

void scan_ids(uint32_t wait_ms=1500) {

Serial.println(F("全IDにGET_ID(タイプ0)を送信..."));

for (uint16_t id = 0; id <= 127; ++id) {

send_get_id((uint8_t)id);

delayMicroseconds(200);

}

Serial.println(F("応答待ち..."));

uint32_t t0 = millis();

last_found_id = 0xFF;

while (millis() - t0 < wait_ms) {

twai_message_t rx = {};

if (twai_receive(&rx, pdMS_TO_TICKS(10)) != ESP_OK) continue;

if ((rx.flags & TWAI_MSG_FLAG_EXTD) == 0) continue;

if (rx.data_length_code != 8) continue;

uint32_t extid = rx.identifier;

uint8_t mode = (extid >> 24) & 0x1F;

uint16_t data16= (extid >> 8) & 0xFFFF;

uint8_t id8 = extid & 0xFF;

if (mode == 0x00 && id8 == RESP_MARKER) {

uint8_t motor_can_id = (uint8_t)(data16 & 0x00FF); // 応答中位16bitの下位8bit=CAN ID

last_found_id = motor_can_id;

Serial.printf("[RX] EXTID=0x%08lX mode=%u data16=0x%04X id8=0x%02X => CAN_ID=0x%02X\n",

extid, mode, data16, id8, motor_can_id);

}

if (last_found_id == 0xFF) Serial.println(F("応答なし"));

}

void setup() {

Serial.begin(115200);

delay(200);

Serial.println(F("\nRS05 IDツール: G=スキャン / S <hex>=ID変更"));

if (twai_driver_install(&g_config, &t_config, &f_config) != ESP_OK) { Serial.println(F("driver_install失敗")); while(1) delay(1000); }

if (twai_start() != ESP_OK) { Serial.println(F("twai_start失敗")); while(1) delay(1000); }

Serial.printf("CAN start OK (TX=%d RX=%d @1Mbps)\n", CAN_TX_PIN, CAN_RX_PIN);

// 起動時に一発スキャン

scan_ids();

}

void loop() {

// シリアルコマンド:

// G -> 再スキャン

// S xx -> 現在見えてるID(last_found_id)を 0xXX に変更

if (Serial.available()) {

String cmd = Serial.readStringUntil('\n');

cmd.trim();

if (cmd.length() == 0) return;

if (cmd.equalsIgnoreCase("G")) {

scan_ids();

} else if (cmd.length() >= 2 && (cmd[0] == 'S' || cmd[0] == 's')) {

// 例: "S 3B" で 0x3B に変更

int sp = cmd.indexOf(' ');

if (sp > 0) {

String hexStr = cmd.substring(sp + 1);

hexStr.trim();

long v = strtol(hexStr.c_str(), nullptr, 16);

if (v >= 0 && v <= 0x7F) {

if (last_found_id == 0xFF) {

Serial.println(F("現在見えてるIDが無い。先にGでスキャンしろ。"));

} else {

uint8_t new_id = (uint8_t)v;

Serial.printf("SET_ID: 0x%02X -> 0x%02X ...\n", last_found_id, new_id);

if (send_set_id(last_found_id, new_id)) {

delay(50);

// 変更直後にブロードキャスト応答が来る仕様。ついでに再スキャンで確定させる。

scan_ids();

} else {

Serial.println(F("送信失敗"));

}

} else {

Serial.println(F("ID範囲外。0x00..0x7Fだけや。"));

}

} else {

Serial.println(F("使い方: S <hex> 例) S 3B"));

}

} else {

Serial.println(F("コマンド: G / S <hex>"));

}

CAN通信はESP32のTWAI (Two-Wire Automotive Interface) ドライバを使用

CANボードの接続は以下で実施
　TX：IO 25
　RX：IO 21

シリアルコマンド “G” でID表示 (GET_ID = 0x00)
　デフォルトのモータIDは 0x7F (127) でした。

シリアルコマンド “S <hex>” でID変更 (SET_ID = 0x07)
　<hex>：0x00～0x7F

エンコーダ値

CAN IDを取得してROBSTRIDE 05 と会話ができるようになったので
エンコーダ値を取得してみます。

// M5 ATOM (ESP32) + TWAI(CAN) 1Mbps
// RS05: タイプ17(単一パラメータ読出し)で mechPos(0x7019)/mechVel(0x701B) を同時監視
// 出力: CSV形式 "pos_rad,vel_rad_per_s"
// 配線: TX=GPIO25, RX=GPIO21

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

#define CAN_TX_PIN GPIO_NUM_25
#define CAN_RX_PIN GPIO_NUM_21

// === CAN設定 ===
static const twai_timing_config_t t_config = TWAI_TIMING_CONFIG_1MBITS();
static const twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();
static twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT(CAN_TX_PIN, CAN_RX_PIN, TWAI_MODE_NORMAL);

// === RS05拡張IDの組み立て ===
static inline uint32_t build_ext_id(uint8_t mode, uint16_t data16, uint8_t id8) {
  return ( (uint32_t)(mode & 0x1F) << 24 ) | ( (uint32_t)data16 << 8 ) | (uint32_t)id8;
}

// === 定数 ===
static const uint8_t COMM_GET_ID    = 0x00; // タイプ0: ID取得
static const uint8_t COMM_READ_ONE  = 0x11; // タイプ17: 単一パラメータ読出し
static const uint8_t RESP_MARKER    = 0xFE;
static const uint8_t MASTER_ID      = 0xFD;

static const uint16_t IDX_mechPos   = 0x7019; // 機械角 [rad]
static const uint16_t IDX_mechVel   = 0x701B; // 回転速度 [rad/s]

// === 状態 ===
static uint8_t current_id = 0x7F;
static bool     polling = false;
static uint32_t poll_interval_ms = 20;
static uint32_t last_poll_ms = 0;

// === ユーティリティ ===
bool can_send(const twai_message_t &tx) {
  return twai_transmit((twai_message_t*)&tx, pdMS_TO_TICKS(5)) == ESP_OK;
}

bool send_get_id(uint8_t target_id){
  twai_message_t tx = {};
  tx.identifier = build_ext_id(COMM_GET_ID, ((uint16_t)MASTER_ID<<8)|0x00, target_id);
  tx.flags = TWAI_MSG_FLAG_EXTD | TWAI_MSG_FLAG_SS;
  tx.data_length_code = 8;
  memset(tx.data, 0, 8);
  return can_send(tx);
}

bool send_read_one(uint16_t index, uint8_t target_id){
  twai_message_t tx = {};
  tx.identifier = build_ext_id(COMM_READ_ONE, ((uint16_t)MASTER_ID<<8)|0x00, target_id);
  tx.flags = TWAI_MSG_FLAG_EXTD | TWAI_MSG_FLAG_SS;
  tx.data_length_code = 8;
  tx.data[0] = (uint8_t)(index & 0xFF);
  tx.data[1] = (uint8_t)(index >> 8);
  for(int i=2;i<8;++i) tx.data[i]=0;
  return can_send(tx);
}

bool recv_read_one(uint16_t expect_index, uint8_t *src_id, float *out_val){
  twai_message_t rx = {};
  if (twai_receive(&rx, pdMS_TO_TICKS(5)) != ESP_OK) return false;
  if ((rx.flags & TWAI_MSG_FLAG_EXTD) == 0) return false;
  if (rx.data_length_code != 8) return false;

  uint32_t extid = rx.identifier;
  uint8_t  mode  = (extid >> 24) & 0x1F;
  uint16_t data16= (extid >> 8)  & 0xFFFF;

  if (mode != COMM_READ_ONE) return false;

  uint16_t index_le = (uint16_t)rx.data[0] | ((uint16_t)rx.data[1]<<8);
  if (index_le != expect_index) return false;

  if (src_id) *src_id = (uint8_t)((data16 >> 8) & 0xFF);  // 上位8bitがモータID

  uint32_t u = ((uint32_t)rx.data[4]) |
               ((uint32_t)rx.data[5]<<8) |
               ((uint32_t)rx.data[6]<<16)|
               ((uint32_t)rx.data[7]<<24);
  float f;
  memcpy(&f, &u, sizeof(float));
  if (out_val) *out_val = f;
  return true;
}

void scan_ids(uint32_t wait_ms=1200){
  Serial.println(F("全IDにGET_ID送信..."));
  for (uint16_t id=0; id<=127; ++id){
    send_get_id((uint8_t)id);
    delayMicroseconds(200);
  }
  uint32_t t0 = millis();
  while (millis()-t0 < wait_ms){
    twai_message_t rx = {};
    if (twai_receive(&rx, pdMS_TO_TICKS(10)) != ESP_OK) continue;
    if ((rx.flags & TWAI_MSG_FLAG_EXTD)==0) continue;
    if (rx.data_length_code != 8) continue;

    uint32_t extid = rx.identifier;
    uint8_t  mode  = (extid >> 24) & 0x1F;
    uint16_t data16= (extid >> 8)  & 0xFFFF;
    uint8_t  id8   =  extid        & 0xFF;
    if (mode==0x00 && id8==RESP_MARKER){
      uint8_t id = (uint8_t)(data16 & 0xFF);
      current_id = id;
      Serial.printf("[FOUND] CAN_ID=0x%02X\n", id);
    }
  }
  Serial.printf("current_id=0x%02X\n", current_id);
}

void setup(){
  Serial.begin(115200);
  delay(200);
  Serial.println(F("\nRS05監視: G=スキャン / P <hz>=角度+速度 / S=停止"));
  if (twai_driver_install(&g_config, &t_config, &f_config) != ESP_OK){ Serial.println(F("driver_install失敗")); while(1) delay(1000); }
  if (twai_start() != ESP_OK){ Serial.println(F("twai_start失敗")); while(1) delay(1000); }
  Serial.printf("CAN start OK (TX=%d RX=%d @1Mbps), current_id=0x%02X\n", CAN_TX_PIN, CAN_RX_PIN, current_id);
}

void loop(){
  if (Serial.available()){
    String cmd = Serial.readStringUntil('\n'); cmd.trim();

    if (cmd.equalsIgnoreCase("G")){
      scan_ids();

    } else if (cmd.length()>=1 && (cmd[0]=='P' || cmd[0]=='p')){
      int hz = 50;
      int sp = cmd.indexOf(' ');
      if (sp > 0) {
        long hzL = cmd.substring(sp + 1).toInt();
        hz = (int)constrain(hzL, 1L, 500L);
      }
      poll_interval_ms = (uint32_t)round(1000.0 / hz);
      polling = true;
      last_poll_ms = 0;
      Serial.printf("mechPos+mechVel 監視開始: %d Hz (id=0x%02X)\n", hz, current_id);

    } else if (cmd.equalsIgnoreCase("S")){
      polling = false;
      Serial.println(F("監視停止"));
    }
  }

  if (polling){
    uint32_t now = millis();
    if (now - last_poll_ms >= poll_interval_ms){
      last_poll_ms = now;

      // 角度要求
      send_read_one(IDX_mechPos, current_id);
      uint8_t src_id=0; float pos=0.0f;
      if (!recv_read_one(IDX_mechPos, &src_id, &pos)) return;

      // 速度要求
      send_read_one(IDX_mechVel, current_id);
      float vel=0.0f;
      if (!recv_read_one(IDX_mechVel, nullptr, &vel)) return;

      // CSV形式で出力
      Serial.printf("%.6f,%.6f\n", pos, vel);
    }
  }
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

// M5 ATOM (ESP32) + TWAI(CAN) 1Mbps

// RS05: タイプ17(単一パラメータ読出し)で mechPos(0x7019)/mechVel(0x701B) を同時監視

// 出力: CSV形式 "pos_rad,vel_rad_per_s"

// 配線: TX=GPIO25, RX=GPIO21

#include <Arduino.h>

#include "driver/twai.h"

#define CAN_TX_PIN GPIO_NUM_25

#define CAN_RX_PIN GPIO_NUM_21

// === CAN設定 ===

static const twai_timing_config_t t_config = TWAI_TIMING_CONFIG_1MBITS();

static const twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();

static twai_general_config_t g_config = TWAI_GENERAL_CONFIG_DEFAULT(CAN_TX_PIN, CAN_RX_PIN, TWAI_MODE_NORMAL);

// === RS05拡張IDの組み立て ===

static inline uint32_t build_ext_id(uint8_t mode, uint16_t data16, uint8_t id8) {

return ( (uint32_t)(mode & 0x1F) << 24 ) | ( (uint32_t)data16 << 8 ) | (uint32_t)id8;

}

// === 定数 ===

static const uint8_t COMM_GET_ID = 0x00; // タイプ0: ID取得

static const uint8_t COMM_READ_ONE = 0x11; // タイプ17: 単一パラメータ読出し

static const uint8_t RESP_MARKER = 0xFE;

static const uint8_t MASTER_ID = 0xFD;

static const uint16_t IDX_mechPos = 0x7019; // 機械角 [rad]

static const uint16_t IDX_mechVel = 0x701B; // 回転速度 [rad/s]

// === 状態 ===

static uint8_t current_id = 0x7F;

static bool polling = false;

static uint32_t poll_interval_ms = 20;

static uint32_t last_poll_ms = 0;

// === ユーティリティ ===

bool can_send(const twai_message_t &tx) {

return twai_transmit((twai_message_t*)&tx, pdMS_TO_TICKS(5)) == ESP_OK;

}

bool send_get_id(uint8_t target_id){

twai_message_t tx = {};

tx.identifier = build_ext_id(COMM_GET_ID, ((uint16_t)MASTER_ID<<8)|0x00, target_id);

tx.flags = TWAI_MSG_FLAG_EXTD | TWAI_MSG_FLAG_SS;

tx.data_length_code = 8;

memset(tx.data, 0, 8);

return can_send(tx);

}

bool send_read_one(uint16_t index, uint8_t target_id){

twai_message_t tx = {};

tx.identifier = build_ext_id(COMM_READ_ONE, ((uint16_t)MASTER_ID<<8)|0x00, target_id);

tx.flags = TWAI_MSG_FLAG_EXTD | TWAI_MSG_FLAG_SS;

tx.data_length_code = 8;

tx.data[0] = (uint8_t)(index & 0xFF);

tx.data[1] = (uint8_t)(index >> 8);

for(int i=2;i<8;++i) tx.data[i]=0;

return can_send(tx);

}

bool recv_read_one(uint16_t expect_index, uint8_t *src_id, float *out_val){

twai_message_t rx = {};

if (twai_receive(&rx, pdMS_TO_TICKS(5)) != ESP_OK) return false;

if ((rx.flags & TWAI_MSG_FLAG_EXTD) == 0) return false;

if (rx.data_length_code != 8) return false;

uint32_t extid = rx.identifier;

uint8_t mode = (extid >> 24) & 0x1F;

uint16_t data16= (extid >> 8) & 0xFFFF;

if (mode != COMM_READ_ONE) return false;

uint16_t index_le = (uint16_t)rx.data[0] | ((uint16_t)rx.data[1]<<8);

if (index_le != expect_index) return false;

if (src_id) *src_id = (uint8_t)((data16 >> 8) & 0xFF); // 上位8bitがモータID

uint32_t u = ((uint32_t)rx.data[4]) |

((uint32_t)rx.data[5]<<8) |

((uint32_t)rx.data[6]<<16)|

((uint32_t)rx.data[7]<<24);

float f;

memcpy(&f, &u, sizeof(float));

if (out_val) *out_val = f;

return true;

}

void scan_ids(uint32_t wait_ms=1200){

Serial.println(F("全IDにGET_ID送信..."));

for (uint16_t id=0; id<=127; ++id){

send_get_id((uint8_t)id);

delayMicroseconds(200);

}

uint32_t t0 = millis();

while (millis()-t0 < wait_ms){

twai_message_t rx = {};

if (twai_receive(&rx, pdMS_TO_TICKS(10)) != ESP_OK) continue;

if ((rx.flags & TWAI_MSG_FLAG_EXTD)==0) continue;

if (rx.data_length_code != 8) continue;

uint32_t extid = rx.identifier;

uint8_t mode = (extid >> 24) & 0x1F;

uint16_t data16= (extid >> 8) & 0xFFFF;

uint8_t id8 = extid & 0xFF;

if (mode==0x00 && id8==RESP_MARKER){

uint8_t id = (uint8_t)(data16 & 0xFF);

current_id = id;

Serial.printf("[FOUND] CAN_ID=0x%02X\n", id);

}

Serial.printf("current_id=0x%02X\n", current_id);

}

void setup(){

Serial.begin(115200);

delay(200);

Serial.println(F("\nRS05監視: G=スキャン / P <hz>=角度+速度 / S=停止"));

if (twai_driver_install(&g_config, &t_config, &f_config) != ESP_OK){ Serial.println(F("driver_install失敗")); while(1) delay(1000); }

if (twai_start() != ESP_OK){ Serial.println(F("twai_start失敗")); while(1) delay(1000); }

Serial.printf("CAN start OK (TX=%d RX=%d @1Mbps), current_id=0x%02X\n", CAN_TX_PIN, CAN_RX_PIN, current_id);

}

void loop(){

if (Serial.available()){

String cmd = Serial.readStringUntil('\n'); cmd.trim();

if (cmd.equalsIgnoreCase("G")){

scan_ids();

} else if (cmd.length()>=1 && (cmd[0]=='P' || cmd[0]=='p')){

int hz = 50;

int sp = cmd.indexOf(' ');

if (sp > 0) {

long hzL = cmd.substring(sp + 1).toInt();

hz = (int)constrain(hzL, 1L, 500L);

}

poll_interval_ms = (uint32_t)round(1000.0 / hz);

polling = true;

last_poll_ms = 0;

Serial.printf("mechPos+mechVel 監視開始: %d Hz (id=0x%02X)\n", hz, current_id);

} else if (cmd.equalsIgnoreCase("S")){

polling = false;

Serial.println(F("監視停止"));

}

if (polling){

uint32_t now = millis();

if (now - last_poll_ms >= poll_interval_ms){

last_poll_ms = now;

// 角度要求

send_read_one(IDX_mechPos, current_id);

uint8_t src_id=0; float pos=0.0f;

if (!recv_read_one(IDX_mechPos, &src_id, &pos)) return;

// 速度要求

send_read_one(IDX_mechVel, current_id);

float vel=0.0f;

if (!recv_read_one(IDX_mechVel, nullptr, &vel)) return;

// CSV形式で出力

Serial.printf("%.6f,%.6f\n", pos, vel);

}

シリアルコマンド “G” でID表示 (GET_ID = 0x00)
　デフォルトは 0x7F (127)

シリアルコマンド “P <hz>” でモータ機械角 [rad]、回転速度 [rad/s] 表示
　<hz>：1〜500Hz デフォルト 50Hz

シリアルコマンド “S” でエンコーダ表示停止

動作

コミュ障解消
ROBSTRIDE05と会話できるようになった
ひとまずエンコーダ値を伺う pic.twitter.com/gM4IgEIk98

— HomeMadeGarbage (@H0meMadeGarbage) September 10, 2025

回転角制御

ついに回転位置を指定してROBSTRIDE 05 を回します。

// M5 ATOM (ESP32) + TWAI(CAN) 1Mbps
// RS05: 角度指令(CSP)ツール - シリアルは G / L <rad_s> / A <deg> のみ
// ・G           : 接続中モータのCAN IDスキャン（current_idを更新）
// ・L <rad_s>   : CSPの速度上限 [rad/s] を設定（保持）
// ・A <deg>     : 目標角度 [deg] 指定（内部でCSPを有効化→上限適用→loc_ref書き込み）
// 出力: 実行ログのみ（必要なら別の監視ツールでpos/velを監視）
// 配線: TX=GPIO32, RX=GPIO26（この定義に合わせること）

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

#define CAN_TX_PIN GPIO_NUM_25
#define CAN_RX_PIN GPIO_NUM_21

// ==== CAN設定 ====
static const twai_timing_config_t t_config = TWAI_TIMING_CONFIG_1MBITS();
static const twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();
// ★ グローバルはマクロそのまま
static twai_general_config_t g_config =
    TWAI_GENERAL_CONFIG_DEFAULT(CAN_TX_PIN, CAN_RX_PIN, TWAI_MODE_NORMAL);

// ==== 拡張ID(29bit)レイアウト ====
// [28:24]=モード(5bit), [23:8]=data16, [7:0]=id8
static inline uint32_t build_ext_id(uint8_t mode, uint16_t data16, uint8_t id8) {
  return ( (uint32_t)(mode & 0x1F) << 24 ) | ( (uint32_t)data16 << 8 ) | (uint32_t)id8;
}

// ==== モード / インデックス ====
static const uint8_t COMM_GET_ID    = 0x00; // モード0: ID取得
static const uint8_t COMM_RUN_EN    = 0x03; // モード3: 有効化（内部のみで使用）
static const uint8_t COMM_WRITE_ONE = 0x12; // モード18: 単一書込み
static const uint8_t RESP_MARKER    = 0xFE;
static const uint8_t MASTER_ID      = 0xFD;

static const uint16_t IDX_runmode   = 0x7005; // 0:運控 1:PP 2:速度 3:電流 5:CSP
static const uint16_t IDX_loc_ref   = 0x7016; // 位置指令 [rad] (float)
static const uint16_t IDX_limit_spd = 0x7017; // 速度上限 [rad/s] (float, CSP用)

// ==== 状態 ====
static uint8_t current_id = 0x7F;   // スキャンで上書き
static float   limit_spd  = 10.0f;  // 既定のCSP速度上限[rad/s]

// ==== ユーティリティ ====
static inline bool can_send(const twai_message_t &tx) {
  return twai_transmit((twai_message_t*)&tx, pdMS_TO_TICKS(10)) == ESP_OK;
}

static inline void float_to_le(float v, uint8_t* b) {
  uint32_t u; memcpy(&u, &v, 4);
  b[0]=(uint8_t)(u      ); b[1]=(uint8_t)(u>>8);
  b[2]=(uint8_t)(u>>16 ); b[3]=(uint8_t)(u>>24);
}

// ---- 内部送信ヘルパ ----
static bool send_enable(uint8_t id){ // 「有効化」送信
  twai_message_t tx = {};
  tx.identifier = build_ext_id(COMM_RUN_EN, ((uint16_t)MASTER_ID<<8)|0x00, id);
  tx.flags = TWAI_MSG_FLAG_EXTD;
  tx.data_length_code = 8;
  memset(tx.data, 0, 8);
  return can_send(tx);
}

static bool write_param_f(uint16_t index, float val, uint8_t id){
  twai_message_t tx = {};
  tx.identifier = build_ext_id(COMM_WRITE_ONE, ((uint16_t)MASTER_ID<<8)|0x00, id);
  tx.flags = TWAI_MSG_FLAG_EXTD;
  tx.data_length_code = 8;
  tx.data[0] = (uint8_t)(index & 0xFF);
  tx.data[1] = (uint8_t)(index >> 8);
  tx.data[2] = 0; tx.data[3] = 0;
  float_to_le(val, &tx.data[4]);
  return can_send(tx);
}

static bool write_param_u8(uint16_t index, uint8_t val, uint8_t id){
  twai_message_t tx = {};
  tx.identifier = build_ext_id(COMM_WRITE_ONE, ((uint16_t)MASTER_ID<<8)|0x00, id);
  tx.flags = TWAI_MSG_FLAG_EXTD;
  tx.data_length_code = 8;
  tx.data[0] = (uint8_t)(index & 0xFF);
  tx.data[1] = (uint8_t)(index >> 8);
  tx.data[2] = 0; tx.data[3] = 0;
  tx.data[4] = val; tx.data[5] = 0; tx.data[6] = 0; tx.data[7] = 0;
  return can_send(tx);
}

// ---- IDスキャン（G） ----
static void scan_ids(uint32_t wait_ms=800){
  Serial.println(F("全IDへGET_ID..."));
  for (uint16_t id=0; id<=127; ++id){
    twai_message_t tx = {};
    tx.identifier = build_ext_id(COMM_GET_ID, ((uint16_t)MASTER_ID<<8)|0x00, (uint8_t)id);
    tx.flags = TWAI_MSG_FLAG_EXTD;
    tx.data_length_code = 8;
    memset(tx.data, 0, 8);
    can_send(tx);
    delayMicroseconds(700);
  }
  uint32_t t0 = millis();
  while (millis()-t0 < wait_ms){
    twai_message_t rx = {};
    if (twai_receive(&rx, pdMS_TO_TICKS(10)) != ESP_OK) continue;
    if ((rx.flags & TWAI_MSG_FLAG_EXTD)==0) continue;
    if (rx.data_length_code != 8) continue;
    uint32_t extid = rx.identifier;
    uint8_t  mode  = (extid >> 24) & 0x1F;
    uint16_t data16= (extid >> 8)  & 0xFFFF;
    uint8_t  id8   =  extid        & 0xFF;
    if (mode==COMM_GET_ID && id8==RESP_MARKER){
      uint8_t id = (uint8_t)(data16 & 0xFF);
      current_id = id;
      Serial.printf("[FOUND] CAN_ID=0x%02X\n", id);
    }
  }
  Serial.printf("current_id=0x%02X\n", current_id);
}

// ---- 角度指令（A） ----
static bool csp_move_deg(float deg){
  float rad = deg * PI / 180.0f;
  bool ok = true;
  ok &= write_param_u8(IDX_runmode, 5, current_id);          // CSP(5)
  ok &= write_param_f (IDX_limit_spd, limit_spd, current_id);// 上限適用
  delay(5);
  ok &= send_enable(current_id);                              // 有効化
  delay(5);
  ok &= write_param_f (IDX_loc_ref, rad, current_id);        // 目標角
  Serial.printf("A: %.3f deg (%.6f rad) / limit=%.3f rad/s -> %s\n",
                deg, rad, limit_spd, ok ? "OK" : "NG");
  return ok;
}

// ==== セットアップ ====
void setup(){
  Serial.begin(115200);
  delay(200);
  Serial.println(F("\nRS05 角度指令(CSP): G=スキャン / L <rad_s>=上限設定 / A <deg>=角度指令"));

  // ★ ここでキュー長を上書き
  g_config.tx_queue_len = 10;
  g_config.rx_queue_len = 20;

  if (twai_driver_install(&g_config, &t_config, &f_config) != ESP_OK){
    Serial.println(F("driver_install失敗")); while(1) delay(1000);
  }
  if (twai_start() != ESP_OK){
    Serial.println(F("twai_start失敗")); while(1) delay(1000);
  }
  Serial.printf("CAN start OK (TX=%d RX=%d @1Mbps), current_id=0x%02X\n",
                CAN_TX_PIN, CAN_RX_PIN, current_id);
}

// ==== メイン ====
void loop(){
  if (!Serial.available()) return;
  String cmd = Serial.readStringUntil('\n'); cmd.trim();
  if (cmd.length()==0) return;

  if (cmd.equalsIgnoreCase("G")){
    scan_ids();
    return;
  }

  if (cmd.length()>=1 && (cmd[0]=='L' || cmd[0]=='l')){
    int sp = cmd.indexOf(' ');
    if (sp>0){
      float v = cmd.substring(sp+1).toFloat();
      if (v < 0) v = 0;
      bool ok = write_param_f(IDX_limit_spd, v, current_id);
      if (ok) limit_spd = v;
      Serial.printf("CSP速度上限: %.3f rad/s -> %s\n", v, ok ? "OK" : "NG");
    } else {
      Serial.printf("CSP速度上限(現在): %.3f rad/s\n", limit_spd);
    }
    return;
  }

  if (cmd.length()>=1 && (cmd[0]=='A' || cmd[0]=='a')){
    int sp = cmd.indexOf(' ');
    if (sp <= 0){ Serial.println(F("使い方: A <deg>")); return; }
    float deg = cmd.substring(sp+1).toFloat();
    (void)csp_move_deg(deg);
    return;
  }

  Serial.println(F("コマンド: G / L <rad_s> / A <deg>"));
}

100

101

102

103

104

105

106

107

108

109

110

111

112

113

114

115

116

117

118

119

120

121

122

123

124

125

126

127

128

129

130

131

132

133

134

135

136

137

138

139

140

141

142

143

144

145

146

147

148

149

150

151

152

153

154

155

156

157

158

159

160

161

162

163

164

165

166

167

168

169

170

171

172

173

174

175

176

177

178

179

180

181

182

183

184

185

186

187

188

// M5 ATOM (ESP32) + TWAI(CAN) 1Mbps

// RS05: 角度指令(CSP)ツール - シリアルは G / L <rad_s> / A <deg> のみ

// ・G : 接続中モータのCAN IDスキャン（current_idを更新）

// ・L <rad_s> : CSPの速度上限 [rad/s] を設定（保持）

// ・A <deg> : 目標角度 [deg] 指定（内部でCSPを有効化→上限適用→loc_ref書き込み）

// 出力: 実行ログのみ（必要なら別の監視ツールでpos/velを監視）

// 配線: TX=GPIO32, RX=GPIO26（この定義に合わせること）

#include <Arduino.h>

#include "driver/twai.h"

#define CAN_TX_PIN GPIO_NUM_25

#define CAN_RX_PIN GPIO_NUM_21

// ==== CAN設定 ====

static const twai_timing_config_t t_config = TWAI_TIMING_CONFIG_1MBITS();

static const twai_filter_config_t f_config = TWAI_FILTER_CONFIG_ACCEPT_ALL();

// ★ グローバルはマクロそのまま

static twai_general_config_t g_config =

TWAI_GENERAL_CONFIG_DEFAULT(CAN_TX_PIN, CAN_RX_PIN, TWAI_MODE_NORMAL);

// ==== 拡張ID(29bit)レイアウト ====

// [28:24]=モード(5bit), [23:8]=data16, [7:0]=id8

static inline uint32_t build_ext_id(uint8_t mode, uint16_t data16, uint8_t id8) {

return ( (uint32_t)(mode & 0x1F) << 24 ) | ( (uint32_t)data16 << 8 ) | (uint32_t)id8;

}

// ==== モード / インデックス ====

static const uint8_t COMM_GET_ID = 0x00; // モード0: ID取得

static const uint8_t COMM_RUN_EN = 0x03; // モード3: 有効化（内部のみで使用）

static const uint8_t COMM_WRITE_ONE = 0x12; // モード18: 単一書込み

static const uint8_t RESP_MARKER = 0xFE;

static const uint8_t MASTER_ID = 0xFD;

static const uint16_t IDX_runmode = 0x7005; // 0:運控 1:PP 2:速度 3:電流 5:CSP

static const uint16_t IDX_loc_ref = 0x7016; // 位置指令 [rad] (float)

static const uint16_t IDX_limit_spd = 0x7017; // 速度上限 [rad/s] (float, CSP用)

// ==== 状態 ====

static uint8_t current_id = 0x7F; // スキャンで上書き

static float limit_spd = 10.0f; // 既定のCSP速度上限[rad/s]

// ==== ユーティリティ ====

static inline bool can_send(const twai_message_t &tx) {

return twai_transmit((twai_message_t*)&tx, pdMS_TO_TICKS(10)) == ESP_OK;

}

static inline void float_to_le(float v, uint8_t* b) {

uint32_t u; memcpy(&u, &v, 4);

b[0]=(uint8_t)(u ); b[1]=(uint8_t)(u>>8);

b[2]=(uint8_t)(u>>16 ); b[3]=(uint8_t)(u>>24);

}

// ---- 内部送信ヘルパ ----

static bool send_enable(uint8_t id){ // 「有効化」送信

twai_message_t tx = {};

tx.identifier = build_ext_id(COMM_RUN_EN, ((uint16_t)MASTER_ID<<8)|0x00, id);

tx.flags = TWAI_MSG_FLAG_EXTD;

tx.data_length_code = 8;

memset(tx.data, 0, 8);

return can_send(tx);

}

static bool write_param_f(uint16_t index, float val, uint8_t id){

twai_message_t tx = {};

tx.identifier = build_ext_id(COMM_WRITE_ONE, ((uint16_t)MASTER_ID<<8)|0x00, id);

tx.flags = TWAI_MSG_FLAG_EXTD;

tx.data_length_code = 8;

tx.data[0] = (uint8_t)(index & 0xFF);

tx.data[1] = (uint8_t)(index >> 8);

tx.data[2] = 0; tx.data[3] = 0;

float_to_le(val, &tx.data[4]);

return can_send(tx);

}

static bool write_param_u8(uint16_t index, uint8_t val, uint8_t id){

twai_message_t tx = {};

tx.identifier = build_ext_id(COMM_WRITE_ONE, ((uint16_t)MASTER_ID<<8)|0x00, id);

tx.flags = TWAI_MSG_FLAG_EXTD;

tx.data_length_code = 8;

tx.data[0] = (uint8_t)(index & 0xFF);

tx.data[1] = (uint8_t)(index >> 8);

tx.data[2] = 0; tx.data[3] = 0;

tx.data[4] = val; tx.data[5] = 0; tx.data[6] = 0; tx.data[7] = 0;

return can_send(tx);

}

// ---- IDスキャン（G） ----

static void scan_ids(uint32_t wait_ms=800){

Serial.println(F("全IDへGET_ID..."));

for (uint16_t id=0; id<=127; ++id){

twai_message_t tx = {};

tx.identifier = build_ext_id(COMM_GET_ID, ((uint16_t)MASTER_ID<<8)|0x00, (uint8_t)id);

tx.flags = TWAI_MSG_FLAG_EXTD;

tx.data_length_code = 8;

memset(tx.data, 0, 8);

can_send(tx);

delayMicroseconds(700);

}

uint32_t t0 = millis();

while (millis()-t0 < wait_ms){

twai_message_t rx = {};

if (twai_receive(&rx, pdMS_TO_TICKS(10)) != ESP_OK) continue;

if ((rx.flags & TWAI_MSG_FLAG_EXTD)==0) continue;

if (rx.data_length_code != 8) continue;

uint32_t extid = rx.identifier;

uint8_t mode = (extid >> 24) & 0x1F;

uint16_t data16= (extid >> 8) & 0xFFFF;

uint8_t id8 = extid & 0xFF;

if (mode==COMM_GET_ID && id8==RESP_MARKER){

uint8_t id = (uint8_t)(data16 & 0xFF);

current_id = id;

Serial.printf("[FOUND] CAN_ID=0x%02X\n", id);

}

Serial.printf("current_id=0x%02X\n", current_id);

}

// ---- 角度指令（A） ----

static bool csp_move_deg(float deg){

float rad = deg * PI / 180.0f;

bool ok = true;

ok &= write_param_u8(IDX_runmode, 5, current_id); // CSP(5)

ok &= write_param_f (IDX_limit_spd, limit_spd, current_id);// 上限適用

delay(5);

ok &= send_enable(current_id); // 有効化

delay(5);

ok &= write_param_f (IDX_loc_ref, rad, current_id); // 目標角

Serial.printf("A: %.3f deg (%.6f rad) / limit=%.3f rad/s -> %s\n",

deg, rad, limit_spd, ok ? "OK" : "NG");

return ok;

}

// ==== セットアップ ====

void setup(){

Serial.begin(115200);

delay(200);

Serial.println(F("\nRS05 角度指令(CSP): G=スキャン / L <rad_s>=上限設定 / A <deg>=角度指令"));

// ★ ここでキュー長を上書き

g_config.tx_queue_len = 10;

g_config.rx_queue_len = 20;

if (twai_driver_install(&g_config, &t_config, &f_config) != ESP_OK){

Serial.println(F("driver_install失敗")); while(1) delay(1000);

}

if (twai_start() != ESP_OK){

Serial.println(F("twai_start失敗")); while(1) delay(1000);

}

Serial.printf("CAN start OK (TX=%d RX=%d @1Mbps), current_id=0x%02X\n",

CAN_TX_PIN, CAN_RX_PIN, current_id);

}

// ==== メイン ====

void loop(){

if (!Serial.available()) return;

String cmd = Serial.readStringUntil('\n'); cmd.trim();

if (cmd.length()==0) return;

if (cmd.equalsIgnoreCase("G")){

scan_ids();

return;

}

if (cmd.length()>=1 && (cmd[0]=='L' || cmd[0]=='l')){

int sp = cmd.indexOf(' ');

if (sp>0){

float v = cmd.substring(sp+1).toFloat();

if (v < 0) v = 0;

bool ok = write_param_f(IDX_limit_spd, v, current_id);

if (ok) limit_spd = v;

Serial.printf("CSP速度上限: %.3f rad/s -> %s\n", v, ok ? "OK" : "NG");

} else {

Serial.printf("CSP速度上限(現在): %.3f rad/s\n", limit_spd);

}

return;

}

if (cmd.length()>=1 && (cmd[0]=='A' || cmd[0]=='a')){

int sp = cmd.indexOf(' ');

if (sp <= 0){ Serial.println(F("使い方: A <deg>")); return; }

float deg = cmd.substring(sp+1).toFloat();

(void)csp_move_deg(deg);

return;

}

Serial.println(F("コマンド: G / L <rad_s> / A <deg>"));

}

シリアルコマンド “G” でID表示 (GET_ID = 0x00)
　デフォルトは 0x7F (127)

シリアルコマンド “L <rad_s>” で回転速度 [rad/s] 上限値設定

シリアルコマンド “A <deg>” 回転位置指定

動作

ROBSTRIDE05　角度指定

初めてのガチQDDモータ
あらいいですねぇと言ってよいでしょう pic.twitter.com/W9CZTsB4rP

— HomeMadeGarbage (@H0meMadeGarbage) September 10, 2025

姿勢制御モジュール

ROBSTRIDE 05と私との距離がおもくそ縮まったのでサクッと姿勢制御モジュールにしてみました。

ATOM Matrixを用いて内蔵IMUの姿勢情報やモータ回転速度を検知して倒立してます。

QDD (準ダイレクトドライブ)とはよく言ったもので、トルクはもちろんのこと応答も素晴らしいです。

これができてしまえば ROBSTRIDE 05と私はすでに一心同体であると言えます。

おわりに

ここではガチロボットQDDモータ ROBSTRIDE 05 を入手し動作を楽しみました。

動かし方はおおむね理解できましたので、さらに制御ループのパラメータ調整などの細かいところも確認していきたいと考えています。

それではまた

コメントはこちらからコメントをキャンセル

このサイトはスパムを低減するために Akismet を使っています。コメントデータの処理方法の詳細はこちらをご覧ください。

ROBSTRIDE 05

おいじり

CAN通信

CAN ID

エンコーダ値

動作

回転角制御

動作

姿勢制御モジュール

おわりに

関連記事

コメントはこちらから コメントをキャンセル

コメントはこちらからコメントをキャンセル