From e1a01e6b5359df8a9b808d00c7bc0cfb1528d653 Mon Sep 17 00:00:00 2001 From: SkyLi Date: Sun, 7 Jun 2026 02:18:36 +0800 Subject: [PATCH] feat: add 0xA9 feedforward frame and wire feedforward controller to motor output MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit - host 0xA9 frame → master_machine decode → robot_cmd publish via gimbal_feedforward topic → gimbal subscribe → yaw_motor.feed_input - add DM_Feedforward_Controller_s with Kp and feed_input fields - add feedforward term (Kp * feed_input) to motor control output - add DM_Feedforward_Kp to motor init config --- application/cmd/robot_cmd.c | 28 +++++++++++++++++++++++-- application/gimbal/gimbal.c | 20 ++++++++++++++---- application/robot_def.h | 6 ++++++ modules/master_machine/master_machine.c | 7 +++++++ modules/master_machine/master_machine.h | 20 +++++++++++++----- modules/motor/DMmotor/dmmotor.c | 20 ++++++++++++------ modules/motor/DMmotor/dmmotor.h | 7 +++++++ 7 files changed, 91 insertions(+), 17 deletions(-) diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 21a1677..7d518e4 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -29,11 +29,16 @@ static float filtered_y_measure; static float gimbal_yaw_ref; static float gimbal_pitch_ref; +/* 手动模式角度速率 */ +static float yaw_degree_velocity = 0; +static float pitch_degree_velocity = 0; + /* ---- pub-sub ---- */ static Publisher_t *gimbal_cmd_pub; static Subscriber_t *gimbal_feed_sub; static Publisher_t *motor_enable_pub; static Publisher_t *motor_pid_pub; +static Publisher_t *feedforward_pub; static Gimbal_Ctrl_Cmd_s gimbal_cmd_send; static Gimbal_Upload_Data_s gimbal_fetch_data; @@ -94,6 +99,8 @@ static void SwitchToAutoMode(void) vision_data.auto_updated = 0; VisionPIDReset(); + vision_data.feedforward_updated = 0; + /* 从调试模式切回时重新使能电机 */ if (current_mode == VISION_MODE_DEBUG) { @@ -114,6 +121,10 @@ static void SwitchToManualMode(void) // gimbal_pitch_ref = gimbal_fetch_data.gimbal_imu_data->Pitch; vision_data.manual_updated = 0; + yaw_degree_velocity = 0; + pitch_degree_velocity = 0; + + vision_data.feedforward_updated = 0; /* 从调试模式切回时重新使能电机 */ if (current_mode == VISION_MODE_DEBUG) @@ -209,6 +220,7 @@ void RobotCMDInit() gimbal_feed_sub = SubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s)); motor_enable_pub = PubRegister("motor_enable", sizeof(Motor_Enable_s)); motor_pid_pub = PubRegister("motor_pid", sizeof(Motor_PID_Tune_s)); + feedforward_pub = PubRegister("gimbal_feedforward", sizeof(Gimbal_Feedforward_s)); static PID_Init_Config_s yaw_vpid_config = { .Kp = 100.0f, .Ki = 0.0f, .Kd = 0.0f, @@ -272,11 +284,16 @@ static void VisionTracking() gimbal_cmd_send.yaw = gimbal_yaw_ref; gimbal_cmd_send.pitch = gimbal_pitch_ref; + + if (vision_data.feedforward_updated) + { + vision_data.feedforward_updated = 0; + Gimbal_Feedforward_s ff = { .feedforward = vision_data.feedforward }; + PubPushMessage(feedforward_pub, &ff); + } } /* ---- 手动模式: 上位机角度参考 ---- */ -static float yaw_degree_velocity = 0; -static float pitch_degree_velocity = 0; static void ManualMode() { if (vision_data.manual_updated) @@ -295,6 +312,13 @@ static void ManualMode() gimbal_cmd_send.yaw = gimbal_yaw_ref; gimbal_cmd_send.pitch = gimbal_pitch_ref; + + if (vision_data.feedforward_updated) + { + vision_data.feedforward_updated = 0; + Gimbal_Feedforward_s ff = { .feedforward = vision_data.feedforward }; + PubPushMessage(feedforward_pub, &ff); + } } /* ---- 调试模式: 上位机下发 PID 参数调参 ---- */ diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index 296119b..5e7759b 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -11,6 +11,7 @@ static Publisher_t *gimbal_pub; static Subscriber_t *gimbal_sub; static Subscriber_t *motor_enable_sub; static Subscriber_t *motor_pid_sub; +static Subscriber_t *feedforward_sub; static Gimbal_Upload_Data_s gimbal_feedback_data; static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; @@ -25,6 +26,7 @@ void GimbalInit() gimbal_sub = SubRegister("gimbal_cmd", sizeof(Gimbal_Ctrl_Cmd_s)); motor_enable_sub = SubRegister("motor_enable", sizeof(Motor_Enable_s)); motor_pid_sub = SubRegister("motor_pid", sizeof(Motor_PID_Tune_s)); + feedforward_sub = SubRegister("gimbal_feedforward", sizeof(Gimbal_Feedforward_s)); /* ---- Yaw: DM-G6220, CAN1, TX=0x01, RX=0xF1, Velocity模式 ---- */ DM_Motor_Init_Config_s yaw_config = { @@ -50,6 +52,7 @@ void GimbalInit() .motor_type = DM_MOTOR, }, .ctrl_mode = DM_MODE_VELOCITY, + .DM_Feedforward_Kp = 0.9f, .mit_kp = 0, .mit_kd = 0, }; yaw_motor = DMMotorInit(&yaw_config); @@ -87,10 +90,19 @@ void GimbalInit() void GimbalTask() { - SubGetMessage(gimbal_sub, &gimbal_cmd_recv); - - DMMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); - DMMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch); + /* 角度环参考值设定 */ + if(SubGetMessage(gimbal_sub, &gimbal_cmd_recv)) + { + DMMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw); + DMMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch); + } + + /* 前馈输入 */ + static Gimbal_Feedforward_s feedforward_msg; + if (SubGetMessage(feedforward_sub, &feedforward_msg)) + { + yaw_motor->feedforward_controller.feed_input = feedforward_msg.feedforward; + } /* 电机使能/失能控制 */ static Motor_Enable_s enable_msg; diff --git a/application/robot_def.h b/application/robot_def.h index 440ade5..1a4f41f 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -63,6 +63,12 @@ typedef struct float pitch_Kp, pitch_Ki, pitch_Kd; } Motor_PID_Tune_s; +/* ---- CMD 发布给 gimbal 的前馈输入 ---- */ +typedef struct +{ + float feedforward; +} Gimbal_Feedforward_s; + #pragma pack() #endif // !ROBOT_DEF_H diff --git a/modules/master_machine/master_machine.c b/modules/master_machine/master_machine.c index cb537b3..86c4b27 100644 --- a/modules/master_machine/master_machine.c +++ b/modules/master_machine/master_machine.c @@ -62,6 +62,13 @@ static void DecodeVision() memcpy(&vision_data.motor_pitch_Kd, p, 4); vision_data.debug_updated = 1; } + else if (sof == VISION_SOF_FEEDFORWARD && len == VISION_FEEDFORWARD_LEN) + { + if (calc_checksum(buf, VISION_FEEDFORWARD_LEN) != buf[VISION_FEEDFORWARD_LEN - 1]) + return; + memcpy(&vision_data.feedforward, buf + 1, 4); + vision_data.feedforward_updated = 1; + } else if (sof == VISION_SOF_CMD && len == VISION_CMD_LEN) { if (calc_checksum(buf, VISION_CMD_LEN) != buf[VISION_CMD_LEN - 1]) diff --git a/modules/master_machine/master_machine.h b/modules/master_machine/master_machine.h index 23bcf0f..407ea0c 100644 --- a/modules/master_machine/master_machine.h +++ b/modules/master_machine/master_machine.h @@ -31,18 +31,24 @@ * [motor_pitch_Kp:4][motor_pitch_Ki:4][motor_pitch_Kd:4][checksum:1] * 全部 float, 依次为视觉环 yaw/pitch 和电机角度环 yaw/pitch 的 PID 参数 * + * ---- 前馈输入帧 0xA9 (6 字节) ---- + * [SOF:0xA9][feedforward:4][checksum:1] + * feedforward: float, 前馈输入值 + * * checksum: 帧头到数据末尾前一字节累加和取低8位 */ #define VISION_SOF_AUTO 0xA5 #define VISION_SOF_MANUAL 0xA6 #define VISION_SOF_CMD 0xA7 -#define VISION_SOF_DEBUG 0xA8 +#define VISION_SOF_DEBUG 0xA8 +#define VISION_SOF_FEEDFORWARD 0xA9 -#define VISION_AUTO_LEN 18 -#define VISION_MANUAL_LEN 10 -#define VISION_CMD_LEN 3 -#define VISION_DEBUG_LEN 50 +#define VISION_AUTO_LEN 18 +#define VISION_MANUAL_LEN 10 +#define VISION_CMD_LEN 3 +#define VISION_DEBUG_LEN 50 +#define VISION_FEEDFORWARD_LEN 6 /* 命令码 */ #define VISION_CMD_SWITCH_AUTO 0x01 @@ -80,6 +86,10 @@ typedef struct float motor_pitch_Kp, motor_pitch_Ki, motor_pitch_Kd; uint8_t debug_updated; + /* 前馈输入 */ + float feedforward; + uint8_t feedforward_updated; + /* 命令 */ uint8_t cmd; // 最新命令码 uint8_t cmd_pending; // 有待处理命令标志 diff --git a/modules/motor/DMmotor/dmmotor.c b/modules/motor/DMmotor/dmmotor.c index facbe95..027e0b0 100644 --- a/modules/motor/DMmotor/dmmotor.c +++ b/modules/motor/DMmotor/dmmotor.c @@ -107,6 +107,9 @@ void DMMotorSetMode(DM_Motor_Mode_e cmd, DMMotorInstance *motor) motor->motor_controller.angle_PID.Dout = 0; motor->motor_controller.angle_PID.Last_Dout = 0; motor->motor_controller.pid_ref = 0; + + /* 清理前馈控制器 */ + motor->feedforward_controller.feed_input = 0; } memset(motor->motor_can_instance->tx_buff, 0xFF, 7); @@ -183,6 +186,9 @@ DMMotorInstance *DMMotorInit(DM_Motor_Init_Config_s *config) config->motor_config.can_init_config.id = motor; motor->motor_can_instance = CANRegister(&config->motor_config.can_init_config); + /* 设置前馈控制器增益 */ + motor->feedforward_controller.Kp = config->DM_Feedforward_Kp; + /* register daemon */ Daemon_Init_Config_s conf = { .callback = DMMotorLostCallback, @@ -213,7 +219,7 @@ void DMMotorControl(void) if(motor->motor_mode == DM_CMD_DISABLE_MODE) continue; CANInstance *can = motor->motor_can_instance; - + DM_Feedforward_Controller_s feedforward_controller = motor->feedforward_controller; /* handle motor error states */ if (motor->measure.state != 0x00 && motor->measure.state != 0x01) { @@ -224,14 +230,16 @@ void DMMotorControl(void) } /* angle PID: IMU feedback → velocity output */ - float pid_ref = PIDCalculate(&motor->motor_controller.angle_PID, + float pid_out = PIDCalculate(&motor->motor_controller.angle_PID, *motor->motor_controller.other_angle_feedback_ptr, motor->motor_controller.pid_ref); if (motor->output_reverse == OUTPUT_DIRECTION_REVERSE) - pid_ref *= -1; + pid_out *= -1; - float vel_cmd = pid_ref * DEGREE_2_RAD; + pid_out += feedforward_controller.Kp * feedforward_controller.feed_input; + + float vel_cmd = pid_out * DEGREE_2_RAD; LIMIT_MIN_MAX(vel_cmd, DM_V_MIN, DM_V_MAX); switch (motor->ctrl_mode) @@ -241,13 +249,13 @@ void DMMotorControl(void) break; } case DM_MODE_MIT: { - float pos_cmd = pid_ref * DEGREE_2_RAD; + float pos_cmd = pid_out * DEGREE_2_RAD; LIMIT_MIN_MAX(pos_cmd, DM_P_MIN, DM_P_MAX); DMMotorSendMIT(can, pos_cmd, 0.0f, motor->mit_kp, motor->mit_kd, 0.0f); break; } case DM_MODE_POS_VEL: { - float pos_cmd = pid_ref * DEGREE_2_RAD; + float pos_cmd = pid_out * DEGREE_2_RAD; LIMIT_MIN_MAX(pos_cmd, DM_P_MIN, DM_P_MAX); DMMotorSendPosVel(can, pos_cmd, vel_cmd); break; diff --git a/modules/motor/DMmotor/dmmotor.h b/modules/motor/DMmotor/dmmotor.h index 2c09516..e595e6b 100644 --- a/modules/motor/DMmotor/dmmotor.h +++ b/modules/motor/DMmotor/dmmotor.h @@ -54,11 +54,17 @@ typedef struct float total_angle; // multi-turn angle (degrees) } DM_Motor_Measure_s; +typedef struct{ + float Kp; + float feed_input; +} DM_Feedforward_Controller_s; + /* DM motor instance (runtime state) */ typedef struct { DM_Motor_Measure_s measure; Motor_Controller_s motor_controller; + DM_Feedforward_Controller_s feedforward_controller; CANInstance *motor_can_instance; // CAN communication instance DaemonInstance *motor_daemon; // daemon watchdog @@ -75,6 +81,7 @@ typedef struct { Motor_Init_Config_s motor_config; // generic motor PID and setting config DM_Motor_Ctrl_Mode_e ctrl_mode; // default control mode + float DM_Feedforward_Kp; float mit_kp; // MIT mode KP (only for MIT mode) float mit_kd; // MIT mode KD (only for MIT mode) } DM_Motor_Init_Config_s;