robofish/MRobot
1
0
Fork 0
mirror of https://github.com/goldenfishs/MRobot.git synced 2025-11-02 04:23:10 +08:00
Code Issues Actions Packages Projects Releases Wiki Activity

修复灵足

Browse Source
This commit is contained in:
Robofish 2025-09-20 00:36:59 +08:00
parent d5d580f384
commit 5ba916c40a
3 changed files with 173 additions and 151 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

102
app/tools/update_cmake_sources.py Normal file
View File

@ -0,0 +1,102 @@
#!/usr/bin/env python3
"""
自动更新CMakeLists.txt中的User源文件列表
这个脚本会扫描User目录下的所有.c文件，并自动更新CMakeLists.txt中的target_sources部分
"""
import os
import re
from pathlib import Path
def find_user_c_files(user_dir):
"""查找User目录下的所有.c文件"""
c_files = []
user_path = Path(user_dir)
if not user_path.exists():
print(f"错误: User目录不存在: {user_dir}")
return []
# 递归查找所有.c文件
for c_file in user_path.rglob("*.c"):
# 获取相对于项目根目录的路径
relative_path = c_file.relative_to(user_path.parent)
c_files.append(str(relative_path))
# 按目录和文件名排序
c_files.sort()
return c_files
def update_cmake_sources(cmake_file, c_files):
"""更新CMakeLists.txt中的源文件列表"""
if not os.path.exists(cmake_file):
print(f"错误: CMakeLists.txt文件不存在: {cmake_file}")
return False
# 读取CMakeLists.txt内容
with open(cmake_file, 'r', encoding='utf-8') as f:
content = f.read()
# 构建新的源文件列表
sources_section = "# Add sources to executable\ntarget_sources(${CMAKE_PROJECT_NAME} PRIVATE\n"
sources_section += " # Add user sources here\n"
# 按目录分组
current_dir = ""
for c_file in c_files:
file_dir = str(Path(c_file).parent)
if file_dir != current_dir:
if current_dir: # 不是第一个目录，添加空行
sources_section += "\n"
sources_section += f" # {file_dir} sources\n"
current_dir = file_dir
sources_section += f" {c_file}\n"
sources_section += ")"
# 使用正则表达式替换target_sources部分
pattern = r'# Add sources to executable\s*\ntarget_sources\(\$\{CMAKE_PROJECT_NAME\}\s+PRIVATE\s*\n.*?\)'
if re.search(pattern, content, re.DOTALL):
new_content = re.sub(pattern, sources_section, content, flags=re.DOTALL)
# 写回文件
with open(cmake_file, 'w', encoding='utf-8') as f:
f.write(new_content)
print("✅ 成功更新CMakeLists.txt中的源文件列表")
return True
else:
print("❌ 错误: 在CMakeLists.txt中找不到target_sources部分")
return False
def main():
"""主函数"""
script_dir = Path(__file__).parent
project_root = script_dir
user_dir = project_root / "User"
cmake_file = project_root / "CMakeLists.txt"
print("🔍 正在扫描User目录下的.c文件...")
c_files = find_user_c_files(user_dir)
if not c_files:
print("⚠️ 警告: 在User目录下没有找到.c文件")
return
print(f"📁 找到 {len(c_files)} 个.c文件:")
for c_file in c_files:
print(f" - {c_file}")
print(f"\n📝 正在更新 {cmake_file}...")
success = update_cmake_sources(cmake_file, c_files)
if success:
print("🎉 更新完成！现在可以重新编译项目了。")
else:
print("💥 更新失败，请检查错误信息。")
if __name__ == "__main__":
main()

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

@ -1,5 +1,10 @@
/*
灵足电机驱动
灵足电机通信协议：
- CAN 2.0通信接口，波特率1Mbps
- 采用扩展帧格式(29位ID)
- ID格式：Bit28~24(通信类型) + Bit23~8(数据区2) + Bit7~0(目标地址)
*/
/* Includes ----------------------------------------------------------------- */
#include "motor_lz.h"
@ -22,17 +27,17 @@
#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.28f)
#define MOTOR_TX_BUF_SIZE (8)
#define MOTOR_RX_BUF_SIZE (8)
/* Private macro ------------------------------------------------------------ */
MOTOR_LZ_MotionParam_t lz_recover_param = {
MOTOR_LZ_MotionParam_t lz_relax_param = {
.target_angle = 0.0f,
.target_velocity = 0.0f,
.kp = 20.0f,
.kd = 1.0f,
.kp = 0.0f,
.kd = 0.0f,
.torque = 0.0f,
};
/* Private typedef ---------------------------------------------------------- */
@ -130,7 +135,6 @@ static int8_t MOTOR_LZ_SendExtFrame(BSP_CAN_t can, uint32_t ext_id, uint8_t *dat
memset(tx_frame.data, 0, dlc);
}
BSP_CAN_WaitTxMailboxEmpty(can, 1); // 等待发送邮箱空闲
return BSP_CAN_TransmitExtDataFrame(can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
@ -178,56 +182,53 @@ 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);
float torque = MOTOR_LZ_RawToFloat(raw_torque, LZ_TORQUE_RANGE_NM);
while (angle <0){
angle += M_2PI;
}
while (angle > M_2PI){
angle -= M_2PI;
}
// 自动反向
if (motor->param.reverse) {
angle = M_2PI - angle;
velocity = -velocity;
torque = -torque;
}
motor->lz_feedback.current_angle = angle;
motor->lz_feedback.current_velocity = velocity;
motor->lz_feedback.current_torque = torque;
// Byte6~7: 当前温度 (温度*10) (高字节在前，低字节在后)
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;
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();
}
@ -340,49 +341,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;
BSP_CAN_WaitForEmptyMailbox(param->can, 1);
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;
@ -438,7 +429,7 @@ MOTOR_LZ_t* MOTOR_LZ_GetMotor(MOTOR_LZ_Param_t *param) {
}
int8_t MOTOR_LZ_Relax(MOTOR_LZ_Param_t *param) {
return MOTOR_LZ_Disable(param, false);
return MOTOR_LZ_MotionControl(param, &lz_relax_param);
}
int8_t MOTOR_LZ_Offline(MOTOR_LZ_Param_t *param) {
@ -457,72 +448,3 @@ 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;
MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param);
if (motor == NULL) return DEVICE_ERR_NO_DEV;
// 获取当前角度
MOTOR_LZ_Feedback_t *feedback = MOTOR_LZ_GetFeedback(param);
if (feedback == NULL) return DEVICE_ERR_NO_DEV;
float current_angle = feedback->current_angle;
// 计算目标角度为0时的最短路径
float angle_diff = -current_angle; // 目标是0，所以差值就是-current_angle
// 限制最大差值，防止过大跳变
if (angle_diff > LZ_MAX_RECOVER_DIFF_RAD) angle_diff = LZ_MAX_RECOVER_DIFF_RAD;
if (angle_diff < -LZ_MAX_RECOVER_DIFF_RAD) angle_diff = -LZ_MAX_RECOVER_DIFF_RAD;
float target_angle = current_angle + angle_diff;
// 创建运控参数，设置位置和速度限制
MOTOR_LZ_MotionParam_t motion_param = lz_recover_param; // 使用预设的恢复参数
motion_param.target_angle = target_angle;
return MOTOR_LZ_MotionControl(param, &motion_param);
}

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

@ -213,8 +213,6 @@ int8_t MOTOR_LZ_Relax(MOTOR_LZ_Param_t *param);
*/
int8_t MOTOR_LZ_Offline(MOTOR_LZ_Param_t *param);
int8_t MOTOR_LZ_RecoverToZero(MOTOR_LZ_Param_t *param);
#ifdef __cplusplus
}
#endif