コメント無し

## はじめに 今回は、SPRESENSEと今話題のQDD BLDCモーターを使い、モーター越しに「相手の力を感じる」バイラテラル制御を実装しました！

## ROBSTRIDEモーター  QDD（準ダイレクトドライブモーター）がロボット界隈では今流行っています 四足歩行からヒューマノイドまで、最近話題のロボットにはこのQDDがよく使われています。 今回はそんなQDDの中から、ROBSTRIDE 02を使用しました。 こちらのモーターは比較的安価にも関わらず、最大17Nmと非常に大きなトルクを発生でき、エンコーダや電流センサなども内蔵していることが特徴です。 ROBSRIDEはCAN通信によってマイコンと通信を行いますが、今回SPRESENSEとはMCP2515という変換基板を通して繋げて通信を行いました。   ### 部品 ・MCP2515 ・SPRESENSE ・24V電源 ・ROBSTRIDE 02 * 2台 ### 配線  ROBSTRIDEとの通信はデータシートに記載されており、今回は私たちの作成したライブラリを使って通信を行なっています。ライブラリは下記で公開しています。 https://github.com/Beavers-Hive/ROBSTRIDE_Library ## 制御方法 今回、バイラテラル制御を行う上で、モーターは電流制御を使用しています。 CAN通信を介して高頻度で位置・電流（トルク）を取得して、計算された電流値を送り、微細な反力を生成。これにより、人間がモーターを回した時の「抵抗感」を提示できるようにしています。 PD制御による仮想物理モデル2つのモーター間に、仮想的な「バネ」と「ダンパー」を設置します。 モーターを「センサー」兼「アクチュエータ」として使う今回使用したRS02モーターは、CAN通信を介して高頻度で状態を取得・指令できます。特筆すべきは、位置決めモードを使わず、電流制御（トルク制御）モードで動かしている点です。位置の取得： 磁気エンコーダにより $0.01^{\circ}$ 単位のズレを検知。力の出力： 計算された電流値を送り、微細な反力を生成。これにより、人間がモーターを回した時の「抵抗感」を自由自在にプログラムでデザインできるようになります。 PD制御による仮想物理モデル2つのモーター間に、仮想的な「バネ」と「ダンパー」を設置します。  ## 実装のポイント CANバスの高速性バイラテラル制御において、遅延（レイテンシ）は最大の敵です 遅延が発生すると、バネの反応が遅れ、システムが発散（暴走）してしまいます。 今回は、1Mbpsの高速CAN通信を使用し、高性能なSPRESENSEの高速な制御ループを回すことで、違和感のない「ダイレクトな手応え」を実現しました！ ## 遠隔腕相撲 この制御を使用して、こちらの遠隔腕相撲を作りました！ @[youtube](https://www.youtube.com/watch?v=0cKDDTf5M3E) ## ソースコード ```SPRESENSE #include <SPI.h> #include <mcp_can.h> #include "RS02PrivateCAN.h" // ===== Configuration ===== #define CAN_CS_PIN 24 #define CAN_BAUD CAN_1000KBPS #define MCP_CLOCK MCP_16MHZ constexpr uint8_t HOST_ID = 0x00; constexpr uint8_t ID_LEADER = 0x7E; constexpr uint8_t ID_FOLLOWER = 0x7F; // Control Gains (Virtual Spring-Damper) // P Gain: stiffness // D Gain: damping (stability) // Output is Current (Amps) float KP = 3.0f; float KD = 0.1f; float MAX_CURRENT = 6.0f; // Safety limit (Amps) // ===== Globals ===== MCP_CAN CAN(CAN_CS_PIN); RS02PrivateCAN RS(CAN, HOST_ID); // Angle Tracking for Multi-turn struct MotorState { uint8_t id; bool hasInit; float prevRaw; double multiTurnRad; // Unwrapped angle float velocity; MotorState(uint8_t _id) : id(_id), hasInit(false), prevRaw(0.0f), multiTurnRad(0.0f), velocity(0.0f) {} }; MotorState mLeader(ID_LEADER); MotorState mFollower(ID_FOLLOWER); // ===== Functions ===== // Helper to write uint8 static bool writeU8(uint8_t node, uint16_t idx, uint8_t v) { uint8_t le[4] = {v, 0, 0, 0}; return RS.writeParamLE(node, idx, le); } // Helper to read float static bool readF32(uint8_t node, uint16_t idx, float &out) { uint8_t le[4] = {0}; if (!RS.readParamRaw(node, idx, le)) return false; memcpy(&out, le, 4); return true; } // GUI Helper void logStatus(const char* label, bool success) { spr.setTextColor(success ? GREEN : RED, BLACK); spr.printf("%-12s : %s\n", label, success ? "OK" : "FAIL"); spr.pushSprite(0, 0); delay(50); } // Update unwrapped angle void updateState(MotorState &m) { float rawPos = 0.0f; float rawVel = 0.0f; bool okPos = readF32(m.id, RS02Idx::MECH_POS, rawPos); bool okVel = readF32(m.id, RS02Idx::MECH_VEL, rawVel); // Speed for D-term if (!okPos) return; // Failed to read if (!m.hasInit) { m.hasInit = true; m.prevRaw = rawPos; m.multiTurnRad = 0.0; // Reset origin to 0 at startup } else { float diff = rawPos - m.prevRaw; // Wrap handling (-PI to PI usually, or 0 to 2PI) // Assuming 0-2PI or similar, standard shortest path unwrap using PI threshold if (diff > M_PI) diff -= 2 * M_PI; else if (diff < -M_PI) diff += 2 * M_PI; m.multiTurnRad += diff; m.prevRaw = rawPos; } if (okVel) m.velocity = rawVel; } void setupMotor(uint8_t id) { bool ok = RS.stop(id, true); ok &= writeU8(id, RS02Idx::RUN_MODE, 3); // Mode 3: Current Control delay(20); ok &= RS.writeFloatParam(id, RS02Idx::LIMIT_CUR, 6.0f); // Safety limit on motor side ok &= RS.enable(id); logStatus(" State", ok); } void setup() { SPI.begin(); if (CAN.begin(MCP_ANY, CAN_BAUD, MCP_CLOCK) != CAN_OK) { Serial.println("CAN Init FAIL"); while (1) delay(1000); } CAN.setMode(MCP_NORMAL); RS.begin(); RS.setMasterId(0xFD); // Setup Motors setupMotor(ID_LEADER); setupMotor(ID_FOLLOWER); Serial.println("Ready. Loop Start."); delay(1000); } unsigned long lastDraw = 0; void loop() { // 1. Read States updateState(mLeader); updateState(mFollower); // 2. Control Law (Spring Coupling - REVERSE) // Target: LeaderAngle + FollowerAngle = 0 (Opposite directions) // We want to force (PosA + PosB) -> 0 float posSum = mLeader.multiTurnRad + mFollower.multiTurnRad; float velSum = mLeader.velocity + mFollower.velocity; // Damping on the sum // Force = -Stiffness * Displacement float torqueCmd = -(KP * posSum) - (KD * velSum); // Clamp if (torqueCmd > MAX_CURRENT) torqueCmd = MAX_CURRENT; if (torqueCmd < -MAX_CURRENT) torqueCmd = -MAX_CURRENT; // 3. Write Commands // Apply same torque to both to push them toward A+B=0 // e.g. If A is +1, B is 0 -> Sum +1 -> Torque -ve. // A feels resistance (-), B moves negative (-). Correct. RS.currentIqRef(ID_FOLLOWER, torqueCmd); RS.currentIqRef(ID_LEADER, torqueCmd); // 4. GUI Update (Low priority) if (millis() - lastDraw > 100) { lastDraw = millis(); } } ```