From f1303d46a04f40a66a573f125ce4a801e506f0af Mon Sep 17 00:00:00 2001 From: DoveXY Date: Sun, 12 Apr 2026 12:21:39 +0800 Subject: [PATCH 1/5] =?UTF-8?q?=E5=88=9D=E6=AC=A1=E6=8F=90=E4=BA=A4j60?= =?UTF-8?q?=E7=94=B5=E6=9C=BA=E9=80=82=E9=85=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- GSRL/Device/inc/dvc_motor.hpp | 98 +++++++- GSRL/Device/src/dvc_motor.cpp | 293 ++++++++++++++++++++++ GSRL/Documentation/Device/MotorDeepJ60.md | 134 ++++++++++ 3 files changed, 524 insertions(+), 1 deletion(-) create mode 100644 GSRL/Documentation/Device/MotorDeepJ60.md diff --git a/GSRL/Device/inc/dvc_motor.hpp b/GSRL/Device/inc/dvc_motor.hpp index 213fcd9..0fecaf1 100644 --- a/GSRL/Device/inc/dvc_motor.hpp +++ b/GSRL/Device/inc/dvc_motor.hpp @@ -103,7 +103,7 @@ class Motor Motor(uint32_t canControlID, uint32_t canFeedbackID, Controller *controller, uint16_t encoderOffset = 0); virtual bool decodeCanRxMessage(const can_rx_message_t &rxMessage) = 0; virtual void convertControllerOutputToMotorControlData() = 0; - fp32 updateCurrentRevolutions(); + virtual fp32 updateCurrentRevolutions(); inline void increaseMotorFeedbackErrorCount(); inline void clearMotorFeedbackErrorCount(); }; @@ -261,6 +261,102 @@ class MotorLKMG : public Motor void convertControllerOutputToMotorControlData() override; }; +/** + * @brief 云深处 DeepRobotics J60 关节电机类 + * @note CAN 波特率 1Mbps，标准帧。使用前请用 J60 调试工具标定零位并设置关节 ID (默认为 1)。 + * @note 构造后，首次 convertControllerOutputToMotorControlData() 会发送 ENABLE 命令， + * 收到驱动器应答后进入控制模式；基类检测到掉线后会自动重发 ENABLE 重连。 + * @note 默认"板外 PID"模式 (Kp=Kd=0)，扭矩目标由 m_controllerOutput 提供 (单位 Nm， + * 需与 GSRLMath 框架里的 PID 配合)，可直接使用基类 torqueCurrentClosedloopControl、 + * externalClosedloopControl、angleClosedloopControl、revolutionsClosedloopControl 等接口。 + * @note 若需"板载 PID"模式 (关节驱动板内部执行 PD)，调用 hardwareMitControl(p, v, t, kp, kd)； + * 切回板外 PID 请调用 exitHardwareMitMode()。 + * @details 该类实现了 Motor 类的纯虚函数，用于控制云深处 J60 关节电机。 + */ +class MotorDeepJ60 : public Motor +{ +public: + enum J60State : uint8_t { + J60_DISABLED = 0, // 未使能 (上电初始态或 disable 响应已收到) + J60_ENABLED = 1, // 使能完成，允许发送控制帧 + }; + + enum J60Intent : uint8_t { + INTENT_ENABLE = 0, // 默认意图：保持使能 + INTENT_DISABLE = 1, // 用户调用 disable() 后进入 + }; + +protected: + uint8_t m_jointID; // 关节 ID (1~15) + J60State m_state; // 实际硬件状态 (由反馈帧更新) + J60Intent m_intent; // 用户意图 + fp32 m_currentTorqueNm; // 反馈扭矩 (Nm) + int8_t m_mosfetTemperature; // MOSFET 温度 (℃) + int8_t m_motorTemperature; // 电机温度 (℃) + // 硬件 MIT 模式缓存 (hardwareMitControl 使用) + bool m_useHardwareMitMode; + fp32 m_mitTargetPosition; // rad, [-40, 40] + fp32 m_mitTargetVelocity; // rad/s, [-40, 40] + fp32 m_mitTargetTorque; // Nm, [-40, 40] + fp32 m_mitKp; // [0, 1023] + fp32 m_mitKd; // [0, 51] + + // 协议物理量范围 + static constexpr fp32 J60_POSITION_MAX = 40.0f; + static constexpr fp32 J60_POSITION_MIN = -40.0f; + static constexpr fp32 J60_VELOCITY_MAX = 40.0f; + static constexpr fp32 J60_VELOCITY_MIN = -40.0f; + static constexpr fp32 J60_TORQUE_MAX = 40.0f; + static constexpr fp32 J60_TORQUE_MIN = -40.0f; + static constexpr fp32 J60_KP_MAX = 1023.0f; + static constexpr fp32 J60_KD_MAX = 51.0f; + static constexpr fp32 J60_TEMP_MAX = 200.0f; + static constexpr fp32 J60_TEMP_MIN = -20.0f; + // J60 命令索引 + static constexpr uint8_t CMD_DISABLE = 1; + static constexpr uint8_t CMD_ENABLE = 2; + static constexpr uint8_t CMD_CONTROL = 4; + static constexpr uint8_t CMD_ERROR_RESET = 17; + +public: + MotorDeepJ60(uint8_t jointID, Controller *controller); + + // 用户意图控制 + void enable(); // 意图置为使能 (下一帧尝试重连/重使能) + void disable(); // 意图置为失能 (下一帧开始发 DISABLE) + void clearError(); // 发送 ERROR_RESET，后续随重新 ENABLE 恢复 + + // 硬件 MIT 模式 (可选，绕开基类 Controller，直接下发五元组) + void hardwareMitControl(fp32 targetPositionRad, fp32 targetVelocityRadps, + fp32 targetTorqueNm, fp32 kp, fp32 kd); + void exitHardwareMitMode(); + + // 状态查询 + J60State getJ60State() const { return m_state; } + fp32 getCurrentTorqueNm() const { return m_currentTorqueNm; } + int8_t getMosfetTemperature() const { return m_mosfetTemperature; } + int8_t getMotorTemperature() const { return m_motorTemperature; } + uint8_t getJointID() const { return m_jointID; } + +protected: + bool decodeCanRxMessage(const can_rx_message_t &rxMessage) override; + void convertControllerOutputToMotorControlData() override; + fp32 updateCurrentRevolutions() override { return m_currentRevolutions; } + + // 构造 CAN ID: (cmdIdx << 5) | jointField + static constexpr uint16_t makeCanId(uint8_t jointField, uint8_t cmdIdx) + { + return (uint16_t)(((uint16_t)cmdIdx << 5) | (jointField & 0x1Fu)); + } + void buildEnableFrame(); + void buildDisableFrame(); + void buildErrorResetFrame(); + void buildControlFrame(fp32 pRad, fp32 vRadps, fp32 tNm, fp32 kp, fp32 kd); +}; + +// 型别别名，供用户短写 +using MotorJ60 = MotorDeepJ60; + /* Exported constants --------------------------------------------------------*/ /* Exported macro ------------------------------------------------------------*/ diff --git a/GSRL/Device/src/dvc_motor.cpp b/GSRL/Device/src/dvc_motor.cpp index d720a81..404f8b0 100644 --- a/GSRL/Device/src/dvc_motor.cpp +++ b/GSRL/Device/src/dvc_motor.cpp @@ -1028,3 +1028,296 @@ void MotorLKMG::setBrake(bool isBraked) m_motorControlData[7] = 0x00; } } + +/****************************************************************************** + * 云深处 J60 关节电机类实现 + ******************************************************************************/ + +/** + * @brief 云深处 J60 关节电机构造函数 + * @param jointID 关节 ID (1~15)，需与 J60 调试工具中设置的 ID 一致 + * @param controller 电机绑定的控制器 (板外 PID 模式下使用；若仅使用 hardwareMitControl 可传 nullptr) + * @note CAN 控制 ID 与反馈 ID 均按协议 (cmdIdx<<5) | jointField 动态生成， + * 基类存储的 ID 仅作 CONTROL 帧的"默认值"，实际发送帧的 StdId 会在状态机切换时更新。 + */ +MotorDeepJ60::MotorDeepJ60(uint8_t jointID, Controller *controller) + : Motor(makeCanId(jointID, CMD_CONTROL), + makeCanId((uint8_t)(jointID + 0x10), CMD_CONTROL), + controller, + 0), + m_jointID(jointID), + m_state(J60_DISABLED), + m_intent(INTENT_ENABLE), + m_currentTorqueNm(0.0f), + m_mosfetTemperature(0), + m_motorTemperature(0), + m_useHardwareMitMode(false), + m_mitTargetPosition(0.0f), + m_mitTargetVelocity(0.0f), + m_mitTargetTorque(0.0f), + m_mitKp(0.0f), + m_mitKd(0.0f) +{ + // 基类构造函数已将 DLC=8、IDE=STD、RTR=DATA 设置完毕。 + // 首次 convertControllerOutputToMotorControlData() 会按 m_state 构建 ENABLE 帧。 +} + +/** + * @brief 将用户意图置为使能 + * @note 仅修改意图；实际 ENABLE 帧在下一次 convertControllerOutputToMotorControlData() 中发送。 + */ +void MotorDeepJ60::enable() +{ + m_intent = INTENT_ENABLE; +} + +/** + * @brief 将用户意图置为失能 + * @note 仅修改意图；实际 DISABLE 帧在下一次 convertControllerOutputToMotorControlData() 中发送。 + */ +void MotorDeepJ60::disable() +{ + m_intent = INTENT_DISABLE; +} + +/** + * @brief 请求发送清错命令 + * @note 发送完 ERROR_RESET 后，驱动器回到失能态，m_state 下一周期会被重置为 J60_DISABLED， + * 若 m_intent 仍为 INTENT_ENABLE，会自动重新 ENABLE。 + * @note 为简化状态机，本方法通过复用 buildErrorResetFrame 在下一次 convert 时插入一帧， + * 使用独立标志位触发一次性发送。 + */ +void MotorDeepJ60::clearError() +{ + // 复位硬件状态为 DISABLED，下一次 convert 会先发 ERROR_RESET 再按 intent 走 ENABLE 流程。 + // 这里直接构造一次 ERROR_RESET header，由 convert 中的状态判断兜底。 + buildErrorResetFrame(); + m_state = J60_DISABLED; +} + +/** + * @brief 硬件 MIT 模式控制 (板载 PD) + * @param targetPositionRad 目标位置 (rad, [-40, 40]) + * @param targetVelocityRadps 目标速度 (rad/s, [-40, 40]) + * @param targetTorqueNm 前馈扭矩 (Nm, [-40, 40]) + * @param kp 位置环刚度 ([0, 1023]) + * @param kd 速度环阻尼 ([0, 51]) + * @note 调用后控制帧由本方法参数生成，直到 exitHardwareMitMode() 被调用才恢复板外 PID 模式。 + * @note 电机板执行的控制律为 T = Kp*(pDes - pFb) + Kd*(vDes - vFb) + tFF。 + */ +void MotorDeepJ60::hardwareMitControl(fp32 targetPositionRad, fp32 targetVelocityRadps, + fp32 targetTorqueNm, fp32 kp, fp32 kd) +{ + m_useHardwareMitMode = true; + m_mitTargetPosition = targetPositionRad; + m_mitTargetVelocity = targetVelocityRadps; + m_mitTargetTorque = targetTorqueNm; + m_mitKp = kp; + m_mitKd = kd; +} + +/** + * @brief 退出硬件 MIT 模式，恢复使用基类 Controller 作为力矩源 + */ +void MotorDeepJ60::exitHardwareMitMode() +{ + m_useHardwareMitMode = false; + m_mitTargetPosition = 0.0f; + m_mitTargetVelocity = 0.0f; + m_mitTargetTorque = 0.0f; + m_mitKp = 0.0f; + m_mitKd = 0.0f; +} + +/** + * @brief 构造 ENABLE 帧 (cmdIdx=2, DLC=0) + */ +void MotorDeepJ60::buildEnableFrame() +{ + m_motorControlHeader.StdId = makeCanId(m_jointID, CMD_ENABLE); + m_motorControlHeader.DLC = 0; + memset(m_motorControlData, 0, sizeof(m_motorControlData)); +} + +/** + * @brief 构造 DISABLE 帧 (cmdIdx=1, DLC=0) + */ +void MotorDeepJ60::buildDisableFrame() +{ + m_motorControlHeader.StdId = makeCanId(m_jointID, CMD_DISABLE); + m_motorControlHeader.DLC = 0; + memset(m_motorControlData, 0, sizeof(m_motorControlData)); +} + +/** + * @brief 构造 ERROR_RESET 帧 (cmdIdx=17, DLC=0) + */ +void MotorDeepJ60::buildErrorResetFrame() +{ + m_motorControlHeader.StdId = makeCanId(m_jointID, CMD_ERROR_RESET); + m_motorControlHeader.DLC = 0; + memset(m_motorControlData, 0, sizeof(m_motorControlData)); +} + +/** + * @brief 构造 CONTROL 帧 (cmdIdx=4, DLC=8, MIT 单帧控制) + * @param pRad 目标位置 (rad) + * @param vRadps 目标速度 (rad/s) + * @param tNm 目标扭矩 (Nm) + * @param kp 刚度 ([0, 1023]) + * @param kd 阻尼 ([0, 51]) + * @note 按协议小端位序：pos[0:15]|vel[16:29]|kp[30:39]|kd[40:47]|tor[48:63]， + * 通过 uint64_t 拼装后 memcpy 到 CAN 数据区 (STM32 为小端架构)。 + */ +void MotorDeepJ60::buildControlFrame(fp32 pRad, fp32 vRadps, fp32 tNm, fp32 kp, fp32 kd) +{ + m_motorControlHeader.StdId = makeCanId(m_jointID, CMD_CONTROL); + m_motorControlHeader.DLC = 8; + + // 限幅到协议允许的物理量范围 + GSRLMath::constrain(pRad, J60_POSITION_MIN, J60_POSITION_MAX); + GSRLMath::constrain(vRadps, J60_VELOCITY_MIN, J60_VELOCITY_MAX); + GSRLMath::constrain(tNm, J60_TORQUE_MIN, J60_TORQUE_MAX); + GSRLMath::constrain(kp, 0.0f, J60_KP_MAX); + GSRLMath::constrain(kd, 0.0f, J60_KD_MAX); + + uint64_t pos = (uint64_t)GSRLMath::convertFloatToUint(pRad, J60_POSITION_MIN, J60_POSITION_MAX, 16); + uint64_t vel = (uint64_t)GSRLMath::convertFloatToUint(vRadps, J60_VELOCITY_MIN, J60_VELOCITY_MAX, 14); + uint64_t kpI = (uint64_t)GSRLMath::convertFloatToUint(kp, 0.0f, J60_KP_MAX, 10); + uint64_t kdI = (uint64_t)GSRLMath::convertFloatToUint(kd, 0.0f, J60_KD_MAX, 8); + uint64_t tor = (uint64_t)GSRLMath::convertFloatToUint(tNm, J60_TORQUE_MIN, J60_TORQUE_MAX, 16); + + uint64_t payload = 0; + payload |= (pos & 0xFFFFull) << 0; // bit 0-15 + payload |= (vel & 0x3FFFull) << 16; // bit 16-29 + payload |= (kpI & 0x3FFull) << 30; // bit 30-39 + payload |= (kdI & 0xFFull) << 40; // bit 40-47 + payload |= (tor & 0xFFFFull) << 48; // bit 48-63 + + memcpy(m_motorControlData, &payload, sizeof(payload)); +} + +/** + * @brief 根据状态机与用户意图构造当前应发送的帧 + * @note 状态转移矩阵： + * | intent | hwState | 发送帧 | + * | -------- | -------- | ----------- | + * | ENABLE | DISABLED | ENABLE | + * | ENABLE | ENABLED | CONTROL | + * | DISABLE | * | DISABLE | + * @note 掉线检测：若基类判定失联，则把 m_state 重置为 J60_DISABLED 以触发自动重连。 + */ +void MotorDeepJ60::convertControllerOutputToMotorControlData() +{ + // 掉线探测：若基类已判定失联，则重置硬件状态，下一帧起重发 ENABLE。 + if (!m_isMotorConnected) { + m_state = J60_DISABLED; + } + + if (m_intent == INTENT_DISABLE) { + buildDisableFrame(); + return; + } + + // m_intent == INTENT_ENABLE + if (m_state == J60_DISABLED) { + buildEnableFrame(); + return; + } + + // m_intent == INTENT_ENABLE && m_state == J60_ENABLED → 发送控制帧 + if (m_useHardwareMitMode) { + buildControlFrame(m_mitTargetPosition, m_mitTargetVelocity, + m_mitTargetTorque, m_mitKp, m_mitKd); + } else { + // 板外 PID 模式：Kp=Kd=0，位置/速度目标置零 (不参与板载 PD)，扭矩前馈由 m_controllerOutput 提供。 + // 基类的 angleClosedloopControl / torqueCurrentClosedloopControl 等已处理 m_controllerOutputPolarity。 + buildControlFrame(0.0f, 0.0f, m_controllerOutput, 0.0f, 0.0f); + } +} + +/** + * @brief 解析 J60 反馈 CAN 帧 + * @param rxMessage CAN 接收消息 + * @return true ID 属于本关节且命令索引已识别，解析成功 + * @return false ID 不属于本关节或命令索引未支持 + * @note J60 反馈 ID 的 Bit0~Bit4 = jointID + 0x10 (驱动器应答时 bit4 置 1)， + * Bit5~Bit10 = 命令索引。本函数按位段精匹配，避免与其它电机 ID 冲突。 + * @note CONTROL 响应帧 (DLC=8) 的位段 (小端)： + * pos[0:19]|vel[20:39]|tor[40:55]|tempFlag[56]|temp[57:63] + */ +bool MotorDeepJ60::decodeCanRxMessage(const can_rx_message_t &rxMessage) +{ + uint16_t rxId = (uint16_t)rxMessage.header.StdId; + uint8_t rxJointField = (uint8_t)(rxId & 0x1Fu); + uint8_t rxCmdIndex = (uint8_t)((rxId >> 5) & 0x3Fu); + uint8_t expectedField = (uint8_t)(m_jointID + 0x10u); + + if (rxJointField != expectedField) { + return false; + } + + switch (rxCmdIndex) { + case CMD_ENABLE: { + // 使能应答 DLC=1, byte0=0 表示成功 + if (rxMessage.header.DLC >= 1 && rxMessage.data[0] == 0) { + m_state = J60_ENABLED; + } + return true; + } + case CMD_DISABLE: + case CMD_ERROR_RESET: { + // 失能成功 / 清错成功后驱动器均回到失能态，由后续 ENABLE 流程恢复 + if (rxMessage.header.DLC >= 1 && rxMessage.data[0] == 0) { + m_state = J60_DISABLED; + } + return true; + } + case CMD_CONTROL: { + if (rxMessage.header.DLC != 8) { + return false; + } + + // 收到控制应答即证明硬件已使能 (驱动器在失能态下收到控制帧也会回应，但仍可作为 online 证据) + m_state = J60_ENABLED; + + uint64_t payload = 0; + memcpy(&payload, rxMessage.data, 8); + + uint32_t posRaw = (uint32_t)((payload >> 0) & 0xFFFFFull); // 20 bit + uint32_t velRaw = (uint32_t)((payload >> 20) & 0xFFFFFull); // 20 bit + uint32_t torRaw = (uint32_t)((payload >> 40) & 0xFFFFull); // 16 bit + uint8_t tempFlg = (uint8_t)((payload >> 56) & 0x1ull); + uint8_t tempRaw = (uint8_t)((payload >> 57) & 0x7Full); + + fp32 decodedPosRad = + GSRLMath::convertUintToFloat((int)posRaw, J60_POSITION_MIN, J60_POSITION_MAX, 20); + fp32 decodedVelRadps = + GSRLMath::convertUintToFloat((int)velRaw, J60_VELOCITY_MIN, J60_VELOCITY_MAX, 20); + fp32 decodedTorNm = + GSRLMath::convertUintToFloat((int)torRaw, J60_TORQUE_MIN, J60_TORQUE_MAX, 16); + fp32 decodedTempC = + GSRLMath::convertUintToFloat((int)tempRaw, J60_TEMP_MIN, J60_TEMP_MAX, 7); + + // 维持基类不变式：m_currentAngle ∈ [0, 2PI) + m_currentAngle = GSRLMath::normalizeAngle(decodedPosRad); + m_lastAngle = m_currentAngle; // 防止基类 updateCurrentRevolutions 误累积 (本类已 override) + m_currentRevolutions = decodedPosRad / (2.0f * MATH_PI); + m_currentAngularVelocity = decodedVelRadps; + m_currentTorqueNm = decodedTorNm; + // 兼容基类 getCurrentTorqueCurrent()：以 mNm 存储，±40 Nm → ±40000，在 int16 范围内 + m_currentTorqueCurrent = (int16_t)(decodedTorNm * 1000.0f); + + if (tempFlg == 1) { + m_motorTemperature = (int8_t)decodedTempC; + m_temperature = m_motorTemperature; + } else { + m_mosfetTemperature = (int8_t)decodedTempC; + m_temperature = m_mosfetTemperature; + } + return true; + } + default: + return false; + } +} diff --git a/GSRL/Documentation/Device/MotorDeepJ60.md b/GSRL/Documentation/Device/MotorDeepJ60.md new file mode 100644 index 0000000..c88cb3d --- /dev/null +++ b/GSRL/Documentation/Device/MotorDeepJ60.md @@ -0,0 +1,134 @@ +# 云深处 J60 关节电机驱动使用说明 + +版本：v1.0.0 +最后更新日期：2026-04-11 + +--- + +## 1：概述 + +本驱动基于 GSRL 库，为云深处 DeepRobotics J60 关节电机提供 CAN 通信（1Mbps）支持。 + +模块基于 C++ 封装，包含： +- 电机驱动类 `MotorDeepJ60`（别名 `MotorJ60`） +- J60 私有协议（命令索引式 CAN ID）的打包/解析逻辑 +- 上电后 DISABLE → ENABLE → CONTROL 的自动状态机，掉线自动重连 + +## 2：功能特性 + +本驱动可实现： +- 与 GSRL 框架其他电机一致的基类接口： + - `torqueCurrentClosedloopControl` 力矩前馈控制（板外 PID） + - `angleClosedloopControl` / `revolutionsClosedloopControl` 位置闭环 + - `externalClosedloopControl` 外部反馈闭环 +- `hardwareMitControl(p, v, t, kp, kd)` 可选的板载 PD 模式（关节板内部执行 PD 控制律） +- `enable()` / `disable()` / `clearError()` 的高级状态控制 +- 实时获取电机反馈： + - 当前角度 `getCurrentAngle()`（单位 rad，范围 [0, 2π)） + - 多圈绝对位置 `getCurrentRevolutions()`（单位 圈，J60 原生支持 ±6.37 圈） + - 当前角速度 `getCurrentAngularVelocity()`（单位 rad/s） + - 当前扭矩 `getCurrentTorqueNm()`（单位 Nm，高精度） + - 电机温度 / MOSFET 温度（℃） + - 电机连接状态 `isMotorConected()` + +特性： +- 使用 J60 CAN 协议 V2.0（MIT 单帧控制模式） +- 数据自动限幅（P: ±40 rad，V: ±40 rad/s，T: ±40 Nm，Kp: [0,1023]，Kd: [0,51]） +- 掉线后自动重发 ENABLE 命令重连 +- 不依赖独立的保活任务，每次 `convert...()` 调用即根据状态机自动选择要发送的帧 + +## 3：适用环境 + +- MCU：STM32 系列（F4/H7 等） +- 开发环境：VS Code + EIDE / Keil / CubeMX +- 依赖库： + - STM32 HAL `drv_can.h` + - 自定义库：`dvc_motor.hpp`、`alg_pid.hpp`、`alg_general.hpp` +- 通信环境：CAN（波特率 1Mbps，建议开启自动重传与自动错误恢复，本项目已在 CubeMX 中配置） + +## 4：J60 电机准备工作 + +使用前请先用 J60 调试工具： +1. 设置关节 ID（默认为 1） +2. 标定零位（运行时 CAN 协议不提供设零位命令） +3. 合理设置 CAN 超时时间（建议 200ms 左右），避免掉线后电机失控乱跳 +4. 若需要配置电流环带宽等参数，请发送 `CMD_SAVE_CONFIG` 永久生效 + +## 5：典型用法实例 + +```cpp +#include "dvc_motor.hpp" +#include "alg_pid.hpp" +#include "main.h" + +// 1. 构造板外 PID 控制器（力矩输出，单位 Nm） +SimplePID::PIDParam torqueParam = { + 2.0f, // Kp + 0.05f, // Ki + 0.0f, // Kd + 40.0f, // outputLimit (Nm) + 20.0f // integralLimit +}; +SimplePID j60TorquePID(SimplePID::PID_POSITION, torqueParam); + +// 2. 构造 J60 电机实例（关节 ID = 1） +MotorJ60 j60Motor(1, &j60TorquePID); + +extern "C" void can1RxCallback(can_rx_message_t *pRxMsg); +inline void transmitMotorsControlData(); + +int main() +{ + // CAN 初始化（绑定回调，具体函数名依据本项目 BSP 封装） + CAN_Init(&hcan1, can1RxCallback); + + // 构造即默认意图为使能，无需手动调用 enable() + // （若希望延迟到某一时刻才使能，可在构造后先调 j60Motor.disable()） + + while (1) { + // 3. 计算并打包 CAN 控制数据 + // 方式 A：力矩目标闭环（上位控制器算出力矩，Nm -> mNm） + // j60Motor.torqueCurrentClosedloopControl(500); // 目标 0.5 Nm + + // 方式 B：角度目标闭环（基类已自动算出扭矩） + j60Motor.angleClosedloopControl(0.0f); + + // 方式 C：板载 PD 模式（绕开基类控制器，直接下发 MIT 五元组） + // j60Motor.hardwareMitControl(0.0f, 0.0f, 0.0f, 5.0f, 0.1f); + + // 4. 发送 CAN 帧 + transmitMotorsControlData(); + + // 控制周期建议 500Hz ~ 1kHz；本示例使用 FreeRTOS 延时 + osDelay(2); + } +} + +// CAN 接收中断回调 +extern "C" void can1RxCallback(can_rx_message_t *pRxMsg) +{ + j60Motor.decodeCanRxMessageFromISR(pRxMsg); +} + +/** + * @brief 传输电机控制数据 + */ +inline void transmitMotorsControlData() +{ + uint32_t mailbox; + HAL_CAN_AddTxMessage(&hcan1, + j60Motor.getMotorControlHeader(), + const_cast(j60Motor.getMotorControlData()), + &mailbox); +} +``` + +## 6：注意事项 + +1. **关节 ID**：确保 `MotorJ60` 构造参数与调试工具中设置的 ID 一致，否则 `decodeCanRxMessage` 无法匹配反馈帧，`isMotorConected()` 将一直返回 `false`。 +2. **上电顺序**：建议先给控制板上电并启动 CAN，再给 J60 电机上电；驱动器上电默认失能，本驱动构造后默认意图为使能，随即发送 ENABLE 命令，收到应答后进入控制模式。 +3. **控制周期**：建议在 `500Hz ~ 1kHz` 范围内调用闭环函数 + 发送 CAN 帧。控制周期过低会导致位置闭环响应迟钝，过高会占用 CAN 总线带宽影响多电机通信。 +4. **力矩安全**：J60 最大输出扭矩 ±40 N·m，调试初期请使用较小的控制器输出限幅（如 ±5 N·m）。 +5. **板载 PD vs 板外 PID**：默认启用板外 PID（`Kp=Kd=0`，仅下发扭矩前馈），适合已有 GSRL 框架内 PID 的场景。若需要板载快速 PD 响应，调用 `hardwareMitControl()` 切换模式，调用 `exitHardwareMitMode()` 退回板外 PID 模式。 +6. **掉线重连**：若 CAN 总线断开，基类连续检测到序号无更新后会将 `isMotorConected()` 置为 `false`；驱动会自动回到 `J60_DISABLED` 状态并在下一帧重发 ENABLE。CAN 恢复后几帧内即可重新使能。 +7. **温度监控**：J60 以单字节复用方式上报温度，`Bit56` 标志位区分 MOSFET 温度与电机温度。使用 `getMotorTemperature()` 和 `getMosfetTemperature()` 分别读取，建议在高负载运行时轮询检查。 From 3d03aa71428b616f3dc00633980c527983db6338 Mon Sep 17 00:00:00 2001 From: DoveXY Date: Sun, 12 Apr 2026 20:39:31 +0800 Subject: [PATCH 2/5] =?UTF-8?q?=E4=BF=AE=E5=A4=8Dcodex=E8=AF=B4=E7=9A=84bu?= =?UTF-8?q?g?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- GSRL/Device/inc/dvc_motor.hpp | 20 +++++++++----------- GSRL/Device/src/dvc_motor.cpp | 31 +++++++++++++++++++++++-------- 2 files changed, 32 insertions(+), 19 deletions(-) diff --git a/GSRL/Device/inc/dvc_motor.hpp b/GSRL/Device/inc/dvc_motor.hpp index 0fecaf1..80240ae 100644 --- a/GSRL/Device/inc/dvc_motor.hpp +++ b/GSRL/Device/inc/dvc_motor.hpp @@ -287,12 +287,13 @@ class MotorDeepJ60 : public Motor }; protected: - uint8_t m_jointID; // 关节 ID (1~15) - J60State m_state; // 实际硬件状态 (由反馈帧更新) - J60Intent m_intent; // 用户意图 - fp32 m_currentTorqueNm; // 反馈扭矩 (Nm) - int8_t m_mosfetTemperature; // MOSFET 温度 (℃) - int8_t m_motorTemperature; // 电机温度 (℃) + uint8_t m_jointID; // 关节 ID (1~15) + J60State m_state; // 实际硬件状态 (由反馈帧更新) + J60Intent m_intent; // 用户意图 + fp32 m_currentTorqueNm; // 反馈扭矩 (Nm) + int16_t m_mosfetTemperature; // MOSFET 温度 (℃)，协议范围 [-20, 200] + int16_t m_motorTemperature; // 电机温度 (℃)，协议范围 [-20, 200] + bool m_pendingErrorReset; // clearError() 设置，convert 中消费 // 硬件 MIT 模式缓存 (hardwareMitControl 使用) bool m_useHardwareMitMode; fp32 m_mitTargetPosition; // rad, [-40, 40] @@ -334,8 +335,8 @@ class MotorDeepJ60 : public Motor // 状态查询 J60State getJ60State() const { return m_state; } fp32 getCurrentTorqueNm() const { return m_currentTorqueNm; } - int8_t getMosfetTemperature() const { return m_mosfetTemperature; } - int8_t getMotorTemperature() const { return m_motorTemperature; } + int16_t getMosfetTemperature() const { return m_mosfetTemperature; } + int16_t getMotorTemperature() const { return m_motorTemperature; } uint8_t getJointID() const { return m_jointID; } protected: @@ -354,9 +355,6 @@ class MotorDeepJ60 : public Motor void buildControlFrame(fp32 pRad, fp32 vRadps, fp32 tNm, fp32 kp, fp32 kd); }; -// 型别别名，供用户短写 -using MotorJ60 = MotorDeepJ60; - /* Exported constants --------------------------------------------------------*/ /* Exported macro ------------------------------------------------------------*/ diff --git a/GSRL/Device/src/dvc_motor.cpp b/GSRL/Device/src/dvc_motor.cpp index 404f8b0..7ed6715 100644 --- a/GSRL/Device/src/dvc_motor.cpp +++ b/GSRL/Device/src/dvc_motor.cpp @@ -1051,6 +1051,7 @@ MotorDeepJ60::MotorDeepJ60(uint8_t jointID, Controller *controller) m_currentTorqueNm(0.0f), m_mosfetTemperature(0), m_motorTemperature(0), + m_pendingErrorReset(false), m_useHardwareMitMode(false), m_mitTargetPosition(0.0f), m_mitTargetVelocity(0.0f), @@ -1091,7 +1092,7 @@ void MotorDeepJ60::clearError() { // 复位硬件状态为 DISABLED，下一次 convert 会先发 ERROR_RESET 再按 intent 走 ENABLE 流程。 // 这里直接构造一次 ERROR_RESET header，由 convert 中的状态判断兜底。 - buildErrorResetFrame(); + m_pendingErrorReset = true; m_state = J60_DISABLED; } @@ -1114,6 +1115,7 @@ void MotorDeepJ60::hardwareMitControl(fp32 targetPositionRad, fp32 targetVelocit m_mitTargetTorque = targetTorqueNm; m_mitKp = kp; m_mitKd = kd; + convertControllerOutputToMotorControlData(); } /** @@ -1214,6 +1216,13 @@ void MotorDeepJ60::convertControllerOutputToMotorControlData() m_state = J60_DISABLED; } + // 一次性 ERROR_RESET 帧插入 (clearError() 设置) + if (m_pendingErrorReset) { + m_pendingErrorReset = false; + buildErrorResetFrame(); + return; + } + if (m_intent == INTENT_DISABLE) { buildDisableFrame(); return; @@ -1305,15 +1314,21 @@ bool MotorDeepJ60::decodeCanRxMessage(const can_rx_message_t &rxMessage) m_currentRevolutions = decodedPosRad / (2.0f * MATH_PI); m_currentAngularVelocity = decodedVelRadps; m_currentTorqueNm = decodedTorNm; - // 兼容基类 getCurrentTorqueCurrent()：以 mNm 存储，±40 Nm → ±40000，在 int16 范围内 - m_currentTorqueCurrent = (int16_t)(decodedTorNm * 1000.0f); - + // 兼容基类 getCurrentTorqueCurrent()：以 mNm 存储，钳位到 int16 范围 (±32767)； + // 精确扭矩请使用 getCurrentTorqueNm()。 + fp32 torqueMnm = decodedTorNm * 1000.0f; + GSRLMath::constrain(torqueMnm, -32767.0f, 32767.0f); + m_currentTorqueCurrent = (int16_t)torqueMnm; + + // 温度：J60 专有成员用 int16_t 存完整范围，基类 m_temperature (int8_t) 钳位 + fp32 tempClamped = decodedTempC; + GSRLMath::constrain(tempClamped, -128.0f, 127.0f); if (tempFlg == 1) { - m_motorTemperature = (int8_t)decodedTempC; - m_temperature = m_motorTemperature; + m_motorTemperature = (int16_t)decodedTempC; + m_temperature = (int8_t)tempClamped; } else { - m_mosfetTemperature = (int8_t)decodedTempC; - m_temperature = m_mosfetTemperature; + m_mosfetTemperature = (int16_t)decodedTempC; + m_temperature = (int8_t)tempClamped; } return true; } From 8fde73e1ce710edac47d27694ea5c87819363c09 Mon Sep 17 00:00:00 2001 From: DoveXY Date: Mon, 20 Apr 2026 00:52:34 +0800 Subject: [PATCH 3/5] =?UTF-8?q?=E4=BF=AE=E8=AE=A2=E9=83=A8=E5=88=86?= =?UTF-8?q?=E9=94=99=E8=AF=AF=E5=92=8C=E5=BE=88AI=E7=9A=84=E6=96=87?= =?UTF-8?q?=E6=A1=A3=EF=BC=8C=E6=A0=BC=E5=BC=8F=E5=8C=96md=E6=96=87?= =?UTF-8?q?=E4=BB=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- GSRL/Documentation/Device/MotorDeepJ60.md | 47 ++++++++++------------- 1 file changed, 21 insertions(+), 26 deletions(-) diff --git a/GSRL/Documentation/Device/MotorDeepJ60.md b/GSRL/Documentation/Device/MotorDeepJ60.md index c88cb3d..88e7058 100644 --- a/GSRL/Documentation/Device/MotorDeepJ60.md +++ b/GSRL/Documentation/Device/MotorDeepJ60.md @@ -10,51 +10,46 @@ 本驱动基于 GSRL 库，为云深处 DeepRobotics J60 关节电机提供 CAN 通信（1Mbps）支持。 模块基于 C++ 封装，包含： -- 电机驱动类 `MotorDeepJ60`（别名 `MotorJ60`） + +- 电机驱动类 `MotorDeepJ60` - J60 私有协议（命令索引式 CAN ID）的打包/解析逻辑 - 上电后 DISABLE → ENABLE → CONTROL 的自动状态机，掉线自动重连 ## 2：功能特性 本驱动可实现： + - 与 GSRL 框架其他电机一致的基类接口： - - `torqueCurrentClosedloopControl` 力矩前馈控制（板外 PID） - - `angleClosedloopControl` / `revolutionsClosedloopControl` 位置闭环 - - `externalClosedloopControl` 外部反馈闭环 + - `torqueCurrentClosedloopControl` 力矩前馈控制（板外 PID） + - `angleClosedloopControl` / `revolutionsClosedloopControl` 位置闭环 + - `externalClosedloopControl` 外部反馈闭环 - `hardwareMitControl(p, v, t, kp, kd)` 可选的板载 PD 模式（关节板内部执行 PD 控制律） - `enable()` / `disable()` / `clearError()` 的高级状态控制 - 实时获取电机反馈： - - 当前角度 `getCurrentAngle()`（单位 rad，范围 [0, 2π)） - - 多圈绝对位置 `getCurrentRevolutions()`（单位 圈，J60 原生支持 ±6.37 圈） - - 当前角速度 `getCurrentAngularVelocity()`（单位 rad/s） - - 当前扭矩 `getCurrentTorqueNm()`（单位 Nm，高精度） - - 电机温度 / MOSFET 温度（℃） - - 电机连接状态 `isMotorConected()` + - 当前角度 `getCurrentAngle()`（单位 rad，范围 [0, 2π)） + - 多圈绝对位置 `getCurrentRevolutions()`（单位 圈，J60 原生支持 ±6.37 圈） + - 当前角速度 `getCurrentAngularVelocity()`（单位 rad/s） + - 当前扭矩 `getCurrentTorqueNm()`（单位 Nm，高精度） + - 电机温度 / MOSFET 温度（℃） + - 电机连接状态 `isMotorConected()` 特性： + - 使用 J60 CAN 协议 V2.0（MIT 单帧控制模式） - 数据自动限幅（P: ±40 rad，V: ±40 rad/s，T: ±40 Nm，Kp: [0,1023]，Kd: [0,51]） - 掉线后自动重发 ENABLE 命令重连 - 不依赖独立的保活任务，每次 `convert...()` 调用即根据状态机自动选择要发送的帧 -## 3：适用环境 - -- MCU：STM32 系列（F4/H7 等） -- 开发环境：VS Code + EIDE / Keil / CubeMX -- 依赖库： - - STM32 HAL `drv_can.h` - - 自定义库：`dvc_motor.hpp`、`alg_pid.hpp`、`alg_general.hpp` -- 通信环境：CAN（波特率 1Mbps，建议开启自动重传与自动错误恢复，本项目已在 CubeMX 中配置） +## 3：J60 电机准备工作 -## 4：J60 电机准备工作 +使用前请调试J60参数： -使用前请先用 J60 调试工具： -1. 设置关节 ID（默认为 1） +1. 使用串口调试助手设置关节 ID（默认为 1 但仍需设置） 2. 标定零位（运行时 CAN 协议不提供设零位命令） 3. 合理设置 CAN 超时时间（建议 200ms 左右），避免掉线后电机失控乱跳 -4. 若需要配置电流环带宽等参数，请发送 `CMD_SAVE_CONFIG` 永久生效 +4. 若需要使用CAN配置电机参数，请发送 `CMD_SAVE_CONFIG` 永久生效 -## 5：典型用法实例 +## 4：典型用法实例 ```cpp #include "dvc_motor.hpp" @@ -123,12 +118,12 @@ inline void transmitMotorsControlData() } ``` -## 6：注意事项 +## 5：注意事项 -1. **关节 ID**：确保 `MotorJ60` 构造参数与调试工具中设置的 ID 一致，否则 `decodeCanRxMessage` 无法匹配反馈帧，`isMotorConected()` 将一直返回 `false`。 +1. **关节 ID**：确保 `MotorJ60` 构造参数与调试工具中设置的 ID 一致，否则 `decodeCanRxMessage` 无法匹配反馈帧，`isMotorConected()` 将一直返回 `false`，目前发现默认ID不一定为1，请务必自行使用串口调试助手进行设置。 2. **上电顺序**：建议先给控制板上电并启动 CAN，再给 J60 电机上电；驱动器上电默认失能，本驱动构造后默认意图为使能，随即发送 ENABLE 命令，收到应答后进入控制模式。 3. **控制周期**：建议在 `500Hz ~ 1kHz` 范围内调用闭环函数 + 发送 CAN 帧。控制周期过低会导致位置闭环响应迟钝，过高会占用 CAN 总线带宽影响多电机通信。 -4. **力矩安全**：J60 最大输出扭矩 ±40 N·m，调试初期请使用较小的控制器输出限幅（如 ±5 N·m）。 +4. **力矩安全**：J60 最大输出扭矩 ±40 N·m，调试初期请使用较小的输出限幅，保证自身安全。 5. **板载 PD vs 板外 PID**：默认启用板外 PID（`Kp=Kd=0`，仅下发扭矩前馈），适合已有 GSRL 框架内 PID 的场景。若需要板载快速 PD 响应，调用 `hardwareMitControl()` 切换模式，调用 `exitHardwareMitMode()` 退回板外 PID 模式。 6. **掉线重连**：若 CAN 总线断开，基类连续检测到序号无更新后会将 `isMotorConected()` 置为 `false`；驱动会自动回到 `J60_DISABLED` 状态并在下一帧重发 ENABLE。CAN 恢复后几帧内即可重新使能。 7. **温度监控**：J60 以单字节复用方式上报温度，`Bit56` 标志位区分 MOSFET 温度与电机温度。使用 `getMotorTemperature()` 和 `getMosfetTemperature()` 分别读取，建议在高负载运行时轮询检查。 From a0b7cf764712bedb2a10c83b8326da81bac411c0 Mon Sep 17 00:00:00 2001 From: DoveXY Date: Wed, 22 Apr 2026 22:24:12 +0800 Subject: [PATCH 4/5] =?UTF-8?q?=E5=88=A0=E9=99=A4=E5=A4=9A=E5=9C=88?= =?UTF-8?q?=E7=BC=96=E7=A0=81=E7=9A=84=E9=87=8D=E5=86=99=EF=BC=8C=E9=81=B5?= =?UTF-8?q?=E5=BE=AA=E5=9F=BA=E7=B1=BB=E8=AE=A1=E6=95=B0=E9=80=BB=E8=BE=91?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- GSRL/Device/inc/dvc_motor.hpp | 1 - GSRL/Device/src/dvc_motor.cpp | 13 +++++-------- 2 files changed, 5 insertions(+), 9 deletions(-) diff --git a/GSRL/Device/inc/dvc_motor.hpp b/GSRL/Device/inc/dvc_motor.hpp index 80240ae..f5a5c23 100644 --- a/GSRL/Device/inc/dvc_motor.hpp +++ b/GSRL/Device/inc/dvc_motor.hpp @@ -342,7 +342,6 @@ class MotorDeepJ60 : public Motor protected: bool decodeCanRxMessage(const can_rx_message_t &rxMessage) override; void convertControllerOutputToMotorControlData() override; - fp32 updateCurrentRevolutions() override { return m_currentRevolutions; } // 构造 CAN ID: (cmdIdx << 5) | jointField static constexpr uint16_t makeCanId(uint8_t jointField, uint8_t cmdIdx) diff --git a/GSRL/Device/src/dvc_motor.cpp b/GSRL/Device/src/dvc_motor.cpp index 7ed6715..6e95b80 100644 --- a/GSRL/Device/src/dvc_motor.cpp +++ b/GSRL/Device/src/dvc_motor.cpp @@ -548,7 +548,7 @@ const uint8_t *MotorGM6020::getMergedControlData(MotorGM6020 &otherMotor) } memcpy(m_mergedData, selfData, 8); - uint8_t offset = (uint8_t)(((otherMotor.m_djiMotorID - 1) & 0x3) * 2); + uint8_t offset = (uint8_t)(((otherMotor.m_djiMotorID - 1) & 0x3) * 2); m_mergedData[offset] = otherData[offset]; m_mergedData[offset + 1] = otherData[offset + 1]; return m_mergedData; @@ -731,8 +731,8 @@ MotorDMmulti::MotorDMmulti(uint8_t dmMotorID, Controller *controller, uint16_t e */ void MotorDMmulti::convertControllerOutputToMotorControlData() { - int16_t giveControlValue = (int16_t)m_controllerOutput; // 控制电流标幺值 - uint8_t offset = (uint8_t)(((m_dmMotorID - 1) & 0x3) * 2); + int16_t giveControlValue = (int16_t)m_controllerOutput; // 控制电流标幺值 + uint8_t offset = (uint8_t)(((m_dmMotorID - 1) & 0x3) * 2); m_motorControlData[offset] = (uint8_t)(giveControlValue & 0xFF); // 低 8 位 m_motorControlData[offset + 1] = (uint8_t)((giveControlValue >> 8) & 0xFF); // 高 8 位 } @@ -797,7 +797,7 @@ const uint8_t *MotorDMmulti::getMergedControlData(MotorDMmulti &otherMotor) } memcpy(m_mergedData, selfData, 8); - uint8_t offset = (uint8_t)(((otherMotor.m_dmMotorID - 1) & 0x3) * 2); + uint8_t offset = (uint8_t)(((otherMotor.m_dmMotorID - 1) & 0x3) * 2); m_mergedData[offset] = otherData[offset]; m_mergedData[offset + 1] = otherData[offset + 1]; return m_mergedData; @@ -1093,7 +1093,7 @@ void MotorDeepJ60::clearError() // 复位硬件状态为 DISABLED，下一次 convert 会先发 ERROR_RESET 再按 intent 走 ENABLE 流程。 // 这里直接构造一次 ERROR_RESET header，由 convert 中的状态判断兜底。 m_pendingErrorReset = true; - m_state = J60_DISABLED; + m_state = J60_DISABLED; } /** @@ -1308,10 +1308,7 @@ bool MotorDeepJ60::decodeCanRxMessage(const can_rx_message_t &rxMessage) fp32 decodedTempC = GSRLMath::convertUintToFloat((int)tempRaw, J60_TEMP_MIN, J60_TEMP_MAX, 7); - // 维持基类不变式：m_currentAngle ∈ [0, 2PI) m_currentAngle = GSRLMath::normalizeAngle(decodedPosRad); - m_lastAngle = m_currentAngle; // 防止基类 updateCurrentRevolutions 误累积 (本类已 override) - m_currentRevolutions = decodedPosRad / (2.0f * MATH_PI); m_currentAngularVelocity = decodedVelRadps; m_currentTorqueNm = decodedTorNm; // 兼容基类 getCurrentTorqueCurrent()：以 mNm 存储，钳位到 int16 范围 (±32767)； From 30ab2fbcc4b1718d25b4e43d0e95f9c20db936a2 Mon Sep 17 00:00:00 2001 From: DoveXY Date: Fri, 24 Apr 2026 16:40:22 +0800 Subject: [PATCH 5/5] =?UTF-8?q?=E6=92=A4=E9=94=80=E6=9B=B4=E6=94=B9?= =?UTF-8?q?=EF=BC=8C=E4=BF=9D=E6=8C=81motor=E5=9F=BA=E7=B1=BB=E4=B8=8D?= =?UTF-8?q?=E5=8F=98?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- GSRL/Device/inc/dvc_motor.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/GSRL/Device/inc/dvc_motor.hpp b/GSRL/Device/inc/dvc_motor.hpp index f5a5c23..4092b60 100644 --- a/GSRL/Device/inc/dvc_motor.hpp +++ b/GSRL/Device/inc/dvc_motor.hpp @@ -103,7 +103,7 @@ class Motor Motor(uint32_t canControlID, uint32_t canFeedbackID, Controller *controller, uint16_t encoderOffset = 0); virtual bool decodeCanRxMessage(const can_rx_message_t &rxMessage) = 0; virtual void convertControllerOutputToMotorControlData() = 0; - virtual fp32 updateCurrentRevolutions(); + fp32 updateCurrentRevolutions(); inline void increaseMotorFeedbackErrorCount(); inline void clearMotorFeedbackErrorCount(); };