RoboDuoduo/R1_up
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

初始化在最顶上

Browse Source
This commit is contained in:
ws 2025-06-26 22:06:41 +08:00
parent 048319df42
commit 4fb6b424a1
6 changed files with 408 additions and 389 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
MDK-ARM/.vscode/uv4.log vendored
View File

@ -1,8 +1,9 @@
*** Using Compiler 'V5.06 update 7 (build 960)', folder: 'D:\keil\ARM\ARMCC\Bin' *** Using Compiler 'V5.06 update 7 (build 960)', folder: 'D:\keil\ARM\ARMCC\Bin'
Build target 'R1' Build target 'R1'
compiling ball.cpp...
compiling shoot.cpp... compiling shoot.cpp...
linking... 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... FromELF: creating hex file...
"R1\R1.axf" - 0 Error(s), 0 Warning(s). "R1\R1.axf" - 0 Error(s), 0 Warning(s).
Build Time Elapsed: 00:00:05 Build Time Elapsed: 00:00:06

2
MDK-ARM/.vscode/uv4.log.lock vendored
View File

@ -1 +1 @@
2025/6/26 19:15:12 2025/6/26 22:06:06

3
User/bsp/TopDefine.h
View File

@ -18,9 +18,6 @@
#define ONE_CONTROL 1 #define ONE_CONTROL 1
//是否使用大疆DT7遥控器 //是否使用大疆DT7遥控器
#ifndef DT7 #ifndef DT7
#define DT7 0 #define DT7 0

9
User/module/ball.cpp
View File

@ -258,8 +258,7 @@ void Ball::ball_control()
} }
// 发送小米电机控制数据
Send_control(); Send_control();
} }
@ -322,7 +321,11 @@ void Ball::Idle_control()
xiaomi.position = WAIT_POS; xiaomi.position = WAIT_POS;
if(feedback->position_deg >= WAIT_POS-3) 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 else

776
User/module/shoot.cpp
View File

@ -13,17 +13,17 @@
extern RC_ctrl_t rc_ctrl; extern RC_ctrl_t rc_ctrl;
NUC_t nuc_v; NUC_t nuc_v;
//sw[7]👆 1694 中 1000 👇306 // sw[7]👆 1694 中 1000 👇306
//sw[2]👆 1694 👇306 // sw[2]👆 1694 👇306
//E键 sw[1]👆 200 shoot 中 1000 stop 👇1800 error // E键 sw[1]👆 200 shoot 中 1000 stop 👇1800 error
//F键 sw[0]👆 1800 开 👇200 关 // F键 sw[0]👆 1800 开 👇200 关
//B键 sw[3]👆 200 开 👇1800 关 // B键 sw[3]👆 200 开 👇1800 关
//左旋钮 sw[4] 👈 200 👉1800 // 左旋钮 sw[4] 👈 200 👉1800
//尽量别动这组pid // 尽量别动这组pid
const fp32 Shoot:: M2006_speed_PID[3] = {5, 0, 0.01}; const fp32 Shoot::M2006_speed_PID[3] = {5, 0, 0.01};
const fp32 Shoot:: M2006_angle_PID[3] = { 15, 0.1, 0}; const fp32 Shoot::M2006_angle_PID[3] = {15, 0.1, 0};
#define TO_TOP 10.0f #define TO_TOP 10.0f
#define TO_BOTTOM 5.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 INIT_POS -130
#define TOP_POS -211 #define TOP_POS -211
#define BOTTOM_POS 0 #define BOTTOM_POS 0
#define GO_ERROR 1.0f #define GO_ERROR 1.0f
#define Tigger_DO 0 #define Tigger_DO 0
#define Tigger_ZERO 100 #define Tigger_ZERO 100
#define Tigger_ERROR 3 #define Tigger_ERROR 3
float knob_increment; float knob_increment;
Shoot::Shoot() Shoot::Shoot()
{ {
//扳机初始化 // 扳机初始化
trigger_Motor= get_motor_point(0);//id 201 trigger_Motor = get_motor_point(0); // id 201
trigger_Motor->type=M2006; trigger_Motor->type = M2006;
PID_init(&speed_pid,PID_POSITION,M2006_speed_PID,5000, 2000);//pid初始化 PID_init(&speed_pid, PID_POSITION, M2006_speed_PID, 5000, 2000); // pid初始化
PID_init(&angle_pid,PID_POSITION,M2006_angle_PID,5000, 2000);//pid初始化 PID_init(&angle_pid, PID_POSITION, M2006_angle_PID, 5000, 2000); // pid初始化
t_speedSet = 0; t_speedSet = 0;
result = 0; result = 0;
go1.id =1, go1.id = 1,
go1.mode = 1, go1.mode = 1,
go1.K_P = 1.0f, go1.K_P = 1.0f,
go1.K_W = 0.05, go1.K_W = 0.05,
go1.Pos = 0,//上电先到一个舒服的位置 go1.Pos = 0, // 上电先到一个舒服的位置
go1.W = 0, go1.W = 0,
go1.T = 0, go1.T = 0,
limit_speed=TO_TOP;//快速上去 limit_speed = TO_TOP; // 快速上去
fire_pos = INIT_POS; // 发射位置 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; currentState = SHOOT_IDLE;
LowPassFilter2p_Init(&distance_filter,500.0f,80.0f); //给distance 做滤波
LowPassFilter2p_Init(&distance_filter, 500.0f, 80.0f); // 给distance 做滤波
} }
void Shoot::trigger_control() void Shoot::trigger_control()
{ {
int delta = 0; int delta = 0;
delta = PID_calc(&angle_pid,trigger_Motor->total_angle,t_posSet); // 计算位置环PID delta = PID_calc(&angle_pid, trigger_Motor->total_angle, t_posSet); // 计算位置环PID
result = PID_calc(&speed_pid,trigger_Motor->speed_rpm,delta); // 计算速度环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 = LowPassFilter2p_Apply(&distance_filter, nuc_v.vision.x); // 对视觉距离进行滤波处理
//distance=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 int is_calibration=0;
//// static fp32 error=0; //误差量 //// static fp32 error=0; //误差量
// 读取参数 // 读取参数
// float tff = go1.T; // 前馈力矩 // float tff = go1.T; // 前馈力矩
@ -102,20 +96,25 @@ int Shoot::GO_SendData(float pos,float limit)
// float kd = go1.K_W; // 速度阻尼 // float kd = go1.K_W; // 速度阻尼
// float q_desired = go1.Pos; // 期望位置（rad） // float q_desired = go1.Pos; // 期望位置（rad）
float q_current = go1_data->Pos; // 当前角度位置（rad） float q_current = go1_data->Pos; // 当前角度位置（rad）
// float dq_desired = go1.W; // 期望角速度（rad/s） // float dq_desired = go1.W; // 期望角速度（rad/s）
// float dq_current = go1_data->W; // 当前角速度（rad/s） // float dq_current = go1_data->W; // 当前角速度（rad/s）
// 计算输出力矩 tau // 计算输出力矩 tau
//// float tau = tff + kp * (q_desired - q_current) + kd * (dq_desired - dq_current); //// float tau = tff + kp * (q_desired - q_current) + kd * (dq_desired - dq_current);
/*限制最大输入来限制最大输出*/ /*限制最大输入来限制最大输出*/
if (pos - q_current > limit) { if (pos - q_current > limit)
go1.Pos = q_current + limit; // 限制位置 {
}else if (pos - q_current < -limit) { go1.Pos = q_current + limit; // 限制位置
go1.Pos = q_current - limit; // 限制位置 }
}else { else if (pos - q_current < -limit)
go1.Pos = pos; // 允许位置 {
} go1.Pos = q_current - limit; // 限制位置
}
else
{
go1.Pos = pos; // 允许位置
}
// 发送数据 // 发送数据
GO_M8010_send_data(&go1); GO_M8010_send_data(&go1);
@ -123,45 +122,45 @@ int Shoot::GO_SendData(float pos,float limit)
return 0; return 0;
} }
// F键 sw[0]👆 1800 👇200 // F键 sw[0]👆 1800 👇200
// E键 sw[1] 👆 200 shoot 中 1000 stop sw[2]200 sw[2]👇1800 // E键 sw[1] 👆 200 shoot 中 1000 stop sw[2]200 sw[2]👇1800
// G键 sw[6]👆 200 中 1000 👇1800 // G键 sw[6]👆 200 中 1000 👇1800
// B键 sw[3]👆 200 开 中 1000 👇1800 关 // B键 sw[3]👆 200 开 中 1000 👇1800 关
// sw[5] 👆 200 👇1800 // sw[5] 👆 200 👇1800
//左旋 sw[7] 200 --1800 // 左旋 sw[7] 200 --1800
void Shoot::rc_mode() 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) // 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_min = 200;
const int knob_max = 1800; const int knob_max = 1800;
// 目标映射范围 // 目标映射范围
const float map_min = 130.0f; const float map_min = 130.0f;
const float map_max = -60.0f; const float map_max = -60.0f;
int current_knob_value = rc_ctrl.sw[7]; 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 (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
void Shoot::shoot_control()
#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()
{ {
switch (currentState) { // 方便调试
case SHOOT_IDLE: feedback.fd_gopos = go1_data->Pos;
// 初始状态：钩子移动到顶部，钩住拉簧 feedback.fd_tpos = trigger_Motor->total_angle;
control_pos = TOP_POS; // 顶部位置 switch (mode_key)
limit_speed=TO_TOP;//快速上去 {
go1.Pos = control_pos; case VSION:
t_posSet = Tigger_ZERO; // 扳机松开 fire_pos = distance; // 视觉拟合的力
if (feedback.fd_gopos <-209) { switch (rc_key)
currentState = SHOOT_TOP; // 切换到准备发射状态 {
} case DOWN1:
break;
case SHOOT_TOP: control_pos = INIT_POS; // 默认中间挡位位置
t_posSet = Tigger_DO; // 扳机扣动钩住 go1.Pos = fire_pos; // 设置蓄力电机位置
if (trigger_Motor->total_angle<Tigger_DO+1.0f && trigger_Motor->total_angle>Tigger_DO-1.0f) // t_posSet = Tigger_ZERO; // 扳机松开
{
//判定为钩住
is_hooked = true; // 标记钩子已钩住
currentState = SHOOT_READY; // 切换到准备发射状态
}
break;
case SHOOT_READY: if (currentState == SHOOT_READY)
if (is_hooked)
{ {
go1.Pos = fire_pos; // 下拉到中间挡位位置 // 如果当前状态是准备发射，松开钩子发射
limit_speed=TO_BOTTOM;//慢速下来 t_posSet = Tigger_ZERO; // 扳机扣动
if (feedback.fd_gopos >= fire_pos - GO_ERROR && feedback.fd_gopos <= fire_pos + GO_ERROR) { BSP_Buzzer_Stop();
BSP_Buzzer_Start(); if (t_posSet >= Tigger_ZERO - 20)
BSP_Buzzer_Set(1,5000); { // 假设120度为发射完成角度
//currentState = SHOOT_WAIT; // 等待发射信号 currentState = SHOOT_IDLE; // 切换到空闲状态
//在拨杆切换时触发了 is_hooked = false; // 重置钩子状态
} }
} }
break; else
{
currentState = SHOOT_IDLE; // 默认回到空闲状态
}
break;
case MIDDLE1:
shoot_Current();
break;
case UP1:
RemoveError();
break;
default: default:
currentState = SHOOT_IDLE; // 默认回到空闲状态
break; break;
} }
} break;
case TEST:
// 实时可调蓄力位置
fire_pos = INIT_POS + knob_increment; // 根据旋钮值调整发射位置
void Shoot::RemoveError() { switch (rc_key)
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) // 如果收到伸缩完成的通知
{ {
go1.Pos = fire_pos; // 设置蓄力电机位置 case DOWN1:
if(feedback.fd_gopos >= fire_pos - GO_ERROR && feedback.fd_gopos <= fire_pos + GO_ERROR)
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; // 重置钩子状态
}
} }
} else
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)
{ {
BSP_Buzzer_Start(); currentState = SHOOT_IDLE; // 默认回到空闲状态
BSP_Buzzer_Set(1,5000);
} }
} break;
else
{
ball_receive(); // 执行接收球的操作
}
break;
case DOWN1: case MIDDLE1:
if(shoot_thread & EXTEND_OK) shoot_Current();
{ break;
t_posSet = Tigger_ZERO; // 扳机扣动 射出
if(feedback.fd_tpos>=Tigger_ZERO-20) case UP1:
{ RemoveError();
BSP_Buzzer_Stop(); break;
currentState = SHOOT_IDLE; // 默认回到空闲状态
osThreadFlagsClear(EXTEND_OK); default:
break;
} }
}
break; break;
case UP1:
RemoveError();
break;
default: default:
break; break;
} }
break;
default:
break;
}
} abs_limit_min_max_fp(&go1.Pos, -210.0f, 2.0f);
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);
// 发送数据到蓄力电机 // 发送数据到蓄力电机
GO_SendData(go1.Pos, limit_speed); GO_SendData(go1.Pos, limit_speed);
@ -483,67 +298,270 @@ else
trigger_control(); 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() void Shoot ::ball_receive()
{ {
switch (currentState) switch (currentState)
{ {
case SHOOT_IDLE: case SHOOT_IDLE:
// 初始状态：钩子移动到顶部，钩住拉簧 // 初始状态：钩子移动到顶部，钩住拉簧
if(shoot_thread & READY_TELL) // 如果收到等待通知 if (shoot_thread & READY_TELL) // 如果收到等待通知
{ {
control_pos = TOP_POS; control_pos = TOP_POS;
limit_speed=TO_TOP;//快速上去 limit_speed = TO_TOP; // 快速上去
t_posSet = Tigger_ZERO; t_posSet = Tigger_ZERO;
if(feedback.fd_gopos <-209) if (feedback.fd_gopos < -209)
{ {
currentState = GO_TOP; // 切换到准备发射状态 currentState = GO_TOP; // 切换到准备发射状态
} }
} }
break; break;
case GO_TOP: case GO_TOP:
t_posSet = Tigger_DO; t_posSet = Tigger_DO;
if (trigger_Motor->total_angle<Tigger_DO+1.0f && trigger_Motor->total_angle>Tigger_DO-1.0f) if (trigger_Motor->total_angle < Tigger_DO + 1.0f && trigger_Motor->total_angle > Tigger_DO - 1.0f)
{ {
currentState = BAKC; // 切换到准备发射状态 currentState = BAKC; // 切换到准备发射状态
} }
break; break;
case BAKC: case BAKC:
control_pos = BOTTOM_POS; control_pos = BOTTOM_POS;
limit_speed=TO_BOTTOM;//慢速下来 limit_speed = TO_BOTTOM; // 慢速下来
if(feedback.fd_gopos >= BOTTOM_POS - 1.0f && feedback.fd_gopos <= BOTTOM_POS + 1.0f) if (feedback.fd_gopos >= BOTTOM_POS - 1.0f && feedback.fd_gopos <= BOTTOM_POS + 1.0f)
{ {
osThreadFlagsClear(READY_TELL); // 清除任务通知标志位 osThreadFlagsClear(READY_TELL); // 清除任务通知标志位
} }
break; break;
} }
//调整go电机位置 // 调整go电机位置
go1.Pos = control_pos; go1.Pos = control_pos;
} }
void Shoot::RemoveError() { void Shoot::RemoveError()
{
currentState = SHOOT_IDLE; currentState = SHOOT_IDLE;
control_pos = TOP_POS; 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; t_posSet = Tigger_ZERO;
is_hooked = false; is_hooked = false;
} }
if(trigger_Motor->total_angle>=Tigger_ZERO-5) if (trigger_Motor->total_angle >= Tigger_ZERO - 5)
{ {
//认为完全松开 // 认为完全松开
control_pos=INIT_POS; control_pos = INIT_POS;
BSP_Buzzer_Stop(); BSP_Buzzer_Stop();
} }
limit_speed=3.0f;//慢慢送上去 limit_speed = 3.0f; // 慢慢送上去
go1.Pos = control_pos; go1.Pos = control_pos;
} }
#endif #endif

2
User/task/shootTask.cpp
View File

@ -17,7 +17,7 @@ void FunctionShoot(void *argument)
(void)argument; /* 未使用argument，消除警告 */ (void)argument; /* 未使用argument，消除警告 */
const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CTRL_SHOOT; const uint32_t delay_tick = osKernelGetTickFreq() / TASK_FREQ_CTRL_SHOOT;
osDelay(3000);//等待M2006电机启动
uint32_t tick = osKernelGetTickCount(); uint32_t tick = osKernelGetTickCount();
while(1) while(1)