robofish/MRobot
1
0
Fork 0
mirror of https://github.com/goldenfishs/MRobot.git synced 2025-09-14 12:54:33 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

修灵足

Browse Source
This commit is contained in:
Robofish 2025-08-31 15:41:05 +08:00
parent 70d3add6da
commit 22498e9a4f
3 changed files with 133 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
assets/User_code/device/motor.h
View File

@ -17,6 +17,24 @@ typedef struct {
float temp; /* 温度 */
} MOTOR_Feedback_t;
/**
* @brief mit电机输出参数结构体
*/
typedef struct {
float torque; /* 目标力矩 */
float velocity; /* 目标速度 */
float angle; /* 目标位置 */
float kp; /* 位置环增益 */
float kd; /* 速度环增益 */
} MOTOR_MIT_Output_t;
/**
* @brief 转矩电流控制模式参数结构体
*/
typedef struct {
float current; /* 目标电流 */
} MOTOR_Current_Output_t;
typedef struct {
DEVICE_Header_t header;
bool reverse; /* 是否反装 true表示反装 */

103
assets/User_code/device/motor_lz.c
View File

@ -82,7 +82,7 @@ static uint32_t MOTOR_LZ_BuildExtID(MOTOR_LZ_CmdType_t cmd_type, uint16_t data2,
}
/**
* @brief 浮点值转换为原始值
* @brief 浮点值转换为原始值（对称范围：-max_value ~ +max_value）
*/
static uint16_t MOTOR_LZ_FloatToRaw(float value, float max_value) {
// 限制范围
@ -93,6 +93,18 @@ static uint16_t MOTOR_LZ_FloatToRaw(float value, float max_value) {
return (uint16_t)((value + max_value) / (2.0f * max_value) * (float)LZ_RAW_VALUE_MAX);
}
/**
* @brief 浮点值转换为原始值（单向范围：0 ~ +max_value）
*/
static uint16_t MOTOR_LZ_FloatToRawPositive(float value, float max_value) {
// 限制范围
if (value > max_value) value = max_value;
if (value < 0.0f) value = 0.0f;
// 转换为0~65535范围，对应0~max_value
return (uint16_t)(value / max_value * (float)LZ_RAW_VALUE_MAX);
}
/**
* @brief 原始值转换为浮点值
*/
@ -137,7 +149,7 @@ static uint32_t MOTOR_LZ_IdParser(uint32_t original_id, BSP_CAN_FrameType_t fram
// 解析扩展ID各个字段
uint8_t cmd_type = (original_id >> 24) & 0x1F; // Bit28~24: 通信类型
uint16_t data2 = (original_id >> 8) & 0xFFFF; // Bit23~8: 数据区2
uint8_t target_id = original_id & 0xFF; // Bit7~0: 目标地址
uint8_t host_id = (uint8_t)(original_id & 0xFF); // Bit7~0: 主机CAN ID
// 对于反馈数据帧，我们使用特殊的解析规则
if (cmd_type == MOTOR_LZ_CMD_FEEDBACK) {
@ -145,11 +157,11 @@ static uint32_t MOTOR_LZ_IdParser(uint32_t original_id, BSP_CAN_FrameType_t fram
// Bit8~15: 当前电机CAN ID
// Bit16~21: 故障信息
// Bit22~23: 模式状态
uint8_t motor_can_id = data2 & 0xFF;
uint8_t motor_can_id = data2 & 0xFF; // bit8~15: 当前电机CAN ID
// 返回格式化的ID，便于匹配
// 格式：0x02HHMMTT (02=反馈命令, HH=主机ID, MM=电机ID, TT=目标ID)
return (0x02000000) | ((data2 & 0xFF00) << 8) | (motor_can_id << 8) | target_id;
// 格式：0x02HHMMTT (02=反馈命令, HH=主机ID, MM=电机ID, TT=主机ID)
return (0x02000000) | (host_id << 16) | (motor_can_id << 8) | host_id;
}
// 对于其他命令类型，直接返回原始ID
@ -163,11 +175,12 @@ static void MOTOR_LZ_Decode(MOTOR_LZ_t *motor, BSP_CAN_Message_t *msg) {
if (motor == NULL || msg == NULL) return;
// 检查是否为反馈数据帧 (通信类型2)
uint8_t cmd_type = (msg->parsed_id >> 24) & 0x1F;
// 需要使用原始ID来解析，因为parsed_id已经被IdParser处理过了
uint8_t cmd_type = (msg->original_id >> 24) & 0x1F;
if (cmd_type != MOTOR_LZ_CMD_FEEDBACK) return;
// 解析ID中的信息
uint16_t id_data2 = (msg->parsed_id >> 8) & 0xFFFF;
// 解析原始ID中的数据区2 (bit23~8)
uint16_t id_data2 = (msg->original_id >> 8) & 0xFFFF;
uint8_t motor_can_id = id_data2 & 0xFF; // Bit8~15: 当前电机CAN ID
uint8_t fault_info = (id_data2 >> 8) & 0x3F; // Bit16~21: 故障信息
uint8_t mode_state = (id_data2 >> 14) & 0x03; // Bit22~23: 模式状态
@ -263,8 +276,9 @@ int8_t MOTOR_LZ_Register(MOTOR_LZ_Param_t *param) {
new_motor->motor.reverse = param->reverse;
// 注册CAN接收ID - 使用解析后的反馈数据ID
// 构建原始扩展ID
uint32_t original_feedback_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_FEEDBACK, param->host_id, param->motor_id);
// 构建反馈数据的原始扩展ID
// 反馈数据：data2包含电机ID(bit8~15)，target_id是主机ID
uint32_t original_feedback_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_FEEDBACK, param->motor_id, param->host_id);
// 通过ID解析器得到解析后的ID
uint32_t parsed_feedback_id = MOTOR_LZ_IdParser(original_feedback_id, BSP_CAN_FRAME_EXT_DATA);
@ -288,7 +302,7 @@ int8_t MOTOR_LZ_Update(MOTOR_LZ_Param_t *param) {
MOTOR_LZ_t *motor = manager->motors[i];
if (motor && motor->param.motor_id == param->motor_id) {
// 获取反馈数据 - 使用解析后的ID
uint32_t original_feedback_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_FEEDBACK, param->host_id, param->motor_id);
uint32_t original_feedback_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_FEEDBACK, param->motor_id, param->host_id);
uint32_t parsed_feedback_id = MOTOR_LZ_IdParser(original_feedback_id, BSP_CAN_FRAME_EXT_DATA);
BSP_CAN_Message_t msg;
@ -328,29 +342,36 @@ int8_t MOTOR_LZ_MotionControl(MOTOR_LZ_Param_t *param, MOTOR_LZ_MotionParam_t *m
// 更新运控参数
memcpy(&motor->motion_param, motion_param, sizeof(MOTOR_LZ_MotionParam_t));
// 构建扩展ID
uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_MOTION, 0, param->motor_id);
// 根据协议，bit23~8数据区2包含力矩信息
// 力矩范围：-60Nm~60Nm 对应 0~65535
uint16_t raw_torque = MOTOR_LZ_FloatToRaw(motion_param->torque, LZ_TORQUE_RANGE_NM);
// 准备数据
// 构建扩展ID - 运控模式控制指令
// bit28~24: 0x1 (运控模式)
// bit23~8: 力矩数据 (0~65535)，协议中描述为"Byte2:力矩"
// bit7~0: 目标电机CAN_ID
uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_MOTION, raw_torque, param->motor_id);
// 准备8字节数据区
uint8_t data[8];
// Byte0~1: 目标角度 (高字节在前，低字节在后)
// Byte0~1: 目标角度 [0~65535] 对应 (-12.57f~12.57f rad) (高字节在前，低字节在后)
uint16_t raw_angle = MOTOR_LZ_FloatToRaw(motion_param->target_angle, LZ_ANGLE_RANGE_RAD);
data[0] = (raw_angle >> 8) & 0xFF; // 高字节
data[1] = raw_angle & 0xFF; // 低字节
// Byte2~3: 目标角速度 (高字节在前，低字节在后)
// Byte2~3: 目标角速度 [0~65535] 对应 (-20rad/s~20rad/s) (高字节在前，低字节在后)
uint16_t raw_velocity = MOTOR_LZ_FloatToRaw(motion_param->target_velocity, LZ_VELOCITY_RANGE_RAD_S);
data[2] = (raw_velocity >> 8) & 0xFF; // 高字节
data[3] = raw_velocity & 0xFF; // 低字节
// Byte4~5: Kp (高字节在前，低字节在后)
uint16_t raw_kp = MOTOR_LZ_FloatToRaw(motion_param->kp, LZ_KP_MAX);
// Byte4~5: Kp [0~65535] 对应 (0.0~5000.0) (高字节在前，低字节在后)
uint16_t raw_kp = MOTOR_LZ_FloatToRawPositive(motion_param->kp, LZ_KP_MAX);
data[4] = (raw_kp >> 8) & 0xFF; // 高字节
data[5] = raw_kp & 0xFF; // 低字节
// Byte6~7: Kd (高字节在前，低字节在后)
uint16_t raw_kd = MOTOR_LZ_FloatToRaw(motion_param->kd, LZ_KD_MAX);
// Byte6~7: Kd [0~65535] 对应 (0.0~100.0) (高字节在前，低字节在后)
uint16_t raw_kd = MOTOR_LZ_FloatToRawPositive(motion_param->kd, LZ_KD_MAX);
data[6] = (raw_kd >> 8) & 0xFF; // 高字节
data[7] = raw_kd & 0xFF; // 低字节
@ -431,3 +452,45 @@ MOTOR_LZ_Feedback_t* MOTOR_LZ_GetFeedback(MOTOR_LZ_Param_t *param) {
}
return NULL;
}
int8_t MOTOR_LZ_TorqueControl(MOTOR_LZ_Param_t *param, float torque) {
if (param == NULL) return DEVICE_ERR_NULL;
// 创建运控参数，只设置力矩，其他参数为0
MOTOR_LZ_MotionParam_t motion_param = {0};
motion_param.torque = torque;
motion_param.target_angle = 0.0f;
motion_param.target_velocity = 0.0f;
motion_param.kp = 0.0f;
motion_param.kd = 0.0f;
return MOTOR_LZ_MotionControl(param, &motion_param);
}
int8_t MOTOR_LZ_PositionControl(MOTOR_LZ_Param_t *param, float target_angle, float max_velocity) {
if (param == NULL) return DEVICE_ERR_NULL;
// 创建运控参数，设置位置和速度限制
MOTOR_LZ_MotionParam_t motion_param = {0};
motion_param.target_angle = target_angle;
motion_param.target_velocity = max_velocity;
motion_param.torque = 0.0f;
motion_param.kp = 100.0f; // 默认位置增益
motion_param.kd = 5.0f; // 默认微分增益
return MOTOR_LZ_MotionControl(param, &motion_param);
}
int8_t MOTOR_LZ_VelocityControl(MOTOR_LZ_Param_t *param, float target_velocity) {
if (param == NULL) return DEVICE_ERR_NULL;
// 创建运控参数，只设置速度
MOTOR_LZ_MotionParam_t motion_param = {0};
motion_param.target_angle = 0.0f;
motion_param.target_velocity = target_velocity;
motion_param.torque = 0.0f;
motion_param.kp = 0.0f;
motion_param.kd = 1.0f; // 少量阻尼
return MOTOR_LZ_MotionControl(param, &motion_param);
}

32
assets/User_code/device/motor_lz.h
View File

@ -145,6 +145,31 @@ int8_t MOTOR_LZ_UpdateAll(void);
*/
int8_t MOTOR_LZ_MotionControl(MOTOR_LZ_Param_t *param, MOTOR_LZ_MotionParam_t *motion_param);
/**
* @brief 电流(力矩)模式控制电机
* @param param 电机参数
* @param torque 目标力矩 (-60~60 Nm)
* @return 设备状态码
*/
int8_t MOTOR_LZ_TorqueControl(MOTOR_LZ_Param_t *param, float torque);
/**
* @brief 位置模式控制电机
* @param param 电机参数
* @param target_angle 目标角度 (-12.57~12.57 rad)
* @param max_velocity 最大速度 (0~20 rad/s)
* @return 设备状态码
*/
int8_t MOTOR_LZ_PositionControl(MOTOR_LZ_Param_t *param, float target_angle, float max_velocity);
/**
* @brief 速度模式控制电机
* @param param 电机参数
* @param target_velocity 目标速度 (-20~20 rad/s)
* @return 设备状态码
*/
int8_t MOTOR_LZ_VelocityControl(MOTOR_LZ_Param_t *param, float target_velocity);
/**
* @brief 电机使能运行
* @param param 电机参数
@ -188,6 +213,13 @@ int8_t MOTOR_LZ_Relax(MOTOR_LZ_Param_t *param);
*/
int8_t MOTOR_LZ_Offline(MOTOR_LZ_Param_t *param);
/**
* @brief 获取指定电机的反馈信息
* @param param 电机参数
* @return 电机反馈信息指针，失败返回NULL
*/
MOTOR_LZ_Feedback_t* MOTOR_LZ_GetFeedback(MOTOR_LZ_Param_t *param);
#ifdef __cplusplus
}
#endif