SkyLiCoder
9b8e204ff7
- Add DEBUG mode (parallel to AUTO/MANUAL) for real-time PID parameter tuning - New 0xA8 debug frame: 12 floats for vision/motor angle PID tuning params - DMMotorSetMode: send zero-velocity before disable, clear PID to prevent jumps - motor_enable topic: pub from robot_cmd, sub in gimbal, controls motor on/off - motor_pid topic: pub from robot_cmd, sub in gimbal, updates motor angle PID - Rename DM motor enums (DM_CMD_ENABLE_MODE/DISABLE_MODE, DM_Motor_Ctrl_Mode_e) - SwitchToAuto/Manual: re-enable motors when switching back from DEBUG mode - Static-allocate enable_msg/pid_tune in GimbalTask to avoid stack churn - Add docs/host_comm_guide.md with Python Tkinter tuning GUI example - Remove ARCHITECTURE.md
127 lines
4.4 KiB
C
127 lines
4.4 KiB
C
#include "gimbal.h"
|
|
#include "robot_def.h"
|
|
#include "dmmotor.h"
|
|
#include "ins_task.h"
|
|
#include "message_center.h"
|
|
#include "general_def.h"
|
|
|
|
static INS_t *gimbal_IMU_data;
|
|
|
|
static Publisher_t *gimbal_pub;
|
|
static Subscriber_t *gimbal_sub;
|
|
static Subscriber_t *motor_enable_sub;
|
|
static Subscriber_t *motor_pid_sub;
|
|
static Gimbal_Upload_Data_s gimbal_feedback_data;
|
|
static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv;
|
|
|
|
static DMMotorInstance *yaw_motor; // DM-G6220
|
|
static DMMotorInstance *pitch_motor; // DM-J4310-2EC
|
|
|
|
void GimbalInit()
|
|
{
|
|
gimbal_IMU_data = INS_Init();
|
|
|
|
gimbal_pub = PubRegister("gimbal_feed", sizeof(Gimbal_Upload_Data_s));
|
|
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));
|
|
|
|
/* ---- Yaw: DM-G6220, CAN1, TX=0x01, RX=0xF1, Velocity模式 ---- */
|
|
DM_Motor_Init_Config_s yaw_config = {
|
|
.motor_config = {
|
|
.can_init_config = {
|
|
.can_handle = &hcan1,
|
|
.tx_id = 0x01,
|
|
.rx_id = 0xF1,
|
|
},
|
|
.controller_param_init_config = {
|
|
.angle_PID = {
|
|
.Kp = 30, .Ki = 80, .Kd = 0.2f,
|
|
.IntegralLimit = 1000,
|
|
.CoefA = 20, .CoefB = 10,
|
|
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |
|
|
PID_Derivative_On_Measurement | PID_ChangingIntegrationRate |
|
|
PID_DerivativeFilter | PID_OutputFilter,
|
|
.MaxOut = 2000,
|
|
},
|
|
.other_angle_feedback_ptr = &gimbal_IMU_data->YawTotalAngle,
|
|
.other_speed_feedback_ptr = &gimbal_IMU_data->Gyro[2],
|
|
},
|
|
.output_reverse = OUTPUT_DIRECTION_NORMAL,
|
|
.motor_type = DM_MOTOR,
|
|
},
|
|
.ctrl_mode = DM_MODE_VELOCITY,
|
|
.mit_kp = 0, .mit_kd = 0,
|
|
};
|
|
yaw_motor = DMMotorInit(&yaw_config);
|
|
|
|
/* ---- Pitch: DM-J4310-2EC, CAN1, TX=0x02, RX=0xF2, Velocity模式 ---- */
|
|
DM_Motor_Init_Config_s pitch_config = {
|
|
.motor_config = {
|
|
.can_init_config = {
|
|
.can_handle = &hcan1,
|
|
.tx_id = 0x02,
|
|
.rx_id = 0xF2,
|
|
},
|
|
.controller_param_init_config = {
|
|
.angle_PID = {
|
|
.Kp = 20, .Ki = 50, .Kd = 0.1f,
|
|
.IntegralLimit = 500,
|
|
.CoefA = 15, .CoefB = 5,
|
|
.Improve = PID_Trapezoid_Intergral | PID_Integral_Limit |
|
|
PID_Derivative_On_Measurement | PID_ChangingIntegrationRate,
|
|
.MaxOut = 1000,
|
|
},
|
|
.other_angle_feedback_ptr = &gimbal_IMU_data->Pitch,
|
|
.other_speed_feedback_ptr = &gimbal_IMU_data->Gyro[0],
|
|
},
|
|
.output_reverse = OUTPUT_DIRECTION_NORMAL,
|
|
.motor_type = DM_MOTOR,
|
|
},
|
|
.ctrl_mode = DM_MODE_VELOCITY,
|
|
.mit_kp = 0, .mit_kd = 0,
|
|
};
|
|
pitch_motor = DMMotorInit(&pitch_config);
|
|
|
|
gimbal_feedback_data.gimbal_imu_data = gimbal_IMU_data;
|
|
}
|
|
|
|
void GimbalTask()
|
|
{
|
|
SubGetMessage(gimbal_sub, &gimbal_cmd_recv);
|
|
|
|
DMMotorSetRef(yaw_motor, gimbal_cmd_recv.yaw);
|
|
DMMotorSetRef(pitch_motor, gimbal_cmd_recv.pitch);
|
|
|
|
/* 电机使能/失能控制 */
|
|
static Motor_Enable_s enable_msg;
|
|
if (SubGetMessage(motor_enable_sub, &enable_msg))
|
|
{
|
|
if (enable_msg.enable)
|
|
{
|
|
DMMotorSetMode(DM_CMD_ENABLE_MODE, yaw_motor);
|
|
DMMotorSetMode(DM_CMD_ENABLE_MODE, pitch_motor);
|
|
}
|
|
else
|
|
{
|
|
DMMotorSetMode(DM_CMD_DISABLE_MODE, yaw_motor);
|
|
DMMotorSetMode(DM_CMD_DISABLE_MODE, pitch_motor);
|
|
}
|
|
}
|
|
|
|
/* 电机角度环 PID 调参 */
|
|
static Motor_PID_Tune_s pid_tune;
|
|
if (SubGetMessage(motor_pid_sub, &pid_tune))
|
|
{
|
|
yaw_motor->motor_controller.angle_PID.Kp = pid_tune.yaw_Kp;
|
|
yaw_motor->motor_controller.angle_PID.Ki = pid_tune.yaw_Ki;
|
|
yaw_motor->motor_controller.angle_PID.Kd = pid_tune.yaw_Kd;
|
|
|
|
pitch_motor->motor_controller.angle_PID.Kp = pid_tune.pitch_Kp;
|
|
pitch_motor->motor_controller.angle_PID.Ki = pid_tune.pitch_Ki;
|
|
pitch_motor->motor_controller.angle_PID.Kd = pid_tune.pitch_Kd;
|
|
}
|
|
|
|
PubPushMessage(gimbal_pub, (void *)&gimbal_feedback_data);
|
|
}
|