QDDをSPRESENSEでバイラテラル制御して腕相撲してみた！

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

ここに動画が表示されます

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の高速な制御ループを回すことで、違和感のない「ダイレクトな手応え」を実現しました！

この制御を使用して、こちらの遠隔腕相撲を作りました！

ここに動画が表示されます


#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();
    }
}

こちら、理論上遠隔地でも腕相撲ができちゃいそうです！
トルクやモーターを変えて非対称腕相撲とかにもしてみたいです！