robofish/rm_balance
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

修领空

Browse Source
This commit is contained in:
Robofish 2025-09-02 15:06:58 +08:00
parent 38d4139f06
commit 6bb0e96aa8
11 changed files with 166 additions and 152 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
MDK-ARM/DevC.uvprojx
View File

@ -809,6 +809,11 @@
<FileType>1</FileType>
<FilePath>..\User\component\user_math.c</FilePath>
</File>
<File>
<FileName>kinematics.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\component\kinematics.c</FilePath>
</File>
</Files>
</Group>
<Group>

1
MDK-ARM/DevC/DevC.lnp
View File

@ -75,6 +75,7 @@
"devc\limiter.o"
"devc\pid.o"
"devc\user_math.o"
"devc\kinematics.o"
"devc\buzzer.o"
"devc\dm_imu.o"
"devc\dr16.o"

51
User/component/kinematics.c Normal file
View File

@ -0,0 +1,51 @@
/*
一些运动学相关的计算函数
*/
#include "component/kinematics.h"
#include "component/user_math.h"
/*串联腿单腿转摆动杆正运动学*/
/*大腿小腿均按照水平为0点，向下为正方向*/
/**
* @brief 串联腿单腿转摆动杆正运动学
* @param param 腿部长度参数
* @param hip_angle 髋关节角度(弧度)
* @param knee_angle 膝关节角度(弧度)
* @param angle 计算到的等效摆动杆角度
* @param heigh 计算到的等效摆动杆高度
* @return
*/
int8_t KIN_SerialLeg_FK(const KIN_SerialLeg_Param_t *param, const float *hip_angle,const float *knee_angle, float *angle, float *height) {
float L1 = param->thigh_length;
float L2 = param->calf_length;
float q1 = *hip_angle;
float q2 = *knee_angle;
*height = sqrt(L1*L1 + L2*L2 - 2*L1*L2*cos(q1+q2));
*angle = acos((L1*L1 + (*height)*(*height) - L2*L2)/(2*L1*(*height))) + q1;
return 0; // 成功
}
/*串联腿单腿转摆动杆逆运动学*/
/*大腿小腿均按照水平为0点，向下为正方向*/
/**
* @brief 串联腿单腿转摆动杆逆运动学
* @param param 腿部长度参数
* @param angle 输入的等效摆动杆角度
* @param height 输入的等效摆动杆高度
* @param hip_angle 计算到的髋关节角度(弧度)
* @param knee_angle 计算到的膝关节角度(弧度)
* @return
*/
int8_t KIN_SerialLeg_IK(const KIN_SerialLeg_Param_t *param, const float *angle,const float *height, float *hip_angle, float *knee_angle) {
float L1 = param->thigh_length;
float L2 = param->calf_length;
float q = *angle;
float h = *height;
if(h > (L1 + L2) || h < fabs(L1 - L2)) {
return -1; // 无解
}
*hip_angle = acos((L1*L1 + h*h - L2*L2)/(2*L1*h)) + q;
*knee_angle = acos((L1*L1 + L2*L2 - h*h)/(2*L1*L2)) - M_PI;
return 0; // 成功
}

43
User/component/kinematics.h Normal file
View File

@ -0,0 +1,43 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#include <stdbool.h>
#include <math.h>
/**
* @brief 串联腿参数结构体
*/
typedef struct {
float thigh_length; // 大腿长度
float calf_length; // 小腿长度
} KIN_SerialLeg_Param_t; // ← 补上分号
/**
* @brief 串联腿单腿转摆动杆正运动学
* @param param 腿部长度参数
* @param hip_angle 髋关节角度(弧度)
* @param knee_angle 膝关节角度(弧度)
* @param angle 计算到的等效摆动杆角度
* @param height 计算到的等效摆动杆高度
* @return 0:成功, -1:失败
*/
int8_t KIN_SerialLeg_FK(const KIN_SerialLeg_Param_t *param, const float *hip_angle, const float *knee_angle, float *angle, float *height);
/**
* @brief 串联腿单腿转摆动杆逆运动学
* @param param 腿部长度参数
* @param angle 输入的等效摆动杆角度
* @param height 输入的等效摆动杆高度
* @param hip_angle 计算到的髋关节角度(弧度)
* @param knee_angle 计算到的膝关节角度(弧度)
* @return 0:成功, -1:失败
*/
int8_t KIN_SerialLeg_IK(const KIN_SerialLeg_Param_t *param, const float *angle, const float *height, float *hip_angle, float *knee_angle);
#ifdef __cplusplus
}
#endif

10
User/device/motor_lk.c
View File

@ -147,7 +147,7 @@ int8_t MOTOR_LK_Register(MOTOR_LK_Param_t *param) {
// 对于某些LK电机，反馈数据可能通过命令ID发送
// 根据实际测试，使用命令ID接收反馈数据
uint16_t feedback_id = 0x140 + param->id; // 使用命令ID作为反馈ID
uint16_t feedback_id = param->id; // 使用命令ID作为反馈ID
// 注册CAN接收ID
if (BSP_CAN_RegisterId(param->can, feedback_id, 3) != BSP_OK) {
@ -170,7 +170,7 @@ int8_t MOTOR_LK_Update(MOTOR_LK_Param_t *param) {
MOTOR_LK_t *motor = manager->motors[i];
if (motor && motor->param.id == param->id) {
// 对于某些LK电机，反馈数据通过命令ID发送
uint16_t feedback_id = 0x140 + param->id;
uint16_t feedback_id = param->id;
BSP_CAN_Message_t rx_msg;
if (BSP_CAN_GetMessage(param->can, feedback_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
@ -227,7 +227,7 @@ int8_t MOTOR_LK_SetOutput(MOTOR_LK_Param_t *param, float value) {
// 构建CAN帧（根据协议：命令报文标识符 = 0x140 + ID）
BSP_CAN_StdDataFrame_t tx_frame;
tx_frame.id = 0x140 + param->id;
tx_frame.id = param->id;
tx_frame.dlc = MOTOR_TX_BUF_SIZE;
// 设置转矩闭环控制命令数据
@ -253,7 +253,7 @@ int8_t MOTOR_LK_MotorOn(MOTOR_LK_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
BSP_CAN_StdDataFrame_t tx_frame;
tx_frame.id = 0x140 + param->id;
tx_frame.id = param->id;
tx_frame.dlc = MOTOR_TX_BUF_SIZE;
// 电机运行命令
@ -273,7 +273,7 @@ int8_t MOTOR_LK_MotorOff(MOTOR_LK_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
BSP_CAN_StdDataFrame_t tx_frame;
tx_frame.id = 0x140 + param->id;
tx_frame.id = param->id;
tx_frame.dlc = MOTOR_TX_BUF_SIZE;
// 电机关闭命令

174
User/device/motor_lz.c
View File

@ -27,7 +27,7 @@
#define LZ_RAW_VALUE_MAX (65535) /* 16位原始值最大值 */
#define LZ_TEMP_SCALE (10.0f) /* 温度缩放因子 */
#define LZ_MAX_RECOVER_DIFF_RAD (0.4f)
#define LZ_MAX_RECOVER_DIFF_RAD (0.25f)
#define MOTOR_TX_BUF_SIZE (8)
#define MOTOR_RX_BUF_SIZE (8)
@ -36,7 +36,7 @@
MOTOR_LZ_MotionParam_t lz_recover_param = {
.target_angle = 0.0f,
.target_velocity = 0.0f,
.kp = 20.0f,
.kp = 15.0f,
.kd = 1.0f,
.torque = 0.0f,
};
@ -182,56 +182,47 @@ static uint32_t MOTOR_LZ_IdParser(uint32_t original_id, BSP_CAN_FrameType_t fram
*/
static void MOTOR_LZ_Decode(MOTOR_LZ_t *motor, BSP_CAN_Message_t *msg) {
if (motor == NULL || msg == NULL) return;
// 检查是否为反馈数据帧 (通信类型2)
// 需要使用原始ID来解析，因为parsed_id已经被IdParser处理过了
uint8_t cmd_type = (msg->original_id >> 24) & 0x1F;
if (cmd_type != MOTOR_LZ_CMD_FEEDBACK) return;
// 解析原始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: 模式状态
// 更新电机CAN ID
uint8_t motor_can_id = id_data2 & 0xFF;
uint8_t fault_info = (id_data2 >> 8) & 0x3F;
uint8_t mode_state = (id_data2 >> 14) & 0x03;
motor->lz_feedback.motor_can_id = motor_can_id;
// 解析故障信息
motor->lz_feedback.fault.under_voltage = (fault_info & 0x01) != 0; // bit16
motor->lz_feedback.fault.driver_fault = (fault_info & 0x02) != 0; // bit17
motor->lz_feedback.fault.over_temp = (fault_info & 0x04) != 0; // bit18
motor->lz_feedback.fault.encoder_fault = (fault_info & 0x08) != 0; // bit19
motor->lz_feedback.fault.stall_overload = (fault_info & 0x10) != 0; // bit20
motor->lz_feedback.fault.uncalibrated = (fault_info & 0x20) != 0; // bit21
// 解析模式状态
motor->lz_feedback.fault.under_voltage = (fault_info & 0x01) != 0;
motor->lz_feedback.fault.driver_fault = (fault_info & 0x02) != 0;
motor->lz_feedback.fault.over_temp = (fault_info & 0x04) != 0;
motor->lz_feedback.fault.encoder_fault = (fault_info & 0x08) != 0;
motor->lz_feedback.fault.stall_overload = (fault_info & 0x10) != 0;
motor->lz_feedback.fault.uncalibrated = (fault_info & 0x20) != 0;
motor->lz_feedback.state = (MOTOR_LZ_State_t)mode_state;
// 解析数据区
// Byte0~1: 当前角度 (高字节在前，低字节在后)
// 反馈解码并自动反向
uint16_t raw_angle = (uint16_t)((msg->data[0] << 8) | msg->data[1]);
motor->lz_feedback.current_angle = MOTOR_LZ_RawToFloat(raw_angle, LZ_ANGLE_RANGE_RAD);
// Byte2~3: 当前角速度 (高字节在前，低字节在后)
float angle = MOTOR_LZ_RawToFloat(raw_angle, LZ_ANGLE_RANGE_RAD);
uint16_t raw_velocity = (uint16_t)((msg->data[2] << 8) | msg->data[3]);
motor->lz_feedback.current_velocity = MOTOR_LZ_RawToFloat(raw_velocity, LZ_VELOCITY_RANGE_RAD_S);
// Byte4~5: 当前力矩 (高字节在前，低字节在后)
float velocity = MOTOR_LZ_RawToFloat(raw_velocity, LZ_VELOCITY_RANGE_RAD_S);
uint16_t raw_torque = (uint16_t)((msg->data[4] << 8) | msg->data[5]);
motor->lz_feedback.current_torque = MOTOR_LZ_RawToFloat(raw_torque, LZ_TORQUE_RANGE_NM);
// Byte6~7: 当前温度 (温度*10) (高字节在前，低字节在后)
float torque = MOTOR_LZ_RawToFloat(raw_torque, LZ_TORQUE_RANGE_NM);
// 自动反向
if (motor->param.reverse) {
angle = -angle;
velocity = -velocity;
torque = -torque;
}
motor->lz_feedback.current_angle = angle;
motor->lz_feedback.current_velocity = velocity;
motor->lz_feedback.current_torque = torque;
uint16_t raw_temp = (uint16_t)((msg->data[6] << 8) | msg->data[7]);
motor->lz_feedback.temperature = (float)raw_temp / LZ_TEMP_SCALE;
// 更新通用电机反馈信息
motor->motor.feedback.rotor_abs_angle = motor->lz_feedback.current_angle;
motor->motor.feedback.rotor_speed = motor->lz_feedback.current_velocity * 180.0f / M_PI * 6.0f; // 转换为RPM
motor->motor.feedback.torque_current = motor->lz_feedback.current_torque; // 使用力矩作为电流反馈
motor->motor.feedback.rotor_abs_angle = angle;
motor->motor.feedback.rotor_speed = velocity * 180.0f / M_PI * 6.0f;
motor->motor.feedback.torque_current = torque;
motor->motor.feedback.temp = (int8_t)motor->lz_feedback.temperature;
// 更新在线状态
motor->motor.header.online = true;
motor->motor.header.last_online_time = BSP_TIME_Get();
}
@ -344,49 +335,39 @@ int8_t MOTOR_LZ_UpdateAll(void) {
int8_t MOTOR_LZ_MotionControl(MOTOR_LZ_Param_t *param, MOTOR_LZ_MotionParam_t *motion_param) {
if (param == NULL || motion_param == NULL) return DEVICE_ERR_NULL;
MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param);
if (motor == NULL) return DEVICE_ERR_NO_DEV;
// 更新运控参数
// 自动反向控制
MOTOR_LZ_MotionParam_t send_param = *motion_param;
if (param->reverse) {
send_param.target_angle = -send_param.target_angle;
send_param.target_velocity = -send_param.target_velocity;
send_param.torque = -send_param.torque;
}
memcpy(&motor->motion_param, motion_param, sizeof(MOTOR_LZ_MotionParam_t));
// 根据协议，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
uint16_t raw_torque = MOTOR_LZ_FloatToRaw(send_param.torque, LZ_TORQUE_RANGE_NM);
uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_MOTION, raw_torque, param->motor_id);
// 准备8字节数据区
uint8_t data[8];
// 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: 目标角速度 [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 [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 [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; // 低字节
uint16_t raw_angle = MOTOR_LZ_FloatToRaw(send_param.target_angle, LZ_ANGLE_RANGE_RAD);
data[0] = (raw_angle >> 8) & 0xFF;
data[1] = raw_angle & 0xFF;
uint16_t raw_velocity = MOTOR_LZ_FloatToRaw(send_param.target_velocity, LZ_VELOCITY_RANGE_RAD_S);
data[2] = (raw_velocity >> 8) & 0xFF;
data[3] = raw_velocity & 0xFF;
uint16_t raw_kp = MOTOR_LZ_FloatToRawPositive(send_param.kp, LZ_KP_MAX);
data[4] = (raw_kp >> 8) & 0xFF;
data[5] = raw_kp & 0xFF;
uint16_t raw_kd = MOTOR_LZ_FloatToRawPositive(send_param.kd, LZ_KD_MAX);
data[6] = (raw_kd >> 8) & 0xFF;
data[7] = raw_kd & 0xFF;
return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
}
int8_t MOTOR_LZ_Enable(MOTOR_LZ_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
@ -462,47 +443,6 @@ static 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);
}
int8_t MOTOR_LZ_RecoverToZero(MOTOR_LZ_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;

25
User/device/motor_lz.h
View File

@ -145,31 +145,6 @@ 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 电机参数

1
User/module/balance_chassis.c
View File

@ -108,7 +108,6 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd){
MOTOR_LZ_Relax(&c->param->joint_motors[0]);
MOTOR_LZ_Relax(&c->param->joint_motors[1]);
MOTOR_LZ_Relax(&c->param->joint_motors[2]);
BSP_TIME_Delay_us(100); // 确保有足够时间发送CAN消息
MOTOR_LZ_Relax(&c->param->joint_motors[3]);
MOTOR_LK_Relax(&c->param->wheel_motors[0]);
MOTOR_LK_Relax(&c->param->wheel_motors[1]);

4
User/module/config.c
View File

@ -128,13 +128,13 @@ Config_RobotParam_t robot_config = {
.wheel_motors = {
{ // 左轮电机
.can = BSP_CAN_1,
.id = 1,
.id = 0x141,
.module = MOTOR_LK_MF9025,
.reverse = false,
},
{ // 右轮电机
.can = BSP_CAN_1,
.id = 2,
.id = 0x142,
.module = MOTOR_LK_MF9025,
.reverse = true,
},

2
User/task/config.yaml
View File

@ -19,7 +19,7 @@
function: Task_atti_esti
name: atti_esti
stack: 256
- delay: 0
- delay: 500
description: ''
freq_control: true
frequency: 500.0

2
User/task/user_task.h
View File

@ -23,7 +23,7 @@ extern "C" {
#define BLINK_INIT_DELAY (0)
#define RC_INIT_DELAY (0)
#define ATTI_ESTI_INIT_DELAY (0)
#define CTRL_CHASSIS_INIT_DELAY (0)
#define CTRL_CHASSIS_INIT_DELAY (500)
/* Exported defines --------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */