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-24 17:44:40 +08:00
parent 040176d54f
commit 734d2c90dc
2 changed files with 11 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
User/module/balance_chassis.c
View File

@ -18,6 +18,7 @@ static int8_t Chassis_MotorEnable(Chassis_t *c) {
for (int i = 0; i < 2; i++) { for (int i = 0; i < 2; i++) {
MOTOR_LK_MotorOn(&c->param->wheel_motors[i]); MOTOR_LK_MotorOn(&c->param->wheel_motors[i]);
} }
BSP_TIME_Delay_us(200);
for (int i = 0; i < 4; i++) { for (int i = 0; i < 4; i++) {
MOTOR_LZ_Enable(&c->param->joint_motors[i]); MOTOR_LZ_Enable(&c->param->joint_motors[i]);
} }
@ -256,9 +257,9 @@ int8_t Chassis_Control(Chassis_t *c, const Chassis_CMD_t *c_cmd){
tp1 = 3*PID_Calc(&c->pid.tp[0], 0.0f, c->vmc_[0].leg.theta, c->vmc_[0].leg.d_theta, c->dt); tp1 = 3*PID_Calc(&c->pid.tp[0], 0.0f, c->vmc_[0].leg.theta, c->vmc_[0].leg.d_theta, c->dt);
tp2 = 3*PID_Calc(&c->pid.tp[1], 0.0f, c->vmc_[1].leg.theta, c->vmc_[1].leg.d_theta, c->dt); tp2 = 3*PID_Calc(&c->pid.tp[1], 0.0f, c->vmc_[1].leg.theta, c->vmc_[1].leg.d_theta, c->dt);
VMC_InverseSolve(&c->vmc_[0], fn, tp1); VMC_InverseSolve(&c->vmc_[0], fn, 0.0f);
VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0].torque, &c->output.joint[1].torque); VMC_GetJointTorques(&c->vmc_[0], &c->output.joint[0].torque, &c->output.joint[1].torque);
VMC_InverseSolve(&c->vmc_[1], fn, tp2); VMC_InverseSolve(&c->vmc_[1], fn, 0.0f);
VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3].torque, &c->output.joint[2].torque); VMC_GetJointTorques(&c->vmc_[1], &c->output.joint[3].torque, &c->output.joint[2].torque);
c->output.wheel[0] = 0.0f; c->output.wheel[0] = 0.0f;

16
User/task/ctrl_chassis.c
View File

@ -57,15 +57,15 @@ void Task_ctrl_chassis(void *argument) {
while (1) { while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */ tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */ /* USER CODE BEGIN */
if (osMessageQueueGet(task_runtime.msgq.chassis_imu, &chassis_imu, NULL, 0) == osOK) { // if (osMessageQueueGet(task_runtime.msgq.chassis_imu, &chassis_imu, NULL, 0) == osOK) {
chassis.feedback.imu = chassis_imu; // chassis.feedback.imu = chassis_imu;
} // }
if (osMessageQueueGet(task_runtime.msgq.Chassis_cmd, &chassis_cmd, NULL, 0) == osOK) { // if (osMessageQueueGet(task_runtime.msgq.Chassis_cmd, &chassis_cmd, NULL, 0) == osOK) {
// 成功接收到命令，更新底盘命令 // // 成功接收到命令，更新底盘命令
} // }
Chassis_UpdateFeedback(&chassis); // Chassis_UpdateFeedback(&chassis);
Chassis_Control(&chassis, &chassis_cmd); // Chassis_Control(&chassis, &chassis_cmd);
/* USER CODE END */ /* USER CODE END */