AimBot/Gimbal
14
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: add direction flag to feedforward controller with init config struct

Browse Source 
- add DM_Feedforward_Init_Config_s with Kp and dir fields
- replace plain DM_Feedforward_Kp in DM_Motor_Init_Config_s
- apply direction flag in DMMotorControl to negate feedforward when reverse
- set yaw feedforward: Kp=0.9, dir=REVERSE
This commit is contained in:
李傲琦
2026-06-07 16:22:16 +08:00
parent 89aa777d5c
commit ed6d509c72
3 changed files with 18 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files
application/gimbal/gimbal.c
+4 -1
View File
@@ -52,7 +52,10 @@ void GimbalInit()
.motor_type = DM_MOTOR, .motor_type = DM_MOTOR,
}, },
.ctrl_mode = DM_MODE_VELOCITY, .ctrl_mode = DM_MODE_VELOCITY,
.DM_Feedforward_Kp = 0.9f, .feedforward_config = {
.Kp = 0.9f,
.dir = OUTPUT_DIRECTION_REVERSE,
},
.mit_kp = 0, .mit_kd = 0, .mit_kp = 0, .mit_kd = 0,
}; };
yaw_motor = DMMotorInit(&yaw_config); yaw_motor = DMMotorInit(&yaw_config);
modules/motor/DMmotor/dmmotor.c
+5 -3
View File
@@ -186,8 +186,9 @@ DMMotorInstance *DMMotorInit(DM_Motor_Init_Config_s *config)
config->motor_config.can_init_config.id = motor; config->motor_config.can_init_config.id = motor;
motor->motor_can_instance = CANRegister(&config->motor_config.can_init_config); motor->motor_can_instance = CANRegister(&config->motor_config.can_init_config);
/* 设置前馈控制器增益 */ /* 设置前馈控制器 */
motor->feedforward_controller.Kp = config->DM_Feedforward_Kp; motor->feedforward_controller.Kp = config->feedforward_config.Kp;
motor->feedforward_controller.dir = config->feedforward_config.dir;
/* register daemon */ /* register daemon */
Daemon_Init_Config_s conf = { Daemon_Init_Config_s conf = {
@@ -237,7 +238,8 @@ void DMMotorControl(void)
if (motor->output_reverse == OUTPUT_DIRECTION_REVERSE) if (motor->output_reverse == OUTPUT_DIRECTION_REVERSE)
pid_out *= -1; pid_out *= -1;
pid_out += feedforward_controller.Kp * feedforward_controller.feed_input; float ff = feedforward_controller.Kp * feedforward_controller.feed_input;
pid_out += (feedforward_controller.dir == OUTPUT_DIRECTION_REVERSE) ? -ff : ff;
float vel_cmd = pid_out * DEGREE_2_RAD; float vel_cmd = pid_out * DEGREE_2_RAD;
LIMIT_MIN_MAX(vel_cmd, DM_V_MIN, DM_V_MAX); LIMIT_MIN_MAX(vel_cmd, DM_V_MIN, DM_V_MAX);
modules/motor/DMmotor/dmmotor.h
+9 -1
View File
@@ -56,6 +56,7 @@ typedef struct
typedef struct{ typedef struct{
float Kp; float Kp;
Output_Reverse_Flag_e dir;
float feed_input; float feed_input;
} DM_Feedforward_Controller_s; } DM_Feedforward_Controller_s;
@@ -76,12 +77,19 @@ typedef struct
} DMMotorInstance; } DMMotorInstance;
/* Feedforward controller init config */
typedef struct
{
float Kp;
Output_Reverse_Flag_e dir;
} DM_Feedforward_Init_Config_s;
/* DM motor init config */ /* DM motor init config */
typedef struct typedef struct
{ {
Motor_Init_Config_s motor_config; // generic motor PID and setting config Motor_Init_Config_s motor_config; // generic motor PID and setting config
DM_Motor_Ctrl_Mode_e ctrl_mode; // default control mode DM_Motor_Ctrl_Mode_e ctrl_mode; // default control mode
float DM_Feedforward_Kp; DM_Feedforward_Init_Config_s feedforward_config;
float mit_kp; // MIT mode KP (only for MIT mode) float mit_kp; // MIT mode KP (only for MIT mode)
float mit_kd; // MIT mode KD (only for MIT mode) float mit_kd; // MIT mode KD (only for MIT mode)
} DM_Motor_Init_Config_s; } DM_Motor_Init_Config_s;