xiaocheng/Er_sentry
1
1
Fork 1
Code Issues 1 Pull Requests Actions Packages Projects Releases Wiki Activity

123

Browse Source
This commit is contained in:
Xiaocheng 2026-03-16 13:55:46 +08:00
parent bdb9de8b0b
commit a1084cd371
3 changed files with 45 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

82
User/module/chassis.c
View File

@ -73,7 +73,7 @@ static int8_t Chassis_SetMode(Chassis_t *c, Chassis_Mode_t mode ,uint32_t now)
if (mode == CHASSIS_MODE_ROTOR && c->mode != CHASSIS_MODE_ROTOR) { if (mode == CHASSIS_MODE_ROTOR && c->mode != CHASSIS_MODE_ROTOR) {
srand(now); srand(now);
c->wz_multi =0.1; c->wz_multi =0.25f;
// c->wz_multi = (rand() % 2) ? -1 : 1; // c->wz_multi = (rand() % 2) ? -1 : 1;
} }
for (int i = 0; i < 4; i++) for (int i = 0; i < 4; i++)
@ -403,20 +403,14 @@ int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd,uint32_t now)
case CHASSIS_MODE_FOLLOW_GIMBAL: case CHASSIS_MODE_FOLLOW_GIMBAL:
// 跟随云台模式 // 跟随云台模式
c->move_vec.Vx =-c_cmd->y_l; c->move_vec.Vx =-c_cmd->Vy;
c->move_vec.Vy =-c_cmd->x_l; c->move_vec.Vy =-c_cmd->Vx;
c->move_vec.Vw = PID_Calc(&c->pid.chassis_follow_gimbal_pid, 4.51735544f ,c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle, 0.0f, c->dt); c->move_vec.Vw = PID_Calc(&c->pid.chassis_follow_gimbal_pid, 4.51735544f ,c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle, 0.0f, c->dt);
// c->move_vec.Vw = -PID_Calc(&c->pid.chassis_follow_gimbal_pid, c->motorfeedback.gimbal_yaw_encoder-2.07694483f ,c->motorfeedback.gimbal_yaw_encoder, 0.0f, c->dt); // c->move_vec.Vw = -PID_Calc(&c->pid.chassis_follow_gimbal_pid, c->motorfeedback.gimbal_yaw_encoder-2.07694483f ,c->motorfeedback.gimbal_yaw_encoder, 0.0f, c->dt);
break; break;
/* 临时调控导航*/
if(c->ctrl_mode == CHASSIS_MODE_DAOHANG)
{
c->move_vec.Vx = -c_cmd->Vx/1;
c->move_vec.Vy = c_cmd->Vy/1;
c->move_vec.Vw =PID_Calc(&c->pid.daohang_chassis_follow_gimbal_pid, 4.51735544f ,c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle, 0.0f, c->dt);
}
// case CHASSIS_MODE_DAOHANG: // case CHASSIS_MODE_DAOHANG:
// // 导航模式 // // 导航模式
// c->move_vec.Vx = -c_cmd->Vx/1; // c->move_vec.Vx = -c_cmd->Vx/1;
@ -427,22 +421,28 @@ int8_t Chassis_Control(Chassis_t *c, Chassis_CMD_t *c_cmd,uint32_t now)
default: default:
return CHASSIS_ERR_MODE; /* 未知模式 */ return CHASSIS_ERR_MODE; /* 未知模式 */
} }
/* 临时调控导航*/
if(c->ctrl_mode == CHASSIS_MODE_DAOHANG)
{
c->move_vec.Vx = -c_cmd->Vx/1;
c->move_vec.Vy = c_cmd->Vy/1;
c->move_vec.Vw =0.0f;//PID_Calc(&c->pid.daohang_chassis_follow_gimbal_pid, 4.51735544f ,c->motorfeedback.gimbal_yaw_encoder.rotor_abs_angle, 0.0f, c->dt);
}
/*给输出的Vx，Vy，Vw进行滤波*/ /*给输出的Vx，Vy，Vw进行滤波*/
c->move_vec.Vx = LowPassFilter2p_Apply(&c->filled[0], c->move_vec.Vx); // c->move_vec.Vx = LowPassFilter2p_Apply(&c->filled[0], c->move_vec.Vx);
c->move_vec.Vy = LowPassFilter2p_Apply(&c->filled[1], c->move_vec.Vy); // c->move_vec.Vy = LowPassFilter2p_Apply(&c->filled[1], c->move_vec.Vy);
c->move_vec.Vw = LowPassFilter2p_Apply(&c->filled[2], c->move_vec.Vw); // c->move_vec.Vw = LowPassFilter2p_Apply(&c->filled[2], c->move_vec.Vw);
Chassis_speed_calculate(c, c_cmd); Chassis_speed_calculate(c, c_cmd);
if(c->ctrl_mode == CHASSIS_MODE_RC) // if(c->ctrl_mode == CHASSIS_MODE_RC)
{ // {
switch (c->mode) // switch (c->mode)
{ case CHASSIS_MODE_BREAK: // { case CHASSIS_MODE_BREAK:
case CHASSIS_MODE_RELAX: // case CHASSIS_MODE_RELAX:
case STOP: // case STOP:
case RC: // case RC:
case CHASSIS_MODE_FOLLOW_GIMBAL: // case CHASSIS_MODE_FOLLOW_GIMBAL:
case CHASSIS_MODE_ROTOR: // case CHASSIS_MODE_ROTOR:
for (int i = 0; i < 4; i++) for (int i = 0; i < 4; i++)
{ {
@ -461,30 +461,30 @@ if(c->ctrl_mode == CHASSIS_MODE_RC)
c->motorfeedback.rotor_rpm3508[i], 0.0f, c->dt); c->motorfeedback.rotor_rpm3508[i], 0.0f, c->dt);
c->out.rotor3508_out[i] = LowPassFilter2p_Apply(&c->filled[3+i], c->final_out.final_3508out[i]); c->out.rotor3508_out[i] = LowPassFilter2p_Apply(&c->filled[3+i], c->final_out.final_3508out[i]);
} }
break; // break;
} // }
// if(c->ctrl_mode == CHASSIS_MODE_DAOHANG) // if(c->ctrl_mode == CHASSIS_MODE_DAOHANG)
// { // {
for(int i=0;i<4;i++) // for(int i=0;i<4;i++)
{ // {
float chassis6020_detangle[4]; // 6020解算出的角度 // float chassis6020_detangle[4]; // 6020解算出的角度
c->hopemotorout.motor6020_target[i] = c->hopemotorout.rotor6020_jiesuan_2[i]; // c->hopemotorout.motor6020_target[i] = c->hopemotorout.rotor6020_jiesuan_2[i];
chassis6020_detangle[i] = PID_Calc(&(c->pid.chassis_6020anglePid[i]), c->hopemotorout.motor6020_target[i], // chassis6020_detangle[i] = PID_Calc(&(c->pid.chassis_6020anglePid[i]), c->hopemotorout.motor6020_target[i],
c->motorfeedback.rotor_angle6020[i], 0.0f, c->dt); // c->motorfeedback.rotor_angle6020[i], 0.0f, c->dt);
// c->final_out.final_6020out[i] = chassis6020_detangle[i] ; //单环控制就用这个 // // c->final_out.final_6020out[i] = chassis6020_detangle[i] ; //单环控制就用这个
c->final_out.final_6020out[i] = PID_Calc(&(c->pid.chassis_6020OmegaPid[i]), chassis6020_detangle[i], // c->final_out.final_6020out[i] = PID_Calc(&(c->pid.chassis_6020OmegaPid[i]), chassis6020_detangle[i],
c->motorfeedback.rotor_rpm6020[i], 0.0f, c->dt); // c->motorfeedback.rotor_rpm6020[i], 0.0f, c->dt);
c->out.rotor6020_out[i] = LowPassFilter2p_Apply(&c->filled[7+i], c->final_out.final_6020out[i]); // c->out.rotor6020_out[i] = LowPassFilter2p_Apply(&c->filled[7+i], c->final_out.final_6020out[i]);
c->hopemotorout.motor3508_target[i] = c->hopemotorout.rotor3508_jiesuan_2[i]; // c->hopemotorout.motor3508_target[i] = c->hopemotorout.rotor3508_jiesuan_2[i];
c->final_out.final_3508out[i] = PID_Calc(&(c->pid.chassis_3508DAOHANG_pid[i]), c->hopemotorout.motor3508_target[i], // c->final_out.final_3508out[i] = PID_Calc(&(c->pid.chassis_3508DAOHANG_pid[i]), c->hopemotorout.motor3508_target[i],
c->motorfeedback.rotor_truespeed3508[i], 0.0f, c->dt); // c->motorfeedback.rotor_truespeed3508[i], 0.0f, c->dt);
c->out.rotor3508_out[i] = LowPassFilter2p_Apply(&c->filled[3+i], c->final_out.final_3508out[i]); // c->out.rotor3508_out[i] = LowPassFilter2p_Apply(&c->filled[3+i], c->final_out.final_3508out[i]);
} // }
} // }
// } // }
return CHASSIS_OK; return CHASSIS_OK;
} }

6
User/module/config.c
View File

@ -246,8 +246,8 @@ Config_Param_t config = {
}, },
.pid.daohang_6020_motor_angle={ .pid.daohang_6020_motor_angle={
.k = 2.0f, .k = 1.5f,
.p = 2.5f, .p = 1.7f,
.i = 0.0f, .i = 0.0f,
.d = 0.0f, .d = 0.0f,
.i_limit = 1.0f, .i_limit = 1.0f,
@ -260,7 +260,7 @@ Config_Param_t config = {
.p = 5.0f, .p = 5.0f,
.i = 1.0f, .i = 1.0f,
.d = 0.2f, .d = 0.2f,
.i_limit = 0.0f, .i_limit = 1.0f,
.out_limit = 3.0f, .out_limit = 3.0f,
.d_cutoff_freq = 20.0f, .d_cutoff_freq = 20.0f,
.range = M_2PI .range = M_2PI

2
User/task/chassis.c
View File

@ -66,7 +66,7 @@ osMessageQueueGet(task_runtime.msgq.navi.c_cmd, &c_cmd_ai_result, NULL, 0);
Chassis_CMD_t safe_cmd = {.mode = STOP, .Vx = 0, .Vy = 0, .Vw = 0}; Chassis_CMD_t safe_cmd = {.mode = STOP, .Vx = 0, .Vy = 0, .Vw = 0};
Chassis_Control(&chassis, &safe_cmd,tick); Chassis_Control(&chassis, &safe_cmd,tick);
} }
// Chassis_Setoutput(&chassis); Chassis_Setoutput(&chassis);
/* USER CODE END */ /* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
} }