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-09-01 00:02:40 +08:00
parent 22498e9a4f
commit 485e25fec2
3 changed files with 55 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
assets/User_code/device/motor_lk.c
View File

@ -76,9 +76,16 @@ static int8_t MOTOR_LK_CreateCANManager(BSP_CAN_t can) {
}
static void MOTOR_LK_Decode(MOTOR_LK_t *motor, BSP_CAN_Message_t *msg) {
// 调试信息：打印接收到的数据
// printf("LK Motor ID:%d, CMD:0x%02X, Data: %02X %02X %02X %02X %02X %02X %02X %02X\n",
// motor->param.id, msg->data[0], msg->data[0], msg->data[1], msg->data[2],
// msg->data[3], msg->data[4], msg->data[5], msg->data[6], msg->data[7]);
// 检查命令字节是否为反馈命令
if (msg->data[0] != LK_CMD_FEEDBACK) {
return;
// 如果不是标准反馈命令，可能是其他格式的数据
// 临时跳过命令字节检查，直接解析数据
// return;
}
// 解析温度 (DATA[1])
@ -138,8 +145,9 @@ int8_t MOTOR_LK_Register(MOTOR_LK_Param_t *param) {
memset(&new_motor->motor, 0, sizeof(MOTOR_t));
new_motor->motor.reverse = param->reverse;
// 计算反馈ID（假设为控制ID + 0x40）
uint16_t feedback_id = param->id + 0x40;
// 对于某些LK电机，反馈数据可能通过命令ID发送
// 根据实际测试，使用命令ID接收反馈数据
uint16_t feedback_id = 0x140 + param->id; // 使用命令ID作为反馈ID
// 注册CAN接收ID
if (BSP_CAN_RegisterId(param->can, feedback_id, 3) != BSP_OK) {
@ -161,8 +169,8 @@ int8_t MOTOR_LK_Update(MOTOR_LK_Param_t *param) {
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_LK_t *motor = manager->motors[i];
if (motor && motor->param.id == param->id) {
// 计算反馈ID
uint16_t feedback_id = param->id + 0x100;
// 对于某些LK电机，反馈数据通过命令ID发送
uint16_t feedback_id = 0x140 + param->id;
BSP_CAN_Message_t rx_msg;
if (BSP_CAN_GetMessage(param->can, feedback_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
@ -217,9 +225,9 @@ int8_t MOTOR_LK_SetOutput(MOTOR_LK_Param_t *param, float value) {
// 转矩闭环控制命令 - 将输出值转换为转矩控制值
int16_t torque_control = (int16_t)(value * (float)LK_TORQUE_RANGE);
// 构建CAN帧
// 构建CAN帧（根据协议：命令报文标识符 = 0x140 + ID）
BSP_CAN_StdDataFrame_t tx_frame;
tx_frame.id = param->id;
tx_frame.id = 0x140 + param->id;
tx_frame.dlc = MOTOR_TX_BUF_SIZE;
// 设置转矩闭环控制命令数据
@ -245,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 = param->id;
tx_frame.id = 0x140 + param->id;
tx_frame.dlc = MOTOR_TX_BUF_SIZE;
// 电机运行命令
@ -265,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 = param->id;
tx_frame.id = 0x140 + param->id;
tx_frame.dlc = MOTOR_TX_BUF_SIZE;
// 电机关闭命令

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

@ -27,10 +27,19 @@
#define LZ_RAW_VALUE_MAX (65535) /* 16位原始值最大值 */
#define LZ_TEMP_SCALE (10.0f) /* 温度缩放因子 */
#define LZ_MAX_RECOVER_DIFF_RAD (0.4f)
#define MOTOR_TX_BUF_SIZE (8)
#define MOTOR_RX_BUF_SIZE (8)
/* Private macro ------------------------------------------------------------ */
MOTOR_LZ_MotionParam_t lz_recover_param = {
.target_angle = 0.0f,
.target_velocity = 0.0f,
.kp = 20.0f,
.kd = 1.0f,
.torque = 0.0f,
};
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static MOTOR_LZ_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
@ -445,7 +454,7 @@ int8_t MOTOR_LZ_Offline(MOTOR_LZ_Param_t *param) {
return DEVICE_ERR_NO_DEV;
}
MOTOR_LZ_Feedback_t* MOTOR_LZ_GetFeedback(MOTOR_LZ_Param_t *param) {
static MOTOR_LZ_Feedback_t* MOTOR_LZ_GetFeedback(MOTOR_LZ_Param_t *param) {
MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param);
if (motor && motor->motor.header.online) {
return &motor->lz_feedback;
@ -492,5 +501,32 @@ int8_t MOTOR_LZ_VelocityControl(MOTOR_LZ_Param_t *param, float target_velocity)
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);
}

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

@ -213,12 +213,7 @@ 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);
int8_t MOTOR_LZ_RecoverToZero(MOTOR_LZ_Param_t *param);
#ifdef __cplusplus
}