diff --git a/MDK-ARM/.vscode/uv4.log b/MDK-ARM/.vscode/uv4.log index 0d6c20e..12c1513 100644 --- a/MDK-ARM/.vscode/uv4.log +++ b/MDK-ARM/.vscode/uv4.log @@ -1,8 +1,9 @@ *** Using Compiler 'V5.06 update 7 (build 960)', folder: 'D:\keil\ARM\ARMCC\Bin' Build target 'R1' +compiling ball.cpp... compiling shoot.cpp... linking... -Program Size: Code=31636 RO-data=1832 RW-data=272 ZI-data=32248 +Program Size: Code=31632 RO-data=1832 RW-data=268 ZI-data=32252 FromELF: creating hex file... "R1\R1.axf" - 0 Error(s), 0 Warning(s). -Build Time Elapsed: 00:00:05 +Build Time Elapsed: 00:00:06 diff --git a/MDK-ARM/.vscode/uv4.log.lock b/MDK-ARM/.vscode/uv4.log.lock index 29334e0..87abbbb 100644 --- a/MDK-ARM/.vscode/uv4.log.lock +++ b/MDK-ARM/.vscode/uv4.log.lock @@ -1 +1 @@ -2025/6/26 19:15:12 \ No newline at end of file +2025/6/26 22:06:06 \ No newline at end of file diff --git a/User/bsp/TopDefine.h b/User/bsp/TopDefine.h index 190ffdd..261faac 100644 --- a/User/bsp/TopDefine.h +++ b/User/bsp/TopDefine.h @@ -18,9 +18,6 @@ #define ONE_CONTROL 1 - - - //是否使用大疆DT7遥控器 #ifndef DT7 #define DT7 0 diff --git a/User/module/ball.cpp b/User/module/ball.cpp index d422d68..91b5c00 100644 --- a/User/module/ball.cpp +++ b/User/module/ball.cpp @@ -258,8 +258,7 @@ void Ball::ball_control() } - - + // 发送小米电机控制数据 Send_control(); } @@ -322,7 +321,11 @@ void Ball::Idle_control() xiaomi.position = WAIT_POS; if(feedback->position_deg >= WAIT_POS-3) { - osThreadFlagsSet(task_struct.thread.shoot, READY_TELL); + // 只在READY_TELL未置位时发送，防止重复 + if((osThreadFlagsGet() & READY_TELL) == 0) + { + osThreadFlagsSet(task_struct.thread.shoot, READY_TELL); + } } } else diff --git a/User/module/shoot.cpp b/User/module/shoot.cpp index 4a62041..2337095 100644 --- a/User/module/shoot.cpp +++ b/User/module/shoot.cpp @@ -13,17 +13,17 @@ extern RC_ctrl_t rc_ctrl; NUC_t nuc_v; -//sw[7]👆 1694 中 1000 👇306 -//sw[2]👆 1694 👇306 +// sw[7]👆 1694 中 1000 👇306 +// sw[2]👆 1694 👇306 -//E键 sw[1]👆 200 shoot 中 1000 stop 👇1800 error -//F键 sw[0]👆 1800 开 👇200 关 -//B键 sw[3]👆 200 开 👇1800 关 -//左旋钮 sw[4] 👈 200 👉1800 +// E键 sw[1]👆 200 shoot 中 1000 stop 👇1800 error +// F键 sw[0]👆 1800 开 👇200 关 +// B键 sw[3]👆 200 开 👇1800 关 +// 左旋钮 sw[4] 👈 200 👉1800 -//尽量别动这组pid -const fp32 Shoot:: M2006_speed_PID[3] = {5, 0, 0.01}; -const fp32 Shoot:: M2006_angle_PID[3] = { 15, 0.1, 0}; +// 尽量别动这组pid +const fp32 Shoot::M2006_speed_PID[3] = {5, 0, 0.01}; +const fp32 Shoot::M2006_angle_PID[3] = {15, 0.1, 0}; #define TO_TOP 10.0f #define TO_BOTTOM 5.0f @@ -31,70 +31,64 @@ const fp32 Shoot:: M2006_angle_PID[3] = { 15, 0.1, 0}; #define INIT_POS -130 #define TOP_POS -211 #define BOTTOM_POS 0 -#define GO_ERROR 1.0f -#define Tigger_DO 0 -#define Tigger_ZERO 100 +#define GO_ERROR 1.0f +#define Tigger_DO 0 +#define Tigger_ZERO 100 #define Tigger_ERROR 3 - float knob_increment; Shoot::Shoot() { - //扳机初始化 - trigger_Motor= get_motor_point(0);//id 201 - trigger_Motor->type=M2006; - PID_init(&speed_pid,PID_POSITION,M2006_speed_PID,5000, 2000);//pid初始化 - PID_init(&angle_pid,PID_POSITION,M2006_angle_PID,5000, 2000);//pid初始化 - t_speedSet = 0; + // 扳机初始化 + trigger_Motor = get_motor_point(0); // id 201 + trigger_Motor->type = M2006; + PID_init(&speed_pid, PID_POSITION, M2006_speed_PID, 5000, 2000); // pid初始化 + PID_init(&angle_pid, PID_POSITION, M2006_angle_PID, 5000, 2000); // pid初始化 + t_speedSet = 0; result = 0; - - go1.id =1, + go1.id = 1, go1.mode = 1, go1.K_P = 1.0f, go1.K_W = 0.05, - go1.Pos = 0,//上电先到一个舒服的位置 - go1.W = 0, + go1.Pos = 0, // 上电先到一个舒服的位置 + go1.W = 0, go1.T = 0, - limit_speed=TO_TOP;//快速上去 - fire_pos = INIT_POS; // 发射位置 + limit_speed = TO_TOP; // 快速上去 + fire_pos = INIT_POS; // 发射位置 - BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART1_RxCompleteCallback);//注册串口回调函数，bsp层 + BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART1_RxCompleteCallback); // 注册串口回调函数，bsp层 - go1_data = get_GO_measure_point();//go1数据 + go1_data = get_GO_measure_point(); // go1数据 - currentState= SHOOT_IDLE; - - LowPassFilter2p_Init(&distance_filter,500.0f,80.0f); //给distance 做滤波 + currentState = SHOOT_IDLE; + LowPassFilter2p_Init(&distance_filter, 500.0f, 80.0f); // 给distance 做滤波 } void Shoot::trigger_control() { int delta = 0; - delta = PID_calc(&angle_pid,trigger_Motor->total_angle,t_posSet); // 计算位置环PID - result = PID_calc(&speed_pid,trigger_Motor->speed_rpm,delta); // 计算速度环PID + delta = PID_calc(&angle_pid, trigger_Motor->total_angle, t_posSet); // 计算位置环PID + result = PID_calc(&speed_pid, trigger_Motor->speed_rpm, delta); // 计算速度环PID - - CAN_cmd_200(result,0,0,0,&hcan1); + CAN_cmd_200(result, 0, 0, 0, &hcan1); } - -void Shoot :: distanceGet(const NUC_t &nuc_v) +void Shoot ::distanceGet(const NUC_t &nuc_v) { - distance= LowPassFilter2p_Apply(&distance_filter, nuc_v.vision.x); // 对视觉距离进行滤波处理 - //distance=nuc_v.vision.x; // 获取自瞄数据 - + distance = LowPassFilter2p_Apply(&distance_filter, nuc_v.vision.x); // 对视觉距离进行滤波处理 + // distance=nuc_v.vision.x; // 获取自瞄数据 } -int Shoot::GO_SendData(float pos,float limit) +int Shoot::GO_SendData(float pos, float limit) { - + //// static int is_calibration=0; - //// static fp32 error=0; //误差量 + //// static fp32 error=0; //误差量 // 读取参数 // float tff = go1.T; // 前馈力矩 @@ -102,20 +96,25 @@ int Shoot::GO_SendData(float pos,float limit) // float kd = go1.K_W; // 速度阻尼 // float q_desired = go1.Pos; // 期望位置（rad） float q_current = go1_data->Pos; // 当前角度位置（rad） - // float dq_desired = go1.W; // 期望角速度（rad/s） - // float dq_current = go1_data->W; // 当前角速度（rad/s） + // float dq_desired = go1.W; // 期望角速度（rad/s） + // float dq_current = go1_data->W; // 当前角速度（rad/s） // 计算输出力矩 tau //// float tau = tff + kp * (q_desired - q_current) + kd * (dq_desired - dq_current); - /*限制最大输入来限制最大输出*/ - if (pos - q_current > limit) { - go1.Pos = q_current + limit; // 限制位置 - }else if (pos - q_current < -limit) { - go1.Pos = q_current - limit; // 限制位置 - }else { - go1.Pos = pos; // 允许位置 - } + /*限制最大输入来限制最大输出*/ + if (pos - q_current > limit) + { + go1.Pos = q_current + limit; // 限制位置 + } + else if (pos - q_current < -limit) + { + go1.Pos = q_current - limit; // 限制位置 + } + else + { + go1.Pos = pos; // 允许位置 + } // 发送数据 GO_M8010_send_data(&go1); @@ -123,45 +122,45 @@ int Shoot::GO_SendData(float pos,float limit) return 0; } -// F键 sw[0]👆 1800 👇200 -// E键 sw[1] 👆 200 shoot 中 1000 stop sw[2]200 sw[2]👇1800 +// F键 sw[0]👆 1800 👇200 +// E键 sw[1] 👆 200 shoot 中 1000 stop sw[2]200 sw[2]👇1800 // G键 sw[6]👆 200 中 1000 👇1800 // B键 sw[3]👆 200 开 中 1000 👇1800 关 // sw[5] 👆 200 👇1800 -//左旋 sw[7] 200 --1800 +// 左旋 sw[7] 200 --1800 void Shoot::rc_mode() { - if(rc_ctrl.sw[1]==200) + if (rc_ctrl.sw[1] == 200) { - rc_key=UP1; + rc_key = UP1; } - if(rc_ctrl.sw[1]==1800 && rc_ctrl.sw[2]==200) + if (rc_ctrl.sw[1] == 1800 && rc_ctrl.sw[2] == 200) { - rc_key=MIDDLE1; + rc_key = MIDDLE1; } - if(rc_ctrl.sw[2]==1800 && rc_ctrl.sw[1]==1800) + if (rc_ctrl.sw[2] == 1800 && rc_ctrl.sw[1] == 1800) { - rc_key=DOWN1; + rc_key = DOWN1; } - if(rc_ctrl.sw[0]==1800) + if (rc_ctrl.sw[0] == 1800) { - mode_key=TEST; + mode_key = TEST; } - if(rc_ctrl.sw[0]==200) + if (rc_ctrl.sw[0] == 200) { - mode_key=VSION; + mode_key = VSION; } // if(rc_ctrl.sw[5]==200) // { - // mode_key=OUT; + // mode_key=OUT; // } - if(rc_ctrl.sw[5]==1800) + if (rc_ctrl.sw[5] == 1800) { - ready_key=OFFENSIVE; + ready_key = OFFENSIVE; } - else if(rc_ctrl.sw[5]==200) + else if (rc_ctrl.sw[5] == 200) { - ready_key=DEFENSE; + ready_key = DEFENSE; } // //旋钮增量 @@ -176,306 +175,122 @@ void Shoot::rc_mode() // } // 旋钮物理范围 -const int knob_min = 200; -const int knob_max = 1800; + const int knob_min = 200; + const int knob_max = 1800; -// 目标映射范围 -const float map_min = 130.0f; -const float map_max = -60.0f; + // 目标映射范围 + const float map_min = 130.0f; + const float map_max = -60.0f; -int current_knob_value = rc_ctrl.sw[7]; - -// 限制范围 -if (current_knob_value < knob_min) current_knob_value = knob_min; -if (current_knob_value > knob_max) current_knob_value = knob_max; - -// 线性映射 -knob_increment = map_min + (map_max - map_min) * (current_knob_value - knob_min) / (knob_max - knob_min); + int current_knob_value = rc_ctrl.sw[7]; + // 限制范围 + if (current_knob_value < knob_min) + current_knob_value = knob_min; + if (current_knob_value > knob_max) + current_knob_value = knob_max; + // 线性映射 + knob_increment = map_min + (map_max - map_min) * (current_knob_value - knob_min) / (knob_max - knob_min); } +#if ONE_CONTROL == 0 - -#if ONE_CONTROL==0 - -void Shoot::shoot_control() { - - //方便调试 - feedback.fd_gopos = go1_data->Pos; - feedback.fd_tpos = trigger_Motor->total_angle; - switch (mode_key) - { - case VSION: - fire_pos = distance; // 视觉拟合的力 - switch (rc_key) { - case DOWN1: - - control_pos = INIT_POS; // 默认中间挡位位置 - go1.Pos = fire_pos; // 设置蓄力电机位置 - //t_posSet = Tigger_ZERO; // 扳机松开 - - if (currentState == SHOOT_READY) { - // 如果当前状态是准备发射，松开钩子发射 - t_posSet = Tigger_ZERO; // 扳机扣动 - BSP_Buzzer_Stop(); - if (t_posSet >= Tigger_ZERO-20) { // 假设120度为发射完成角度 - currentState = SHOOT_IDLE; // 切换到空闲状态 - is_hooked = false; // 重置钩子状态 - } - } else { - currentState = SHOOT_IDLE; // 默认回到空闲状态 - } - break; - - case MIDDLE1: - shoot_Current(); - break; - - case UP1: - RemoveError(); - break; - - default: - break; - } -break; - case TEST: - //实时可调蓄力位置 - fire_pos = INIT_POS + knob_increment; // 根据旋钮值调整发射位置 - - switch (rc_key) { - case DOWN1: - - control_pos = INIT_POS; // 默认中间挡位位置 - go1.Pos = fire_pos; // 设置蓄力电机位置 - //t_posSet = Tigger_ZERO; // 扳机松开 - - if (currentState == SHOOT_READY) { - // 如果当前状态是准备发射，松开钩子发射 - t_posSet = Tigger_ZERO; // 扳机扣动 - BSP_Buzzer_Stop(); - if (t_posSet >= Tigger_ZERO-20) { // 假设120度为发射完成角度 - currentState = SHOOT_IDLE; // 切换到空闲状态 - is_hooked = false; // 重置钩子状态 - } - } else { - currentState = SHOOT_IDLE; // 默认回到空闲状态 - } - break; - - case MIDDLE1: - shoot_Current(); - break; - - case UP1: - RemoveError(); - break; - - default: - break; - } - -break; - - default: - break; - } - - - - abs_limit_min_max_fp(&go1.Pos ,-210.0f,2.0f); - // 发送数据到蓄力电机 - GO_SendData(go1.Pos,limit_speed); - - // 控制扳机电机 - trigger_control(); -} - -void Shoot :: shoot_Current() +void Shoot::shoot_control() { - switch (currentState) { - case SHOOT_IDLE: - // 初始状态：钩子移动到顶部，钩住拉簧 - control_pos = TOP_POS; // 顶部位置 - limit_speed=TO_TOP;//快速上去 - go1.Pos = control_pos; - t_posSet = Tigger_ZERO; // 扳机松开 - if (feedback.fd_gopos <-209) { - currentState = SHOOT_TOP; // 切换到准备发射状态 - } - break; + // 方便调试 + feedback.fd_gopos = go1_data->Pos; + feedback.fd_tpos = trigger_Motor->total_angle; + switch (mode_key) + { + case VSION: + fire_pos = distance; // 视觉拟合的力 + switch (rc_key) + { + case DOWN1: - case SHOOT_TOP: - t_posSet = Tigger_DO; // 扳机扣动钩住 - if (trigger_Motor->total_angletotal_angle>Tigger_DO-1.0f) - { - //判定为钩住 - is_hooked = true; // 标记钩子已钩住 - currentState = SHOOT_READY; // 切换到准备发射状态 - } - break; + control_pos = INIT_POS; // 默认中间挡位位置 + go1.Pos = fire_pos; // 设置蓄力电机位置 + // t_posSet = Tigger_ZERO; // 扳机松开 - case SHOOT_READY: - if (is_hooked) + if (currentState == SHOOT_READY) { - go1.Pos = fire_pos; // 下拉到中间挡位位置 - limit_speed=TO_BOTTOM;//慢速下来 - if (feedback.fd_gopos >= fire_pos - GO_ERROR && feedback.fd_gopos <= fire_pos + GO_ERROR) { - BSP_Buzzer_Start(); - BSP_Buzzer_Set(1,5000); - //currentState = SHOOT_WAIT; // 等待发射信号 - //在拨杆切换时触发了 + // 如果当前状态是准备发射，松开钩子发射 + t_posSet = Tigger_ZERO; // 扳机扣动 + BSP_Buzzer_Stop(); + if (t_posSet >= Tigger_ZERO - 20) + { // 假设120度为发射完成角度 + currentState = SHOOT_IDLE; // 切换到空闲状态 + is_hooked = false; // 重置钩子状态 } } - break; + else + { + currentState = SHOOT_IDLE; // 默认回到空闲状态 + } + break; + case MIDDLE1: + shoot_Current(); + break; + + case UP1: + RemoveError(); + break; default: - currentState = SHOOT_IDLE; // 默认回到空闲状态 break; } -} + break; + case TEST: + // 实时可调蓄力位置 + fire_pos = INIT_POS + knob_increment; // 根据旋钮值调整发射位置 -void Shoot::RemoveError() { - currentState = SHOOT_IDLE; - control_pos = TOP_POS+1.0f; - if (feedback.fd_gopos >= control_pos - 0.5f&& feedback.fd_gopos<= control_pos + 0.5f) - { - t_posSet = Tigger_ZERO; - is_hooked = false; - } - if(trigger_Motor->total_angle>=Tigger_ZERO-10) - { - //认为完全松开 - control_pos=INIT_POS; - BSP_Buzzer_Stop(); - - } - limit_speed=3.0f;//慢慢送上去 - go1.Pos = control_pos; -} - -#endif - -#if ONE_CONTROL - -int asd=0; - - -void Shoot::shoot_control() { - -//方便调试 反馈信息 -feedback.fd_gopos = go1_data->Pos; -feedback.fd_tpos = trigger_Motor->total_angle; - - -shoot_thread = osThreadFlagsGet(); // 获取任务通知标志位 - -if(ready_key==OFFENSIVE){ -switch (mode_key) - { - case VSION: - fire_pos = distance; // 视觉拟合的力 - switch (rc_key) { - case MIDDLE1: - ball_receive(); // 执行接收球的操作 - - if(shoot_thread & EXTEND_OK) // 如果收到伸缩完成的通知 + switch (rc_key) { - go1.Pos = fire_pos; // 设置蓄力电机位置 - if(feedback.fd_gopos >= fire_pos - GO_ERROR && feedback.fd_gopos <= fire_pos + GO_ERROR) + case DOWN1: + + control_pos = INIT_POS; // 默认中间挡位位置 + go1.Pos = fire_pos; // 设置蓄力电机位置 + // t_posSet = Tigger_ZERO; // 扳机松开 + + if (currentState == SHOOT_READY) { - BSP_Buzzer_Start(); - BSP_Buzzer_Set(1,5000); + // 如果当前状态是准备发射，松开钩子发射 + t_posSet = Tigger_ZERO; // 扳机扣动 + BSP_Buzzer_Stop(); + if (t_posSet >= Tigger_ZERO - 20) + { // 假设120度为发射完成角度 + currentState = SHOOT_IDLE; // 切换到空闲状态 + is_hooked = false; // 重置钩子状态 + } } - } - break; - - case DOWN1: - if(shoot_thread & EXTEND_OK) - { - BSP_Buzzer_Stop(); - t_posSet = Tigger_ZERO; // 扳机扣动 射出 - currentState = SHOOT_IDLE; // 默认回到空闲状态 - osThreadFlagsClear(EXTEND_OK); - } - - break; - - case UP1: - RemoveError(); - break; - - default: - break; - } -break; - -//无自瞄拟合测试档 - case TEST: - switch (rc_key) { - case MIDDLE1: - - if(shoot_thread & EXTEND_OK) // 如果收到伸缩完成的通知 - { - asd++; - fire_pos = INIT_POS + knob_increment; // 根据旋钮值调整发射位置 - go1.Pos = fire_pos; // 设置蓄力电机位置 - limit_speed=TO_BOTTOM; - if(feedback.fd_gopos >= fire_pos - GO_ERROR && feedback.fd_gopos <= fire_pos + GO_ERROR) + else { - BSP_Buzzer_Start(); - BSP_Buzzer_Set(1,5000); + currentState = SHOOT_IDLE; // 默认回到空闲状态 } - } - else - { - ball_receive(); // 执行接收球的操作 - } - - break; + break; - case DOWN1: - if(shoot_thread & EXTEND_OK) - { - t_posSet = Tigger_ZERO; // 扳机扣动 射出 - if(feedback.fd_tpos>=Tigger_ZERO-20) - { - BSP_Buzzer_Stop(); - currentState = SHOOT_IDLE; // 默认回到空闲状态 - osThreadFlagsClear(EXTEND_OK); + case MIDDLE1: + shoot_Current(); + break; + + case UP1: + RemoveError(); + break; + + default: + break; } - - } break; - case UP1: - RemoveError(); - break; - default: break; } - -break; - - default: - break; - } -} -else -{ - go1.Pos = TOP_POS-3.0f; //-208 - limit_speed=TO_TOP;//快速上去 - t_posSet = Tigger_ZERO; // 扳机松开 - -} - - abs_limit_min_max_fp(&go1.Pos ,-210.0f,2.0f); + abs_limit_min_max_fp(&go1.Pos, -210.0f, 2.0f); // 发送数据到蓄力电机 GO_SendData(go1.Pos, limit_speed); @@ -483,67 +298,270 @@ else trigger_control(); } -//配合运球到发射 +void Shoot ::shoot_Current() +{ + + switch (currentState) + { + case SHOOT_IDLE: + // 初始状态：钩子移动到顶部，钩住拉簧 + control_pos = TOP_POS; // 顶部位置 + limit_speed = TO_TOP; // 快速上去 + go1.Pos = control_pos; + t_posSet = Tigger_ZERO; // 扳机松开 + if (feedback.fd_gopos < -209) + { + currentState = SHOOT_TOP; // 切换到准备发射状态 + } + break; + + case SHOOT_TOP: + t_posSet = Tigger_DO; // 扳机扣动钩住 + if (trigger_Motor->total_angle < Tigger_DO + 1.0f && trigger_Motor->total_angle > Tigger_DO - 1.0f) + { + // 判定为钩住 + is_hooked = true; // 标记钩子已钩住 + currentState = SHOOT_READY; // 切换到准备发射状态 + } + break; + + case SHOOT_READY: + if (is_hooked) + { + go1.Pos = fire_pos; // 下拉到中间挡位位置 + limit_speed = TO_BOTTOM; // 慢速下来 + if (feedback.fd_gopos >= fire_pos - GO_ERROR && feedback.fd_gopos <= fire_pos + GO_ERROR) + { + BSP_Buzzer_Start(); + BSP_Buzzer_Set(1, 5000); + // currentState = SHOOT_WAIT; // 等待发射信号 + // 在拨杆切换时触发了 + } + } + break; + + default: + currentState = SHOOT_IDLE; // 默认回到空闲状态 + break; + } +} + +void Shoot::RemoveError() +{ + currentState = SHOOT_IDLE; + control_pos = TOP_POS + 1.0f; + if (feedback.fd_gopos >= control_pos - 0.5f && feedback.fd_gopos <= control_pos + 0.5f) + { + t_posSet = Tigger_ZERO; + is_hooked = false; + } + if (trigger_Motor->total_angle >= Tigger_ZERO - 10) + { + // 认为完全松开 + control_pos = INIT_POS; + BSP_Buzzer_Stop(); + } + limit_speed = 3.0f; // 慢慢送上去 + go1.Pos = control_pos; +} + +#endif + +#if ONE_CONTROL + +int asd = 0; + +void Shoot::shoot_control() +{ + + // 方便调试 反馈信息 + feedback.fd_gopos = go1_data->Pos; + feedback.fd_tpos = trigger_Motor->total_angle; + + shoot_thread = osThreadFlagsGet(); // 获取任务通知标志位 + + if (ready_key == OFFENSIVE) + { + switch (mode_key) + { + case VSION: + fire_pos = distance; // 视觉拟合的力 + switch (rc_key) + { + case MIDDLE1: + if ((shoot_thread & READY_TELL) && !(shoot_thread & EXTEND_OK)) + { + // 只收到READY_TELL且未收到EXTEND_OK时，顶部蓄力流程 + ball_receive(); + } + else if (shoot_thread & EXTEND_OK) + { + go1.Pos = fire_pos; + limit_speed = TO_BOTTOM; + if (feedback.fd_gopos >= fire_pos - GO_ERROR && feedback.fd_gopos <= fire_pos + GO_ERROR) + { + BSP_Buzzer_Start(); + BSP_Buzzer_Set(1, 5000); + } + } + + break; + + case DOWN1: + if (shoot_thread & EXTEND_OK) + { + t_posSet = Tigger_ZERO; + if (feedback.fd_tpos >= Tigger_ZERO - 20) + { + BSP_Buzzer_Stop(); + currentState = SHOOT_IDLE; + osThreadFlagsClear(EXTEND_OK); + } + } + break; + + case UP1: + RemoveError(); + break; + + default: + break; + } + + break; + + // 无自瞄拟合测试档 + case TEST: + switch (rc_key) + { + case MIDDLE1: + if ((shoot_thread & READY_TELL) && !(shoot_thread & EXTEND_OK)) + { + // 只收到READY_TELL且未收到EXTEND_OK时，顶部蓄力流程 + ball_receive(); // ball_receive内部写go1.Pos + } + else if (shoot_thread & EXTEND_OK) + { + // 只收到EXTEND_OK时，允许调节蓄力位置 + fire_pos = INIT_POS + knob_increment; + go1.Pos = fire_pos; + limit_speed = TO_BOTTOM; + if (feedback.fd_gopos >= fire_pos - GO_ERROR && feedback.fd_gopos <= fire_pos + GO_ERROR) + { + BSP_Buzzer_Start(); + BSP_Buzzer_Set(1, 5000); + } + } + // 没收到READY_TELL不做任何蓄力 + break; + + case DOWN1: + if (shoot_thread & EXTEND_OK) + { + t_posSet = Tigger_ZERO; + if (feedback.fd_tpos >= Tigger_ZERO - 20) + { + BSP_Buzzer_Stop(); + currentState = SHOOT_IDLE; + osThreadFlagsClear(EXTEND_OK); + } + } + break; + + case UP1: + RemoveError(); + break; + + default: + break; + } + + break; + + default: + break; + } + } + else if (ready_key == DEFENSE) + { + control_pos = TOP_POS - 2.0f; //-209 + go1.Pos = control_pos; + limit_speed = TO_TOP; // 快速上去 + t_posSet = Tigger_ZERO; // 扳机松开 + } + else + { + t_posSet = Tigger_DO; // 防止debug时重复 + } + + abs_limit_min_max_fp(&go1.Pos, -210.0f, 2.0f); + // 发送数据到蓄力电机 + GO_SendData(go1.Pos, limit_speed); + + // 控制扳机电机 + trigger_control(); +} + +// 配合运球到发射 void Shoot ::ball_receive() { - switch (currentState) + switch (currentState) { - case SHOOT_IDLE: - // 初始状态：钩子移动到顶部，钩住拉簧 - if(shoot_thread & READY_TELL) // 如果收到等待通知 - { - control_pos = TOP_POS; - limit_speed=TO_TOP;//快速上去 + case SHOOT_IDLE: + // 初始状态：钩子移动到顶部，钩住拉簧 + if (shoot_thread & READY_TELL) // 如果收到等待通知 + { + control_pos = TOP_POS; + limit_speed = TO_TOP; // 快速上去 t_posSet = Tigger_ZERO; - if(feedback.fd_gopos <-209) + if (feedback.fd_gopos < -209) { currentState = GO_TOP; // 切换到准备发射状态 } } - break; + break; - case GO_TOP: - t_posSet = Tigger_DO; - if (trigger_Motor->total_angletotal_angle>Tigger_DO-1.0f) - { - - currentState = BAKC; // 切换到准备发射状态 - } - break; - case BAKC: - control_pos = BOTTOM_POS; - limit_speed=TO_BOTTOM;//慢速下来 - if(feedback.fd_gopos >= BOTTOM_POS - 1.0f && feedback.fd_gopos <= BOTTOM_POS + 1.0f) - { - - osThreadFlagsClear(READY_TELL); // 清除任务通知标志位 - } - - break; + case GO_TOP: + t_posSet = Tigger_DO; + if (trigger_Motor->total_angle < Tigger_DO + 1.0f && trigger_Motor->total_angle > Tigger_DO - 1.0f) + { + + currentState = BAKC; // 切换到准备发射状态 + } + break; + case BAKC: + control_pos = BOTTOM_POS; + limit_speed = TO_BOTTOM; // 慢速下来 + if (feedback.fd_gopos >= BOTTOM_POS - 1.0f && feedback.fd_gopos <= BOTTOM_POS + 1.0f) + { + + osThreadFlagsClear(READY_TELL); // 清除任务通知标志位 + } + + break; } - - //调整go电机位置 - go1.Pos = control_pos; + + // 调整go电机位置 + go1.Pos = control_pos; } -void Shoot::RemoveError() { +void Shoot::RemoveError() +{ currentState = SHOOT_IDLE; control_pos = TOP_POS; - if (feedback.fd_gopos >= control_pos - 0.5f&& feedback.fd_gopos<= control_pos + 0.5f) + if (feedback.fd_gopos < -209) { - t_posSet = Tigger_ZERO; - is_hooked = false; + t_posSet = Tigger_ZERO; + is_hooked = false; } - if(trigger_Motor->total_angle>=Tigger_ZERO-5) + if (trigger_Motor->total_angle >= Tigger_ZERO - 5) { - //认为完全松开 - control_pos=INIT_POS; - BSP_Buzzer_Stop(); - + // 认为完全松开 + control_pos = INIT_POS; + BSP_Buzzer_Stop(); } - limit_speed=3.0f;//慢慢送上去 - go1.Pos = control_pos; + limit_speed = 3.0f; // 慢慢送上去 + go1.Pos = control_pos; } - #endif diff --git a/User/task/shootTask.cpp b/User/task/shootTask.cpp index aa16c41..414729e 100644 --- a/User/task/shootTask.cpp +++ b/User/task/shootTask.cpp @@ -17,7 +17,7 @@ void FunctionShoot(void *argument) (void)argument; /* 未使用argument，消除警告 */ const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CTRL_SHOOT; - + osDelay(3000);//等待M2006电机启动 uint32_t tick = osKernelGetTickCount(); while(1)