robofish/sentry2026
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

更新 User/module/gimbal.c

Browse Source 
大yaw逻辑修正
This commit is contained in:
yunhai 2026-03-20 05:20:15 +08:00
parent ec273c1593
commit 8a86d525df
1 changed files with 6 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
User/module/gimbal.c
View File

@ -45,6 +45,7 @@ Gimbal_Output(&gimbal);
/* /*
*拟合函数，用于对pitch的重力补偿 *拟合函数，用于对pitch的重力补偿
*/ */
static double poly(double x) { static double poly(double x) {
return 0.420757*pow(x,3) + -2.27122*pow(x,2) + 4.13016*x + -1.98984; return 0.420757*pow(x,3) + -2.27122*pow(x,2) + 4.13016*x + -1.98984;
@ -398,10 +399,10 @@ switch (g->mode)
yaw_omega_set_point =PID_Calc(&g->pid.yaw_4310_angle,g->setpoint.yaw_4310, yaw_omega_set_point =PID_Calc(&g->pid.yaw_4310_angle,g->setpoint.yaw_4310,
g->feedback.motor.yaw_4310_motor_feedback.rotor_abs_angle,g->feedback.motor.yaw_4310_motor_feedback.rotor_speed,g->dt); g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle,g->feedback.motor.yaw_6020_motor_feedback.rotor_speed,g->dt);
g->out.yaw_4310 = PID_Calc(&g->pid.yaw_4310_omega,yaw_omega_set_point, // g->out.yaw_4310 = PID_Calc(&g->pid.yaw_4310_omega,yaw_omega_set_point,
g->feedback.motor.yaw_4310_motor_feedback.rotor_speed,0.0f,g->dt); //g->feedback.motor.yaw_4310_motor_feedback.rotor_speed,0.0f,g->dt);
// g->out.yaw_4310 = yaw_omega_set_point; // 直接输出速度环目标值作为电机输出 g->out.yaw_4310 = yaw_omega_set_point; // 直接输出速度环目标值作为电机输出
/*控制云台4310电机 也是单环，但是加了重力补偿函数，可以根据不一样的情况去拟合函数*/ /*控制云台4310电机 也是单环，但是加了重力补偿函数，可以根据不一样的情况去拟合函数*/
pitch_omega_set_point =PID_Calc(&g->pid.pitch_4310_angle,g->setpoint.eulr.pit, pitch_omega_set_point =PID_Calc(&g->pid.pitch_4310_angle,g->setpoint.eulr.pit,
g->feedback.imu.eulr.rol,-g->feedback.imu.gyro.y,g->dt); g->feedback.imu.eulr.rol,-g->feedback.imu.gyro.y,g->dt);
@ -463,7 +464,7 @@ void Gimbal_Output(Gimbal_t *g)
// output_yaw_4310.kd=0.1f; // output_yaw_4310.kd=0.1f;
MOTOR_MIT_Output_t output_pitch_4310 = {0}; MOTOR_MIT_Output_t output_pitch_4310 = {0};
output_pitch_4310.torque = g->out.pitch_4310 * 1.0f; output_pitch_4310.torque = g->out.pitch_4310 * 1.0f+1.5;//1.5是重力补偿
output_pitch_4310.kd=0.2f; output_pitch_4310.kd=0.2f;
MOTOR_RM_Ctrl(&g->param->yaw_6020_motor); MOTOR_RM_Ctrl(&g->param->yaw_6020_motor);