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

feat: add 0xA9 feedforward frame and wire feedforward controller to motor output

Browse Source 
- 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
This commit is contained in:
李傲琦
2026-06-07 02:18:36 +08:00
parent c0f43940bd
commit e1a01e6b53
7 changed files with 91 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
application/cmd/robot_cmd.c
+26 -2
View File
@@ -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 参数调参 ---- */
application/gimbal/gimbal.c
+15 -3
View File
@@ -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);
/* 角度环参考值设定 */
if(SubGetMessage(gimbal_sub, &gimbal_cmd_recv))
{
DMMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw);
DMMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
}
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;
application/robot_def.h
+6
View File
@@ -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
modules/master_machine/master_machine.c
+7
View File
@@ -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])
modules/master_machine/master_machine.h
+15 -5
View File
@@ -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; // 有待处理命令标志
modules/motor/DMmotor/dmmotor.c
+14 -6
View File
@@ -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;
modules/motor/DMmotor/dmmotor.h
+7
View File
@@ -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;