/*
  运动学计算
*/

#pragma once

#ifdef __cplusplus
extern "C" {
#endif

#include "user_math.h"

typedef struct {
  float T; // 当前实际电机输出力矩(电机本身的力矩)(Nm)
  float W; // 当前实际电机速度(电机本身的速度)(rad/s)
  float Pos; // 当前电机位置(rad)
} Kinematics_JointData_t;

typedef struct {
  float T;             // 期望关节的输出力矩(电机本身的力矩)(Nm)
  float W;             // 期望关节速度(电机本身的速度)(rad/s)
  float Pos;           // 期望关节位置(rad)
  float K_P;           // 关节刚度系数(0-25.599)
  float K_W;           // 关节速度系数(0-25.599)
} Kinematics_JointCMD_t;


/**
 * @brief 计算实际反馈
 * @param real 减速后关节数据
 * @param in 输入关节数据
 * @param ratio 减速比
 * @return 
 */
int8_t Kinematics_RealFeedback(Kinematics_JointData_t *real, const Kinematics_JointData_t *in, float ratio, float joint_zero);

/**
 * @brief 计算实际输出
 * @param real 减速后关节输出
 * @param out 实际输出关节数据
 * @param ratio 减速比
 * @return 
 */
int8_t Kinematics_RealOutput(const Kinematics_JointCMD_t *real, Kinematics_JointCMD_t *out, float ratio, float joint_zero);



#ifdef __cplusplus
}
#endif