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++) {
MOTOR_LK_MotorOn(&c->param->wheel_motors[i]);
}
BSP_TIME_Delay_us(200);
for (int i = 0; i < 4; 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);
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_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);
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) {
tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */
if (osMessageQueueGet(task_runtime.msgq.chassis_imu, &chassis_imu, NULL, 0) == osOK) {
chassis.feedback.imu = chassis_imu;
}
if (osMessageQueueGet(task_runtime.msgq.Chassis_cmd, &chassis_cmd, NULL, 0) == osOK) {
// 成功接收到命令，更新底盘命令
}
// if (osMessageQueueGet(task_runtime.msgq.chassis_imu, &chassis_imu, NULL, 0) == osOK) {
// chassis.feedback.imu = chassis_imu;
// }
// if (osMessageQueueGet(task_runtime.msgq.Chassis_cmd, &chassis_cmd, NULL, 0) == osOK) {
// // 成功接收到命令，更新底盘命令
// }
Chassis_UpdateFeedback(&chassis);
Chassis_Control(&chassis, &chassis_cmd);
// Chassis_UpdateFeedback(&chassis);
// Chassis_Control(&chassis, &chassis_cmd);
/* USER CODE END */