diff --git a/User/module/cmd.c b/User/module/cmd.c index acd3bee..deb7c98 100644 --- a/User/module/cmd.c +++ b/User/module/cmd.c @@ -61,6 +61,7 @@ static void CMD_RC_BuildGimbalCmd(CMD_t *ctx) { /* 右摇杆控制云台 */ ctx->output.gimbal.cmd.delta_yaw_6020 = -ctx->input.rc.joy_right.x * 2.0f; + ctx->output.gimbal.cmd.delta_yaw_4310 = -ctx->input.rc.joy_right.x * 1.2f; ctx->output.gimbal.cmd.delta_pitch_4310 = -ctx->input.rc.joy_right.y * 1.5f; } @@ -71,19 +72,19 @@ static void CMD_RC_BuildShootCmd(CMD_t *ctx) { } else { ctx->output.shoot.cmd.mode = SHOOT_MODE_SAFE; } - /* 根据c拨杆控制射击 */ + /* C拨杆选择发弹模式:上单发,中多发,下连发 */ switch (ctx->input.rc.sw[2]) { case CMD_SW_DOWN: - ctx->output.shoot.cmd.mode = SHOOT_MODE_BURST; + ctx->output.shoot.cmd.mode = SHOOT_MODE_CONTINUE; break; case CMD_SW_MID: - ctx->output.shoot.cmd.mode = SHOOT_MODE_SINGLE; + ctx->output.shoot.cmd.mode = SHOOT_MODE_BURST; break; case CMD_SW_UP: - ctx->output.shoot.cmd.ready = SHOOT_MODE_SAFE; + ctx->output.shoot.cmd.mode = SHOOT_MODE_SINGLE; + break; default: - ctx->output.shoot.cmd.ready = false; - ctx->output.shoot.cmd.firecmd = false; + ctx->output.shoot.cmd.mode = SHOOT_MODE_SAFE; break; } /* 根据D拨杆控制射击 */ @@ -108,18 +109,22 @@ static void CMD_RC_BuildShootCmd(CMD_t *ctx) { case CMD_SW_UP: ctx->output.chassis.cmd.ctrl_mode = CHASSIS_MODE_RC; ctx->output.gimbal.cmd.ctrl_mode = GIMBAL_MODE_REMOTE; + ctx->output.shoot.cmd.control_mode = SHOOT_REMOTE; break; case CMD_SW_MID: ctx->output.chassis.cmd.ctrl_mode = CHASSIS_MODE_DAOHANG; ctx->output.gimbal.cmd.mode = GIMBAL_MODE_DAOHANG; + ctx->output.shoot.cmd.control_mode = SHOOT_REMOTE; break; case CMD_SW_DOWN: ctx->output.chassis.cmd.ctrl_mode = CHASSIS_MODE_RELAX; ctx->output.gimbal.cmd.ctrl_mode = GIMBAL_MODE_AI; + ctx->output.shoot.cmd.control_mode = SHOOT_AI; break; default: ctx->output.chassis.cmd.mode = CHASSIS_MODE_RELAX; ctx->output.gimbal.cmd.mode = GIMBAL_MODE_RELAX; + ctx->output.shoot.cmd.control_mode = SHOOT_REMOTE; break; } diff --git a/User/module/gimbal.c b/User/module/gimbal.c index ec77282..3ce354f 100644 --- a/User/module/gimbal.c +++ b/User/module/gimbal.c @@ -54,13 +54,21 @@ static uint8_t g_scan_active = 0; static float g_scan_pitch_center = 0.0f; static float g_scan_pitch_dir = 1.0f; static float g_scan_yaw_dir = 1.0f; +static int8_t g_scan_yaw_edge_latch = 0; /* -1: min边界锁存, 1: max边界锁存 */ -#define GIMBAL_SCAN_YAW_SPEED (0.55f) -#define GIMBAL_SCAN_PITCH_SPEED (0.35f) +#define GIMBAL_SCAN_SMALL_YAW_SPEED (0.20f) +#define GIMBAL_SCAN_BIG_YAW_PERIOD_SEC (5.0f) +#define GIMBAL_SCAN_PITCH_SPEED (0.16f) #define GIMBAL_SCAN_PITCH_AMPLITUDE (0.18f) #define GIMBAL_SCAN_YAW_EDGE_MARGIN (0.16f) +#define GIMBAL_SCAN_YAW_EDGE_HYST (0.22f) +#define GIMBAL_SCAN_YAW_EDGE_RELEASE (0.28f) #define GIMBAL_SCAN_PITCH_EDGE_MARGIN (0.12f) #define GIMBAL_SCAN_EDGE_SLOW_BAND (0.26f) +#define GIMBAL_REMOTE_BIG_YAW_SPEED_GAIN (1.6f) +#define GIMBAL_REMOTE_EDGE_ASSIST_BAND (0.20f) +#define GIMBAL_YAW6020_INPUT_DEADBAND (0.0005f) +#define GIMBAL_CONTROL_DT_MAX (0.02f) @@ -98,6 +106,7 @@ static int8_t Gimbal_SetMode(Gimbal_t *g, Gimbal_Mode_t mode) { g->setpoint.eulr.pit = g->feedback.imu.eulr.rol; g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw; + g->setpoint.yaw_4310 = g->feedback.motor.yaw_4310_motor_feedback.rotor_abs_angle; g->mode = mode; return GIMBAL_OK; @@ -124,6 +133,8 @@ int8_t Gimbal_Init(Gimbal_t *g, Gimbal_Param_t *param,float target_freq) } g->param = param; g->mode = GIMBAL_MODE_RELAX; /* 设置默认模式 不输出*/ + g->last_wakeup = BSP_TIME_Get_us(); + g->dt = 0.0f; /*6020小yaw pid初始化*/ PID_Init(&g->pid.yaw_6020_angle,KPID_MODE_NO_D, target_freq,&g->param->pid.yaw_6020_motor_angle ); PID_Init(&g->pid.yaw_6020_omega,KPID_MODE_CALC_D, target_freq,&g->param->pid.yaw_6020_motor_omega ); @@ -144,6 +155,8 @@ int8_t Gimbal_Init(Gimbal_t *g, Gimbal_Param_t *param,float target_freq) /*机械限位*/ g->limit.yaw_6020.max = g->param->mech_zero.yaw_6020 + g->param->travel.yaw_6020; g->limit.yaw_6020.min = g->param->mech_zero.yaw_6020 ; + g->limit.yaw_4310.max = g->param->mech_zero.yaw_4310 + g->param->travel.yaw_4310; + g->limit.yaw_4310.min = g->param->mech_zero.yaw_4310 ; g->limit.pitch_4310.max = g->param->mech_zero.pitch_4310 + g->param->travel.pitch_4310; g->limit.pitch_4310.min = g->param->mech_zero.pitch_4310 ; @@ -156,7 +169,7 @@ int8_t Gimbal_Init(Gimbal_t *g, Gimbal_Param_t *param,float target_freq) return GIMBAL_OK; } - /** +/** * \brief 通过CAN设备更新云台电机反馈信息 * * \param gimbal 云台 @@ -164,7 +177,6 @@ int8_t Gimbal_Init(Gimbal_t *g, Gimbal_Param_t *param,float target_freq) * * \return 函数运行结果 */ - int8_t Gimbal_UpdateFeedback(Gimbal_t *gimbal) { if (gimbal == NULL) @@ -257,28 +269,59 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){ return GIMBAL_ERR; // 参数错误 } - g->dt = (BSP_TIME_Get_us() - g->last_wakeup) / 1000000.0f; - g->last_wakeup = BSP_TIME_Get_us(); + { + uint64_t now_us = BSP_TIME_Get_us(); + g->dt = (now_us - g->last_wakeup) / 1000000.0f; + g->last_wakeup = now_us; + if (g->dt < 0.0f || g->dt > GIMBAL_CONTROL_DT_MAX) { + g->dt = 0.002f; + } + } Gimbal_SetMode(g, g_cmd->mode); if (g_cmd->ctrl_mode == GIMBAL_MODE_SCAN) { float yaw_speed_scale = 1.0f; + float big_yaw_speed = 0.0f; float pitch_speed_scale = 1.0f; float yaw_abs = g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle; float pitch_abs = g->feedback.motor.pitch_4310_motor_feedback.rotor_abs_angle; if (!g_scan_active) { + float yaw_mid = 0.5f * (g->limit.yaw_6020.max + g->limit.yaw_6020.min); g_scan_active = 1; g_scan_pitch_center = g->feedback.imu.eulr.pit; g_scan_pitch_dir = 1.0f; - g_scan_yaw_dir = 1.0f; + g_scan_yaw_dir = (yaw_abs >= yaw_mid) ? -1.0f : 1.0f; + g_scan_yaw_edge_latch = 0; + g->setpoint.yaw_4310 = g->feedback.motor.yaw_4310_motor_feedback.rotor_abs_angle; } - if (yaw_abs >= g->limit.yaw_6020.max - GIMBAL_SCAN_YAW_EDGE_MARGIN) { + if (GIMBAL_SCAN_BIG_YAW_PERIOD_SEC > 0.0f) { + /* 连续旋转:按周期计算角速度,5秒转一圈即 2PI/5 */ + big_yaw_speed = M_2PI / GIMBAL_SCAN_BIG_YAW_PERIOD_SEC; + } + + if (g_scan_yaw_edge_latch == 1) { g_scan_yaw_dir = -1.0f; + if (yaw_abs < g->limit.yaw_6020.max - GIMBAL_SCAN_YAW_EDGE_RELEASE) { + g_scan_yaw_edge_latch = 0; + } + } else if (g_scan_yaw_edge_latch == -1) { + g_scan_yaw_dir = 1.0f; + if (yaw_abs > g->limit.yaw_6020.min + GIMBAL_SCAN_YAW_EDGE_RELEASE) { + g_scan_yaw_edge_latch = 0; + } + } else if (yaw_abs >= g->limit.yaw_6020.max - GIMBAL_SCAN_YAW_EDGE_MARGIN) { + g_scan_yaw_dir = -1.0f; + g_scan_yaw_edge_latch = 1; } else if (yaw_abs <= g->limit.yaw_6020.min + GIMBAL_SCAN_YAW_EDGE_MARGIN) { g_scan_yaw_dir = 1.0f; + g_scan_yaw_edge_latch = -1; + } else if (g_scan_yaw_dir > 0.0f && yaw_abs > g->limit.yaw_6020.max - GIMBAL_SCAN_YAW_EDGE_HYST) { + g_scan_yaw_dir = -1.0f; + } else if (g_scan_yaw_dir < 0.0f && yaw_abs < g->limit.yaw_6020.min + GIMBAL_SCAN_YAW_EDGE_HYST) { + g_scan_yaw_dir = 1.0f; } if (pitch_abs >= g->limit.pitch_4310.max - GIMBAL_SCAN_PITCH_EDGE_MARGIN) { @@ -293,7 +336,7 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){ float yaw_min_dist = (yaw_dist_to_edge_max < yaw_dist_to_edge_min) ? yaw_dist_to_edge_max : yaw_dist_to_edge_min; if (yaw_min_dist < GIMBAL_SCAN_EDGE_SLOW_BAND) { if (yaw_min_dist < 0.0f) yaw_min_dist = 0.0f; - yaw_speed_scale = 0.25f + 0.75f * (yaw_min_dist / GIMBAL_SCAN_EDGE_SLOW_BAND); + yaw_speed_scale = (yaw_min_dist / GIMBAL_SCAN_EDGE_SLOW_BAND); } } @@ -313,8 +356,15 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){ g_scan_pitch_dir = 1.0f; } - g_cmd->delta_yaw_6020 = g_scan_yaw_dir * GIMBAL_SCAN_YAW_SPEED * yaw_speed_scale; + g_cmd->delta_yaw_6020 = g_scan_yaw_dir * GIMBAL_SCAN_SMALL_YAW_SPEED * yaw_speed_scale; g_cmd->delta_pitch_4310 = g_scan_pitch_dir * GIMBAL_SCAN_PITCH_SPEED * pitch_speed_scale; + if (g->param->travel.yaw_4310 > 0.0f) { + g->setpoint.yaw_4310 += big_yaw_speed * g->dt; + if (g->setpoint.yaw_4310 > g->limit.yaw_4310.max) g->setpoint.yaw_4310 = g->limit.yaw_4310.max; + if (g->setpoint.yaw_4310 < g->limit.yaw_4310.min) g->setpoint.yaw_4310 = g->limit.yaw_4310.min; + } else { + CircleAdd(&(g->setpoint.yaw_4310), big_yaw_speed * g->dt, M_2PI); + } g_cmd->yaw_vel = 0.0f; g_cmd->yaw_accl = 0.0f; g_cmd->pit_vel = 0.0f; @@ -322,30 +372,37 @@ int8_t Gimbal_UpdateIMU(Gimbal_t *gimbal, const Gimbal_IMU_t *imu){ } else { g_scan_active = 0; g_scan_yaw_dir = 1.0f; + g_scan_yaw_edge_latch = 0; } /*处理小yaw轴控制命令*/ -float delta_yaw_6020 = 10.0f*g_cmd->delta_yaw_6020 * g->dt; +float delta_yaw_6020 = 10.0f * g_cmd->delta_yaw_6020 * g->dt; +float yaw_abs_now = g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle; +float yaw_abs_target = yaw_abs_now; -if(g->param->travel.yaw_6020 > 0) // 有限位才处理 -{ -/* 计算当前电机角度与IMU角度的偏差 */ -/*float*/ - motor_imu_offset_6020 = g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle - g->feedback.imu.eulr.yaw; -/* 处理跨越±π的情况 */ -if (motor_imu_offset_6020 > M_PI) motor_imu_offset_6020 -= M_2PI; -if (motor_imu_offset_6020 < -M_PI) motor_imu_offset_6020 += M_2PI; - -/*计算限位距离*/ -const float delta_max_6020 = CircleError(g->limit.yaw_6020.max, - (g->setpoint.eulr.yaw + motor_imu_offset_6020 + delta_yaw_6020), M_2PI); -const float delta_min_6020 = CircleError(g->limit.yaw_6020.min, - (g->setpoint.eulr.yaw + motor_imu_offset_6020 + delta_yaw_6020), M_2PI); - - if(delta_yaw_6020 > delta_max_6020) delta_yaw_6020 = delta_max_6020; - if(delta_yaw_6020 < delta_min_6020) delta_yaw_6020 = delta_min_6020; +/* 摇杆无输入时抑制微小噪声,避免setpoint慢漂 */ +if (fabsf(delta_yaw_6020) < GIMBAL_YAW6020_INPUT_DEADBAND) { + delta_yaw_6020 = 0.0f; } + +if (g->param->travel.yaw_6020 > 0.0f) { + /* 边界方向判定:到边界时只允许朝边界内侧运动 */ + if (yaw_abs_now >= g->limit.yaw_6020.max - GIMBAL_SCAN_YAW_EDGE_MARGIN && delta_yaw_6020 > 0.0f) { + delta_yaw_6020 = 0.0f; + } + if (yaw_abs_now <= g->limit.yaw_6020.min + GIMBAL_SCAN_YAW_EDGE_MARGIN && delta_yaw_6020 < 0.0f) { + delta_yaw_6020 = 0.0f; + } + + yaw_abs_target = yaw_abs_now + delta_yaw_6020; + if (yaw_abs_target > g->limit.yaw_6020.max) yaw_abs_target = g->limit.yaw_6020.max; + if (yaw_abs_target < g->limit.yaw_6020.min) yaw_abs_target = g->limit.yaw_6020.min; + + /* 直接用允许的电机角增量映射到欧拉角目标,避免CircleAdd累积导致回弹 */ + g->setpoint.eulr.yaw = g->feedback.imu.eulr.yaw + (yaw_abs_target - yaw_abs_now); +} else { CircleAdd(&(g->setpoint.eulr.yaw), delta_yaw_6020, M_2PI); +} // g->setpoint.eulr.yaw = g->setpoint.NUC_Yaw; @@ -353,9 +410,30 @@ const float delta_min_6020 = CircleError(g->limit.yaw_6020.min, /*获得小YAW的中点位置,如果小YAW大于中点的一定范围,大YAW开始运动, 跟随小YAW向相同方向运动,将遥控数据进行分配*/ - /*小YAW中点*/ - float small_yaw_center_offset = g->param->mech_zero.yaw_6020 + g->param->travel.yaw_6020* 0.5f ; - g->setpoint.yaw_4310=small_yaw_center_offset; + if (g_cmd->ctrl_mode == GIMBAL_MODE_REMOTE) { + float delta_yaw_4310 = -GIMBAL_REMOTE_BIG_YAW_SPEED_GAIN * g_cmd->delta_yaw_4310 * g->dt; + if (g->param->travel.yaw_6020 > 0.0f) { + float yaw_abs_now = g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle; + float dist_to_max = g->limit.yaw_6020.max - yaw_abs_now; + float dist_to_min = yaw_abs_now - g->limit.yaw_6020.min; + float edge_assist = 1.0f; + if (g_cmd->delta_yaw_6020 > 0.0f && dist_to_max < GIMBAL_REMOTE_EDGE_ASSIST_BAND) { + edge_assist += (GIMBAL_REMOTE_EDGE_ASSIST_BAND - dist_to_max) / GIMBAL_REMOTE_EDGE_ASSIST_BAND; + } else if (g_cmd->delta_yaw_6020 < 0.0f && dist_to_min < GIMBAL_REMOTE_EDGE_ASSIST_BAND) { + edge_assist += (GIMBAL_REMOTE_EDGE_ASSIST_BAND - dist_to_min) / GIMBAL_REMOTE_EDGE_ASSIST_BAND; + } + if (edge_assist < 1.0f) edge_assist = 1.0f; + if (edge_assist > 2.0f) edge_assist = 2.0f; + delta_yaw_4310 *= edge_assist; + } + if (g->param->travel.yaw_4310 > 0.0f) { + g->setpoint.yaw_4310 += delta_yaw_4310; + if (g->setpoint.yaw_4310 > g->limit.yaw_4310.max) g->setpoint.yaw_4310 = g->limit.yaw_4310.max; + if (g->setpoint.yaw_4310 < g->limit.yaw_4310.min) g->setpoint.yaw_4310 = g->limit.yaw_4310.min; + } else { + CircleAdd(&(g->setpoint.yaw_4310), delta_yaw_4310, M_2PI); + } + } /*处理大pitch控制命令*/ float delta_pitch_4310 = 8.0f*g_cmd->delta_pitch_4310*g->dt; @@ -433,7 +511,7 @@ switch (g->mode) yaw_omega_set_point =PID_Calc(&g->pid.yaw_4310_angle,g->setpoint.yaw_4310, - g->feedback.motor.yaw_6020_motor_feedback.rotor_abs_angle,g->feedback.motor.yaw_6020_motor_feedback.rotor_speed,g->dt); + g->feedback.motor.yaw_4310_motor_feedback.rotor_abs_angle,g->feedback.motor.yaw_4310_motor_feedback.rotor_speed,g->dt); g->out.yaw_4310 = PID_Calc(&g->pid.yaw_4310_omega,yaw_omega_set_point, g->feedback.motor.yaw_4310_motor_feedback.rotor_speed,0.0f,g->dt); // g->out.yaw_4310 = yaw_omega_set_point; // 直接输出速度环目标值作为电机输出 diff --git a/User/task/gimbal.c b/User/task/gimbal.c index af9d663..e78cdcc 100644 --- a/User/task/gimbal.c +++ b/User/task/gimbal.c @@ -66,8 +66,10 @@ osMessageQueueGet(task_runtime.msgq.gimbal.cmd, &cmd_gimbal, NULL, 0); if (cmd_gimbal.ctrl_mode == GIMBAL_MODE_AI) { if (ai_gimbal_result_cmd.mode == 0) { + cmd_gimbal.mode = GIMBAL_MODE_ABSOLUTE; cmd_gimbal.ctrl_mode = GIMBAL_MODE_SCAN; } else { + cmd_gimbal.mode = GIMBAL_MODE_ABSOLUTE; cmd_gimbal.ctrl_mode = GIMBAL_MODE_AI; cmd_gimbal.set_pitch = ai_gimbal_result_cmd.gimbal_t.setpoint.pit; cmd_gimbal.set_yaw = ai_gimbal_result_cmd.gimbal_t.setpoint.yaw; diff --git a/User/task/shoot.c b/User/task/shoot.c index 0e7bc27..25a52db 100644 --- a/User/task/shoot.c +++ b/User/task/shoot.c @@ -41,6 +41,10 @@ Shoot_SetMode(&shoot,SHOOT_MODE_SINGLE); shoot_ai_mcu_package.data.bullet_count = 0; static bool last_fire_state = false; bool current_fire_state = false; // 当前是否需要发射 +static uint32_t ai_last_fire_tick = 0; +static const uint32_t ai_single_interval_ms = 220; +static const uint32_t ai_burst_interval_ms = 110; +static const uint32_t ai_continue_interval_ms = 0; /* USER CODE INIT END */ while (1) { @@ -56,20 +60,39 @@ if(shoot_cmd.control_mode==SHOOT_REMOTE) } else if(shoot_cmd.control_mode==SHOOT_AI) { -// shoot_cmd.mode = SHOOT_MODE_SINGLE; -// if(shoot_ai_result_cmd.mode==0 || shoot_ai_result_cmd.mode==1) -// { -// shoot_cmd.firecmd=false; -// shoot_cmd.ready=true; + uint32_t now_ms = osKernelGetTickCount(); + uint32_t interval_ms = ai_single_interval_ms; -// } -// else if(shoot_ai_result_cmd.mode==2) -// { -// shoot_cmd.firecmd=true; -// shoot_cmd.ready=true; -// current_fire_state = true; -// } -current_fire_state = shoot_cmd.firecmd; + shoot_cmd.ready = true; + + if (shoot_ai_result_cmd.mode == 2) { + switch (shoot_cmd.mode) { + case SHOOT_MODE_CONTINUE: + interval_ms = ai_continue_interval_ms; + break; + case SHOOT_MODE_BURST: + interval_ms = ai_burst_interval_ms; + break; + case SHOOT_MODE_SINGLE: + default: + interval_ms = ai_single_interval_ms; + break; + } + + if (interval_ms == 0) { + shoot_cmd.firecmd = true; + } else if ((now_ms - ai_last_fire_tick) >= interval_ms) { + shoot_cmd.firecmd = true; + ai_last_fire_tick = now_ms; + } else { + shoot_cmd.firecmd = false; + } + } else { + shoot_cmd.firecmd = false; + ai_last_fire_tick = now_ms; + } + + current_fire_state = shoot_cmd.firecmd; } if(current_fire_state == true && last_fire_state == false)