arm/User/module/joint.hpp

/**
 * @file joint.hpp
 * @brief 关节类 - 封装单个关节的所有功能
 */

#pragma once

#include <stdint.h>
#include <cmath>
#include "motor_base.hpp"
#include "component/arm_kinematics/arm6dof.h"

namespace arm {

// ============================================================================
// Joint 类 - 关节对象
// ============================================================================
class Joint {
private:
    uint8_t id_;                    // 关节编号
    IMotor* motor_;                 // 电机指针（多态）
    Arm6dof_DHParams_t dh_params_;  // DH参数
    float q_offset_;                // 零位偏移
    
    // 状态
    struct {
        float current_angle;
        float current_velocity;
        float current_torque;
        float target_angle;
        float feedforward_torque;  // 前馈力矩（如重力补偿）
        bool online;
    } state_;
    
public:
    // 构造函数
    Joint(uint8_t id, IMotor* motor, const Arm6dof_DHParams_t& dh_params, 
          float q_offset, float freq)
        : id_(id), motor_(motor), dh_params_(dh_params), q_offset_(q_offset) {
                
        state_.current_angle = 0.0f;
        state_.current_velocity = 0.0f;
        state_.current_torque = 0.0f;
        state_.target_angle = 0.0f;
        state_.feedforward_torque = 0.0f;
        state_.online = false;
    }
    
    // 禁止拷贝（电机资源唯一）
    Joint(const Joint&) = delete;
    Joint& operator=(const Joint&) = delete;
    
    // Getters
    uint8_t GetId() const { return id_; }
    float GetCurrentAngle() const { return state_.current_angle; }
    float GetCurrentVelocity() const { return state_.current_velocity; }
    float GetTargetAngle() const { return state_.target_angle; }
    bool IsOnline() const { return state_.online; }
    const Arm6dof_DHParams_t& GetDHParams() const { return dh_params_; }
    float GetOffset() const { return q_offset_; }
    
    // Setters
    void SetOffset(float offset) { q_offset_ = offset; }
    
    void SetTargetAngle(float target) { 
        state_.target_angle = target; 
    }
    
    void SetFeedforwardTorque(float torque) {
        state_.feedforward_torque = torque;
    }
    
    float GetFeedforwardTorque() const { return state_.feedforward_torque; }
    
    int8_t Register() {
        return motor_->Register();
    }
    
    int8_t Enable() {
        return motor_->Enable();
    }
    
    int8_t Update() {
        int8_t ret = motor_->Update();
        
        float raw = motor_->GetAngle() - q_offset_;

        // 多圈关节（运动范围 > 2π，如 J1/J4/J6）：直接透传多圈绝对值，不截断
        // 单圈关节（运动范围 ≤ 2π，如 J2/J3/J5）：归一化到 [-π, π]
        //   目的：消除 DM 电机 0~2π 单圈编码在过零点时的跳变
        //   零点不变：raw=0 时不触发任何折叠
        //   注意：j3 范围 [0,3]、j5 范围 [-1.75,1.75] 均在 [-π,π] 内，
        //         归一化后其实际运动段不会碰到 ±π 边界，计算安全
        const bool multi_turn = (dh_params_.qmax - dh_params_.qmin) > 2.0f * M_PI;
        if (!multi_turn) {
            if (raw >  M_PI) raw -= 2.0f * M_PI;
            if (raw < -M_PI) raw += 2.0f * M_PI;
        }

        state_.current_angle    = raw;
        state_.current_velocity = motor_->GetVelocity();
        state_.current_torque   = motor_->GetTorque();
        state_.online           = motor_->IsOnline();
        
        return ret;
    }
    
    int8_t Relax() {
        return motor_->Relax();
    }
    
    // 位置控制
    int8_t PositionControl(float target_angle, float dt) {
        // 检查限位
        if (target_angle < dh_params_.qmin || target_angle > dh_params_.qmax) {
            return -1;
        }
        
        state_.target_angle = target_angle;
      
        // 总前馈力矩 = 重力补偿等前馈力矩
        float total_feedforward = state_.feedforward_torque;
        
        // MIT控制参数：优先使用DH参数中配置的kp/kd，否则使用默认值
        float kp = dh_params_.kp;
        float kd = dh_params_.kd;
        
        // 如果未配置（=0），使用默认值（按电机类型）
        if (kp <= 0.0f) kp = (id_ < 3) ? 10.0f : 50.0f;  // LZ vs DM默认值
        if (kd <= 0.0f) kd = (id_ < 3) ? 0.5f : 3.0f;
        
        return motor_->PositionControl(
            target_angle, 
            0.0f,                      // 速度
            kp, kd,                    // 刚度和阻尼
            total_feedforward          // 前馈力矩（重力补偿）
        );
    }
    
    // 纯力矩控制
    // @param torque: 输出力矩（N·m）
    // @return: 0=成功，-1=失败
    int8_t TorqueControl(float torque) {
        state_.feedforward_torque = torque;  // 存储当前前馈力矩
        return motor_->PositionControl(
            state_.current_angle,  // 保持当前位置
            0.0f,                              // 速度
            0.0f, 0.0f,                        // kp=0, kd=0（无刚度无阻尼）
            torque                             // 纯力矩
        );
    }
    
    // 检查是否到达目标
    bool IsReached(float tolerance) const {
        return std::fabs(state_.target_angle - state_.current_angle) < tolerance;
    }
};

} // namespace arm