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 参数调参 ---- */