diff --git a/GSRL/Device/inc/dvc_motor.hpp b/GSRL/Device/inc/dvc_motor.hpp index 213fcd9..90d01b1 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(); }; @@ -123,7 +123,9 @@ class MotorGM6020 : public Motor public: MotorGM6020(uint8_t dji6020MotorID, Controller *controller, uint16_t encoderOffset = 0); uint8_t getDjiMotorID() const; - const uint8_t *getMergedControlData(MotorGM6020 &otherMotor); + const uint8_t *getMergedControlData(MotorGM6020 &m2); + const uint8_t *getMergedControlData(MotorGM6020 &m2, MotorGM6020 &m3); + const uint8_t *getMergedControlData(MotorGM6020 &m2, MotorGM6020 &m3, MotorGM6020 &m4); protected: bool decodeCanRxMessage(const can_rx_message_t &rxMessage) override; @@ -209,20 +211,15 @@ using MotorDM2325 = MotorDM4310; * @note 反馈帧: 反馈ID = 0x300 + 电机ID, 数据段为大端字节序 * @note 控制电流为标幺值，含义参见达妙力位混控模式中的 i_des 说明 */ -class MotorDMmulti : public Motor +class MotorDMmulti : public MotorGM6020 { protected: - uint8_t m_dmMotorID; // 达妙电机ID [1,8] - uint16_t m_encoderHistory[2]; // 0:当前值 1:上一次值 - int16_t m_currentRPMSpeed; // 电机速度, 单位RPM(已除以100后的真实值) - uint8_t m_errorState; // 反馈帧D[7]错误状态字节, 具体含义详见达妙错误状态说明书 - uint8_t m_mergedData[8]; // getMergedControlData 输出缓冲区 + uint8_t m_errorState; // 反馈帧D[7]错误状态字节, 具体含义详见达妙错误状态说明书 public: MotorDMmulti(uint8_t dmMotorID, Controller *controller, uint16_t encoderOffset = 0); uint8_t getDmMotorID() const; uint8_t getErrorState() const; - const uint8_t *getMergedControlData(MotorDMmulti &otherMotor); protected: bool decodeCanRxMessage(const can_rx_message_t &rxMessage) override; @@ -261,6 +258,99 @@ 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) + 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] + 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; } + int16_t getMosfetTemperature() const { return m_mosfetTemperature; } + int16_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; + + // 构造 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); +}; + /* Exported constants --------------------------------------------------------*/ /* Exported macro ------------------------------------------------------------*/ diff --git a/GSRL/Device/src/dvc_motor.cpp b/GSRL/Device/src/dvc_motor.cpp index d720a81..74f5eb2 100644 --- a/GSRL/Device/src/dvc_motor.cpp +++ b/GSRL/Device/src/dvc_motor.cpp @@ -534,23 +534,53 @@ uint8_t MotorGM6020::getDjiMotorID() const /** * @brief 合并两个同控制ID的大疆电机CAN控制数据, 同时触发双方掉线检测 - * @param otherMotor 另一个同控制ID的大疆电机 + * @param m2 另一个同控制ID的大疆电机 * @return const uint8_t* 合并后的8字节CAN控制数据 * @note 返回的指针指向内部静态缓冲区, 下次调用会覆盖 */ -const uint8_t *MotorGM6020::getMergedControlData(MotorGM6020 &otherMotor) +const uint8_t *MotorGM6020::getMergedControlData(MotorGM6020 &m2) { - const uint8_t *selfData = this->getMotorControlData(); - const uint8_t *otherData = otherMotor.getMotorControlData(); + const uint8_t *selfData = this->getMotorControlData(); + const uint8_t *m2Data = m2.getMotorControlData(); - if (m_motorControlMessageID != otherMotor.m_motorControlMessageID) { - return selfData; + // 始终以 self 作为基线, 保证 m_mergedData 永远 "至少包含 self", + // 使 3/4-motor 重载的叠加链在任一 ID 不匹配时仍然正确 + memcpy(m_mergedData, selfData, 8); + if (m_motorControlMessageID == m2.m_motorControlMessageID) { + for (uint8_t i = 0; i < 8; ++i) m_mergedData[i] |= m2Data[i]; } + return m_mergedData; +} - memcpy(m_mergedData, selfData, 8); - uint8_t offset = (uint8_t)(((otherMotor.m_djiMotorID - 1) & 0x3) * 2); - m_mergedData[offset] = otherData[offset]; - m_mergedData[offset + 1] = otherData[offset + 1]; +/** + * @brief 合并三个同控制ID的大疆电机CAN控制数据, 同时触发各方掉线检测 + * @return const uint8_t* 合并后的8字节CAN控制数据 + * @note 返回的指针指向内部静态缓冲区, 下次调用会覆盖 + */ +const uint8_t *MotorGM6020::getMergedControlData(MotorGM6020 &m2, MotorGM6020 &m3) +{ + getMergedControlData(m2); // m_mergedData = self | m2 + + const uint8_t *m3Data = m3.getMotorControlData(); + if (m_motorControlMessageID == m3.m_motorControlMessageID) { + for (uint8_t i = 0; i < 8; ++i) m_mergedData[i] |= m3Data[i]; + } + return m_mergedData; +} + +/** + * @brief 合并四个同控制ID的大疆电机CAN控制数据, 同时触发各方掉线检测 + * @return const uint8_t* 合并后的8字节CAN控制数据 + * @note 返回的指针指向内部静态缓冲区, 下次调用会覆盖 + */ +const uint8_t *MotorGM6020::getMergedControlData(MotorGM6020 &m2, MotorGM6020 &m3, MotorGM6020 &m4) +{ + getMergedControlData(m2, m3); // m_mergedData = self | m2 | m3 + + const uint8_t *m4Data = m4.getMotorControlData(); + if (m_motorControlMessageID == m4.m_motorControlMessageID) { + for (uint8_t i = 0; i < 8; ++i) m_mergedData[i] |= m4Data[i]; + } return m_mergedData; } @@ -713,16 +743,13 @@ void MotorDM4310::setMotorZeroPosition() * | 控制ID | 0x3FE | 0x4FE | */ MotorDMmulti::MotorDMmulti(uint8_t dmMotorID, Controller *controller, uint16_t encoderOffset) - : Motor(dmMotorID <= 4 ? 0x3FE : 0x4FE, - 0x300u + dmMotorID, - controller, - encoderOffset), - m_dmMotorID(dmMotorID), - m_currentRPMSpeed(0), + : MotorGM6020(dmMotorID, controller, encoderOffset), m_errorState(0) { - m_encoderHistory[0] = 0; - m_encoderHistory[1] = 0; + // 调用GM6020构造函数后修正为达妙一控四固件的发送和接收ID + m_motorControlMessageID = dmMotorID <= 4 ? 0x3FE : 0x4FE; + m_motorFeedbackMessageID = 0x300u + dmMotorID; + m_motorControlHeader.StdId = m_motorControlMessageID; } /** @@ -731,8 +758,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_djiMotorID - 1) & 0x3) * 2); m_motorControlData[offset] = (uint8_t)(giveControlValue & 0xFF); // 低 8 位 m_motorControlData[offset + 1] = (uint8_t)((giveControlValue >> 8) & 0xFF); // 高 8 位 } @@ -769,7 +796,7 @@ bool MotorDMmulti::decodeCanRxMessage(const can_rx_message_t &rxMessage) */ uint8_t MotorDMmulti::getDmMotorID() const { - return m_dmMotorID; + return m_djiMotorID; } /** @@ -781,28 +808,6 @@ uint8_t MotorDMmulti::getErrorState() const return m_errorState; } -/** - * @brief 合并两个同控制ID的一控四固件达妙电机CAN控制数据, 同时触发双方掉线检测 - * @param otherMotor 另一个同控制ID的达妙一控四电机 - * @return const uint8_t* 合并后的8字节CAN控制数据 - * @note 返回的指针指向内部静态缓冲区, 下次调用会覆盖 - */ -const uint8_t *MotorDMmulti::getMergedControlData(MotorDMmulti &otherMotor) -{ - const uint8_t *selfData = this->getMotorControlData(); - const uint8_t *otherData = otherMotor.getMotorControlData(); - - if (m_motorControlMessageID != otherMotor.m_motorControlMessageID) { - return selfData; - } - - memcpy(m_mergedData, selfData, 8); - 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; -} - /****************************************************************************** * 瓴控MG系列电机类实现 ******************************************************************************/ @@ -1028,3 +1033,308 @@ 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_pendingErrorReset(false), + 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 中的状态判断兜底。 + m_pendingErrorReset = true; + 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; + convertControllerOutputToMotorControlData(); +} + +/** + * @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; + } + + // 一次性 ERROR_RESET 帧插入 (clearError() 设置) + if (m_pendingErrorReset) { + m_pendingErrorReset = false; + buildErrorResetFrame(); + return; + } + + 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 = GSRLMath::normalizeAngle(decodedPosRad); + m_currentAngularVelocity = decodedVelRadps; + m_currentTorqueNm = decodedTorNm; + // 兼容基类 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 = (int16_t)decodedTempC; + m_temperature = (int8_t)tempClamped; + } else { + m_mosfetTemperature = (int16_t)decodedTempC; + m_temperature = (int8_t)tempClamped; + } + 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..88e7058 --- /dev/null +++ b/GSRL/Documentation/Device/MotorDeepJ60.md @@ -0,0 +1,129 @@ +# 云深处 J60 关节电机驱动使用说明 + +版本：v1.0.0 +最后更新日期：2026-04-11 + +--- + +## 1：概述 + +本驱动基于 GSRL 库，为云深处 DeepRobotics J60 关节电机提供 CAN 通信（1Mbps）支持。 + +模块基于 C++ 封装，包含： + +- 电机驱动类 `MotorDeepJ60` +- 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：J60 电机准备工作 + +使用前请调试J60参数： + +1. 使用串口调试助手设置关节 ID（默认为 1 但仍需设置） +2. 标定零位（运行时 CAN 协议不提供设零位命令） +3. 合理设置 CAN 超时时间（建议 200ms 左右），避免掉线后电机失控乱跳 +4. 若需要使用CAN配置电机参数，请发送 `CMD_SAVE_CONFIG` 永久生效 + +## 4：典型用法实例 + +```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); +} +``` + +## 5：注意事项 + +1. **关节 ID**：确保 `MotorJ60` 构造参数与调试工具中设置的 ID 一致，否则 `decodeCanRxMessage` 无法匹配反馈帧，`isMotorConected()` 将一直返回 `false`，目前发现默认ID不一定为1，请务必自行使用串口调试助手进行设置。 +2. **上电顺序**：建议先给控制板上电并启动 CAN，再给 J60 电机上电；驱动器上电默认失能，本驱动构造后默认意图为使能，随即发送 ENABLE 命令，收到应答后进入控制模式。 +3. **控制周期**：建议在 `500Hz ~ 1kHz` 范围内调用闭环函数 + 发送 CAN 帧。控制周期过低会导致位置闭环响应迟钝，过高会占用 CAN 总线带宽影响多电机通信。 +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()` 分别读取，建议在高负载运行时轮询检查。