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-10-05 14:13:49 +08:00
parent fa75912567
commit db9d9f7db5
4 changed files with 39 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
User/device/virtual_chassis.c
View File

@ -372,31 +372,42 @@ int8_t Virtual_Chassis_SendJointTorque(Virtual_Chassis_t *chassis, float torques
/**
* @brief 发送轮子电机控制命令
*/
int8_t Virtual_Chassis_SendWheelCommand(Virtual_Chassis_t *chassis, uint8_t left_cmd[8], uint8_t right_cmd[8]) {
if (chassis == NULL || left_cmd == NULL || right_cmd == NULL) {
int8_t Virtual_Chassis_SendWheelCommand(Virtual_Chassis_t *chassis, float left_out, float right_out) {
if (chassis == NULL) {
return DEVICE_ERR_NULL;
}
// 发送左轮控制命令
BSP_CAN_StdDataFrame_t left_frame = {
.id = chassis->param.motors.wheel_left_id,
.dlc = 8,
};
memcpy(left_frame.data, left_cmd, 8);
memcpy(chassis->output.wheel_left_cmd, left_cmd, 8);
// 发送右轮控制命令
BSP_CAN_StdDataFrame_t right_frame = {
.id = chassis->param.motors.wheel_right_id,
.dlc = 8,
};
memcpy(right_frame.data, right_cmd, 8);
memcpy(chassis->output.wheel_right_cmd, right_cmd, 8);
int8_t result1 = BSP_CAN_TransmitStdDataFrame(chassis->param.motors.can, &left_frame);
int8_t result2 = BSP_CAN_TransmitStdDataFrame(chassis->param.motors.can, &right_frame);
return (result1 == BSP_OK && result2 == BSP_OK) ? DEVICE_OK : DEVICE_ERR;
int16_t torque_control = (int16_t)(left_out * 2048);
// 构建CAN帧
BSP_CAN_StdDataFrame_t tx_frame;
tx_frame.id = chassis->param.motors.wheel_left_id;
tx_frame.dlc = 8;
tx_frame.data[0] = 0xA1;
tx_frame.data[1] = 0x00;
tx_frame.data[2] = 0x00;
tx_frame.data[3] = 0x00;
tx_frame.data[4] = (uint8_t)(torque_control & 0xFF);
tx_frame.data[5] = (uint8_t)((torque_control >> 8) & 0xFF);
tx_frame.data[6] = 0x00;
tx_frame.data[7] = 0x00;
if (BSP_CAN_TransmitStdDataFrame(chassis->param.motors.can, &tx_frame) != BSP_OK) {
return DEVICE_ERR;
}
torque_control = (int16_t)(right_out * 2048);
// 构建CAN帧
tx_frame.id = chassis->param.motors.wheel_right_id;
tx_frame.dlc = 8;
tx_frame.data[0] = 0xA1;
tx_frame.data[1] = 0x00;
tx_frame.data[2] = 0x00;
tx_frame.data[3] = 0x00;
tx_frame.data[4] = (uint8_t)(torque_control & 0xFF);
tx_frame.data[5] = (uint8_t)((torque_control >> 8) & 0xFF);
tx_frame.data[6] = 0x00;
tx_frame.data[7] = 0x00;
if (BSP_CAN_TransmitStdDataFrame(chassis->param.motors.can, &tx_frame) != BSP_OK) {
return DEVICE_ERR;
}
}

2
User/device/virtual_chassis.h
View File

@ -111,7 +111,7 @@ int8_t Virtual_Chassis_SendJointTorque(Virtual_Chassis_t *chassis, float torques
* @param right_cmd 右轮控制命令数据（8字节）
* @return DEVICE_OK 成功，其他值失败
*/
int8_t Virtual_Chassis_SendWheelCommand(Virtual_Chassis_t *chassis, uint8_t left_cmd[8], uint8_t right_cmd[8]);
int8_t Virtual_Chassis_SendWheelCommand(Virtual_Chassis_t *chassis, float left_out, float right_out);
#ifdef __cplusplus
}

6
User/module/balance_chassis.c
View File

@ -35,9 +35,7 @@ static int8_t Chassis_MotorRelax(Chassis_t *c) {
return CHASSIS_ERR_NULL;
float relax_torque[4] = {0.0f, 0.0f, 0.0f, 0.0f};
Virtual_Chassis_SendJointTorque(&virtual_chassis, relax_torque);
// for (int i = 0; i < 2; i++) {
// MOTOR_LK_Relax(&c->param->wheel_motors[i]);
// }
Virtual_Chassis_SendWheelCommand(&virtual_chassis, 0.1f, 0.1f);
return CHASSIS_OK;
}
@ -106,6 +104,8 @@ int8_t Chassis_Init(Chassis_t *c, Chassis_Params_t *param, float target_freq) {
}
c->param = param;
c->mode = CHASSIS_MODE_RELAX;
/*初始化can*/
BSP_CAN_Init();

2
User/task/ctrl_chassis.c
View File

@ -22,7 +22,7 @@
/* USER STRUCT BEGIN */
Chassis_t chassis;
Chassis_CMD_t chassis_cmd = {
.mode = CHASSIS_MODE_RECOVER, // 改为RECOVER模式，让电机先启动
.mode = CHASSIS_MODE_RELAX, // 改为RECOVER模式，让电机先启动
.move_vec = {
.vx = 0.0f,
.vy = 0.0f,