From 7a9c0eb3feb1c2862643dc2dc5865252373e76b1 Mon Sep 17 00:00:00 2001 From: fcl <2136892861@qq.com> Date: Sat, 21 Mar 2026 18:44:25 +0800 Subject: [PATCH] =?UTF-8?q?=E9=9B=B6=E7=82=B9=E6=84=9F=E8=A7=89=E8=BF=98?= =?UTF-8?q?=E5=8F=AF=E4=BB=A5...?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- application/cmd/robot_cmd.c | 2 +- application/gimbal/gimbal.c | 18 +++++++- application/robot_def.h | 7 ++-- application/shoot/shoot.c | 74 ++++++++++++++++++--------------- modules/remote/remote_control.c | 2 +- 5 files changed, 64 insertions(+), 39 deletions(-) diff --git a/application/cmd/robot_cmd.c b/application/cmd/robot_cmd.c index 80e7480..f54cdeb 100644 --- a/application/cmd/robot_cmd.c +++ b/application/cmd/robot_cmd.c @@ -230,7 +230,7 @@ static void RemoteControlSet() shoot_cmd_send.friction_mode = FRICTION_OFF; // 拨弹控制,遥控器固定为一种拨弹模式,可自行选择 if (rc_data[TEMP].rc.dial < -500) - shoot_cmd_send.load_mode = LOAD_BURSTFIRE; + shoot_cmd_send.load_mode = LOAD_1_BULLET; else shoot_cmd_send.load_mode = LOAD_STOP; // 射频控制,固定每秒1发,后续可以根据左侧拨轮的值大小切换射频, diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index 8626f24..dd8cccc 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -14,7 +14,9 @@ static Publisher_t *gimbal_pub; // 云台应用消息发布者 static Subscriber_t *gimbal_sub; // cmd控制消息订阅者 static Gimbal_Upload_Data_s gimbal_feedback_data; // 回传给cmd的云台状态信息 static Gimbal_Ctrl_Cmd_s gimbal_cmd_recv; // 来自cmd的控制信息 - +static float last_yaw_angle = 0.0f; // 保护模式前的yaw角度 +// static float last_pitch_angle = 0.0f; // 保护模式前的pitch角度 +uint8_t in_protection_mode = 0; // 是否在保护模式 void GimbalInit() { gimba_IMU_data = INS_Init(); // IMU先初始化,获取姿态数据指针赋给yaw电机的其他数据来源 @@ -117,11 +119,25 @@ void GimbalTask() { // 停止 case GIMBAL_ZERO_FORCE: + // 进入保护模式时记录当前角度 + if (!in_protection_mode) { + last_yaw_angle = gimba_IMU_data->YawTotalAngle; + // last_pitch_angle = gimba_IMU_data->Pitch; + in_protection_mode = 1; + } DJIMotorStop(yaw_motor); DJIMotorStop(pitch_motor); break; // 使用陀螺仪的反馈,底盘根据yaw电机的offset跟随云台或视觉模式采用 case GIMBAL_GYRO_MODE: // 后续只保留此模式 + // 从保护模式退出时 + if (in_protection_mode) { + // 计算零飘导致的角度误差 + float angle_error = gimba_IMU_data->YawTotalAngle - last_yaw_angle; + // 补偿目标角度，避免突然旋转 + gimbal_cmd_recv.yaw -= angle_error; + in_protection_mode = 0; + } DJIMotorEnable(yaw_motor); DJIMotorEnable(pitch_motor); DJIMotorChangeFeed(yaw_motor, ANGLE_LOOP, OTHER_FEED); diff --git a/application/robot_def.h b/application/robot_def.h index 21e72aa..c98581f 100644 --- a/application/robot_def.h +++ b/application/robot_def.h @@ -41,9 +41,9 @@ #define STEERING_CHASSIS_ALIGN_ANGLE_RF STEERING_CHASSIS_ALIGN_ECD_RF / 8192.f * 360.f // 舵轮 A 对齐角度 #define STEERING_CHASSIS_ALIGN_ANGLE_LB STEERING_CHASSIS_ALIGN_ECD_LB / 8192.f * 360.f // 舵轮 D 对齐角度 // 发射参数 -#define ONE_BULLET_DELTA_ANGLE 36 // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出 -#define REDUCTION_RATIO_LOADER 36.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f -#define NUM_PER_CIRCLE 10 // 拨盘一圈的装载量 +#define ONE_BULLET_DELTA_ANGLE 51.0f // 发射一发弹丸拨盘转动的距离,由机械设计图纸给出 +#define REDUCTION_RATIO_LOADER 19.0f // 拨盘电机的减速比,英雄需要修改为3508的19.0f +#define NUM_PER_CIRCLE 7 // 拨盘一圈的装载量 // 机器人底盘修改的参数,单位为mm(毫米) #define WHEEL_BASE 350 // 纵向轴距(前进后退方向) #define TRACK_WIDTH 300 // 横向轮距(左右平移方向) @@ -178,6 +178,7 @@ typedef struct Bullet_Speed_e bullet_speed; // 弹速枚举 uint8_t rest_heat; float shoot_rate; // 连续发射的射频,unit per s,发/秒 + float dead_time; // 每发弹丸的间隔时间,unit ms,毫秒 } Shoot_Ctrl_Cmd_s; /* ----------------gimbal/shoot/chassis发布的反馈数据----------------*/ diff --git a/application/shoot/shoot.c b/application/shoot/shoot.c index 1d0c4d7..e6aff3a 100644 --- a/application/shoot/shoot.c +++ b/application/shoot/shoot.c @@ -68,33 +68,36 @@ void ShootInit() .controller_param_init_config = { .angle_PID = { // 如果启用位置环来控制发弹,需要较大的I值保证输出力矩的线性度否则出现接近拨出的力矩大幅下降 - .Kp = 10, // 10 - .Ki = 0, - .Kd = 0, - .MaxOut = 200, + .Kp = 50, + .Ki = 0, + .Kd = 0, + .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement | PID_DerivativeFilter | PID_ErrorHandle, + .IntegralLimit = 10000, + .MaxOut = 15000, + .Derivative_LPF_RC = 0.01, }, .speed_PID = { - .Kp = 10, // 10 - .Ki = 1, // 1 - .Kd = 0, - .Improve = PID_Integral_Limit, - .IntegralLimit = 5000, - .MaxOut = 5000, + .Kp = 5, // 10 + .Ki = 100, // 1 + .Kd = 0, + .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement, + .IntegralLimit = 10000, + .MaxOut = 15000, }, .current_PID = { - .Kp = 0.7, // 0.7 - .Ki = 0.1, // 0.1 - .Kd = 0, - .Improve = PID_Integral_Limit, - .IntegralLimit = 5000, - .MaxOut = 5000, + .Kp = 0.7, // 0.7 + .Ki = 0, // 0.1 + .Kd = 0, + .Improve = PID_Integral_Limit, + .IntegralLimit = 10000, + .MaxOut = 15000, }, }, .controller_setting_init_config = { .angle_feedback_source = MOTOR_FEED, .speed_feedback_source = MOTOR_FEED, .outer_loop_type = SPEED_LOOP, // 初始化成SPEED_LOOP,让拨盘停在原地,防止拨盘上电时乱转 - .close_loop_type = CURRENT_LOOP | SPEED_LOOP, - .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, // 注意方向设置为拨盘的拨出的击发方向 + .close_loop_type = CURRENT_LOOP | SPEED_LOOP| ANGLE_LOOP, + .motor_reverse_flag = MOTOR_DIRECTION_REVERSE, // 注意方向设置为拨盘的拨出的击发方向 }, .motor_type = M2006 // 英雄使用m3508 }; @@ -126,8 +129,8 @@ void ShootTask() // 如果上一次触发单发或3发指令的时间加上不应期仍然大于当前时间(尚未休眠完毕),直接返回即可 // 单发模式主要提供给能量机关激活使用(以及英雄的射击大部分处于单发) - // if (hibernate_time + dead_time > DWT_GetTimeline_ms()) - // return; + if (hibernate_time + dead_time > DWT_GetTimeline_ms()) + return; // 若不在休眠状态,根据robotCMD传来的控制模式进行拨盘电机参考值设定和模式切换 switch (shoot_cmd_recv.load_mode) @@ -138,11 +141,16 @@ void ShootTask() DJIMotorSetRef(loader, 0); // 同时设定参考值为0,这样停止的速度最快 break; // 单发模式,根据鼠标按下的时间,触发一次之后需要进入不响应输入的状态(否则按下的时间内可能多次进入,导致多次发射) - case LOAD_1_BULLET: // 激活能量机关/干扰对方用,英雄用. - DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到角度环 - DJIMotorSetRef(loader, loader->measure.total_angle + ONE_BULLET_DELTA_ANGLE); // 控制量增加一发弹丸的角度 - hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间 - dead_time = 150; // 完成1发弹丸发射的时间 + case LOAD_1_BULLET: // 激活能量机关/干扰对方用,英雄用. + DJIMotorOuterLoop(loader, ANGLE_LOOP); // 切换到角度环 + static float last_target_angle = 0; // 存储上次的目标角度 + if (dead_time == 0) { // 第一次触发 + last_target_angle = loader->measure.total_angle; + } + last_target_angle += ONE_BULLET_DELTA_ANGLE * REDUCTION_RATIO_LOADER; // 每次都在上次目标角度基础上增加固定角度 + DJIMotorSetRef(loader, last_target_angle); + hibernate_time = DWT_GetTimeline_ms(); // 记录触发指令的时间 + dead_time = shoot_cmd_recv.dead_time; // 完成1发弹丸发射的时间 break; // 三连发,如果不需要后续可能删除 case LOAD_3_BULLET: @@ -175,20 +183,20 @@ void ShootTask() switch (shoot_cmd_recv.bullet_speed) { case SMALL_AMU_15: - DJIMotorSetRef(friction_l, 30000); - DJIMotorSetRef(friction_r, 30000); + DJIMotorSetRef(friction_l, 58000); + DJIMotorSetRef(friction_r, 58000); break; case SMALL_AMU_18: - DJIMotorSetRef(friction_l, 30000); - DJIMotorSetRef(friction_r, 30000); + DJIMotorSetRef(friction_l, 58000); + DJIMotorSetRef(friction_r, 58000); break; case SMALL_AMU_30: - DJIMotorSetRef(friction_l, 50000); - DJIMotorSetRef(friction_r, 50000); + DJIMotorSetRef(friction_l, 58000); + DJIMotorSetRef(friction_r, 58000); break; default: // 当前为了调试设定的默认值4000,因为还没有加入裁判系统无法读取弹速. - DJIMotorSetRef(friction_l, 30000); - DJIMotorSetRef(friction_r, 30000); + DJIMotorSetRef(friction_l, 58000); + DJIMotorSetRef(friction_r, 58000); break; } } diff --git a/modules/remote/remote_control.c b/modules/remote/remote_control.c index 0949f6d..8bc32b1 100644 --- a/modules/remote/remote_control.c +++ b/modules/remote/remote_control.c @@ -40,7 +40,7 @@ static void sbus_to_rc(const uint8_t *sbus_buf) rc_ctrl[TEMP].rc.rocker_l_ = (((sbus_buf[2] >> 6) | (sbus_buf[3] << 2) | (sbus_buf[4] << 10)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 2 rc_ctrl[TEMP].rc.rocker_l1 = (((sbus_buf[4] >> 1) | (sbus_buf[5] << 7)) & 0x07ff) - RC_CH_VALUE_OFFSET; //!< Channel 3 rc_ctrl[TEMP].rc.dial = ((sbus_buf[16] | (sbus_buf[17] << 8)) & 0x07FF) - RC_CH_VALUE_OFFSET; - rc_ctrl[TEMP].rc.rocker_l_= rc_ctrl[TEMP].rc.rocker_l_+3; + rc_ctrl[TEMP].rc.rocker_l_= rc_ctrl[TEMP].rc.rocker_l_+2.5; // 左侧拨轮 RectifyRCjoystick(); // 开关,0左1右