649 lines
18 KiB
C++
649 lines
18 KiB
C++
#include "TopDefine.h"
|
||
#include "shoot.hpp"
|
||
#include "userTask.h"
|
||
#include "main.h"
|
||
#include "remote_control.h"
|
||
#include "FreeRTOS.h"
|
||
#include <cmsis_os2.h>
|
||
#include "calc_lib.h"
|
||
#include "vofa.h"
|
||
#include "buzzer.h"
|
||
#include "bsp_buzzer.h"
|
||
#include <math.h>
|
||
|
||
extern RC_ctrl_t rc_ctrl;
|
||
NUC_t nuc_v;
|
||
float vofa[8];
|
||
|
||
double test_distance=4.0;
|
||
|
||
// 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
|
||
|
||
// 尽量别动这组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 6.0f
|
||
|
||
#define INIT_POS -135
|
||
#define TOP_POS -211
|
||
#define BOTTOM_POS 0
|
||
#define WAIT_POS -170
|
||
#define CHANEGE_POS -205
|
||
#define GO_ERROR 1.0f
|
||
#define Tigger_DO -10
|
||
#define Tigger_ZERO 115
|
||
#define Tigger_ERROR 3
|
||
|
||
float knob_increment;
|
||
double last_distance = 4.0f; // 4米做测试吧
|
||
double last_pass = 4.0f; // 4米做测试吧
|
||
|
||
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;
|
||
result = 0;
|
||
|
||
go1.id = 1,
|
||
go1.mode = 1,
|
||
go1.K_P = 1.0f,
|
||
go1.K_W = 0.05,
|
||
go1.Pos = 0, // 上电先到一个舒服的位置
|
||
go1.W = 0,
|
||
go1.T = 0,
|
||
|
||
limit_speed = TO_TOP; // 快速上去
|
||
fire_pos = INIT_POS; // 发射位置
|
||
|
||
BSP_UART_RegisterCallback(BSP_UART_RS485, BSP_UART_RX_CPLT_CB, USART1_RxCompleteCallback); // 注册串口回调函数，bsp层
|
||
|
||
go1_data = get_GO_measure_point(); // go1数据
|
||
|
||
currentState = SHOOT_IDLE;
|
||
|
||
LowPassFilter2p_Init(&distance_filter, 500.0f, 80.0f); // 给distance 做滤波
|
||
LowPassFilter2p_Init(&pass_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
|
||
|
||
CAN_cmd_200(result, 0, 0, 0, &hcan1);
|
||
}
|
||
|
||
float shoot_fitting(float x)
|
||
{
|
||
return 0.67076341f * x * x + 20.212423f * x + -154.53966f + 1.0f;
|
||
}
|
||
|
||
// float shoot_fitting(float x)
|
||
// {
|
||
// return 1.4255807f * x * x + 12.138447f * x + -136.35306f;
|
||
// }
|
||
|
||
|
||
float pass_fitting(float x)
|
||
{
|
||
return 0.36807548f * x * x + 25.002169f * x + -199.2727f;
|
||
}
|
||
|
||
void Shoot::distanceGet(const NUC_t &nuc_v)
|
||
{
|
||
// 判断数据是否在合理范围
|
||
if (nuc_v.vision.x >= 0.0f && nuc_v.vision.x <= 7.5f)
|
||
{
|
||
last_distance = LowPassFilter2p_Apply(&distance_filter, nuc_v.vision.x);
|
||
}
|
||
|
||
if(nuc_v.vision.y >= 0.0f && nuc_v.vision.y <= 7.5f)
|
||
{
|
||
last_pass = LowPassFilter2p_Apply(&pass_filter, nuc_v.vision.y);
|
||
}
|
||
// 否则不更新，保持上一次的值
|
||
distance = last_distance;
|
||
pass_distance =last_pass;
|
||
}
|
||
|
||
int Shoot::GO_SendData(float pos, float limit)
|
||
{
|
||
|
||
//// static int is_calibration=0;
|
||
//// static fp32 error=0; //误差量
|
||
|
||
// 读取参数
|
||
// float tff = go1.T; // 前馈力矩
|
||
// float kp = go1.K_P; // 位置刚度
|
||
// 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）
|
||
|
||
// 计算输出力矩 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; // 允许位置
|
||
}
|
||
|
||
// 发送数据
|
||
GO_M8010_send_data(&go1);
|
||
|
||
return 0;
|
||
}
|
||
|
||
// 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
|
||
|
||
void Shoot::rc_mode()
|
||
{
|
||
// 底部光电检测（假设0为到位，1为未到位，根据实际硬件调整）
|
||
int bottom_sensor = HAL_GPIO_ReadPin(BALL_GPIO_Port, BALL_Pin); // 0为到位
|
||
static bool is_ready = false;
|
||
|
||
// 只要检测到到位就解除保护
|
||
if (!is_ready)
|
||
{
|
||
if (bottom_sensor == 0)
|
||
{
|
||
is_ready = true;
|
||
BSP_Buzzer_Stop();
|
||
}
|
||
else
|
||
{
|
||
BSP_Buzzer_Start();
|
||
BSP_Buzzer_Set(1, 5000);
|
||
return;
|
||
}
|
||
}
|
||
|
||
// 以下为原有遥控按键逻辑
|
||
if (rc_ctrl.sw[1] == 200)
|
||
{
|
||
rc_key = UP1;
|
||
}
|
||
if (rc_ctrl.sw[1] == 1800 && rc_ctrl.sw[2] == 200)
|
||
{
|
||
rc_key = MIDDLE1;
|
||
}
|
||
if (rc_ctrl.sw[2] == 1800 && rc_ctrl.sw[1] == 1800)
|
||
{
|
||
rc_key = DOWN1;
|
||
}
|
||
if (rc_ctrl.sw[0] == 1800)
|
||
{
|
||
mode_key = PASS;
|
||
}
|
||
if (rc_ctrl.sw[0] == 200)
|
||
{
|
||
mode_key = VSION;
|
||
}
|
||
if (rc_ctrl.sw[5] == 1800)
|
||
{
|
||
ready_key = OFFENSIVE;
|
||
}
|
||
else if (rc_ctrl.sw[5] == 200)
|
||
{
|
||
ready_key = DEFENSE;
|
||
}
|
||
|
||
// 旋钮映射部分不变
|
||
const int knob_min = 200;
|
||
const int knob_max = 1800;
|
||
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);
|
||
}
|
||
|
||
#if ONE_CONTROL == 0
|
||
|
||
float and1=2.0f;
|
||
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; // 视觉拟合的力
|
||
// fire_pos =shoot_fitting(distance)+and1;
|
||
fire_pos =shoot_fitting(distance)+2.0f-2.3f;
|
||
//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;
|
||
case PASS:
|
||
// 实时可调蓄力位置
|
||
//fire_pos = INIT_POS + knob_increment; // 根据旋钮值调整发射位置
|
||
fire_pos =pass_fitting(pass_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;
|
||
|
||
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:
|
||
// 初始状态：钩子移动到顶部，钩住拉簧
|
||
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
|
||
|
||
//float and1=-1.5f;
|
||
float and1=0;
|
||
float and2=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 = shoot_fitting(distance)+and1; // 视觉拟合的力
|
||
//fire_pos = INIT_POS + knob_increment; // 根据旋钮值调整发射位置
|
||
|
||
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)
|
||
{
|
||
shoot_wait = 1;
|
||
//BSP_Buzzer_Start();
|
||
//BSP_Buzzer_Set(1, 500);
|
||
}
|
||
}
|
||
|
||
break;
|
||
|
||
case DOWN1:
|
||
if (shoot_thread & EXTEND_OK && shoot_wait == 1)
|
||
{
|
||
t_posSet = Tigger_ZERO;
|
||
if (feedback.fd_tpos >= Tigger_ZERO - 20)
|
||
{
|
||
|
||
//BSP_Buzzer_Stop();
|
||
currentState = SHOOT_IDLE;
|
||
osThreadFlagsClear(EXTEND_OK);
|
||
osThreadFlagsClear(READY_TELL); // 蓄力标志位
|
||
shoot_wait = 0;
|
||
}
|
||
}
|
||
break;
|
||
|
||
case UP1:
|
||
RemoveError();
|
||
break;
|
||
|
||
default:
|
||
break;
|
||
}
|
||
|
||
break;
|
||
|
||
// 传球档
|
||
case PASS:
|
||
switch (rc_key)
|
||
{
|
||
case MIDDLE1:
|
||
fire_pos = pass_fitting(pass_distance)+and2;
|
||
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)
|
||
{
|
||
shoot_wait = 1;
|
||
// BSP_Buzzer_Start();
|
||
// BSP_Buzzer_Set(1, 500);
|
||
}
|
||
}
|
||
// 没收到READY_TELL不做任何蓄力
|
||
break;
|
||
|
||
case DOWN1:
|
||
if (shoot_thread & EXTEND_OK)
|
||
{
|
||
if (shoot_wait == 1)
|
||
{
|
||
t_posSet = Tigger_ZERO;
|
||
if (feedback.fd_tpos >= Tigger_ZERO - 20)
|
||
{
|
||
|
||
//BSP_Buzzer_Stop();
|
||
currentState = SHOOT_IDLE;
|
||
osThreadFlagsClear(EXTEND_OK);
|
||
osThreadFlagsClear(READY_TELL); // 蓄力标志位
|
||
shoot_wait = 0;
|
||
}
|
||
}
|
||
}
|
||
break;
|
||
|
||
case UP1:
|
||
RemoveError();
|
||
break;
|
||
|
||
default:
|
||
break;
|
||
}
|
||
|
||
break;
|
||
|
||
default:
|
||
break;
|
||
}
|
||
}
|
||
else if (ready_key == DEFENSE)
|
||
{
|
||
|
||
control_pos = WAIT_POS; //-209
|
||
go1.Pos = control_pos;
|
||
limit_speed = TO_TOP; // 快速上去
|
||
if(feedback.fd_gopos < WAIT_POS)
|
||
{
|
||
t_posSet = Tigger_ZERO; // 扳机松开
|
||
//osThreadFlagsSet(task_struct.thread.ball, DEF_READY);
|
||
|
||
// 只在READY_TELL未置位时发送，防止重复
|
||
if ((osThreadFlagsGet() & DEF_READY) == 0)
|
||
{
|
||
osThreadFlagsSet(task_struct.thread.ball, DEF_READY);
|
||
}
|
||
|
||
}
|
||
|
||
osThreadFlagsClear(EXTEND_OK);
|
||
osThreadFlagsClear(READY_TELL); // 蓄力标志位
|
||
osThreadFlagsClear(HANDING_FINISH);
|
||
currentState = SHOOT_IDLE;
|
||
|
||
}
|
||
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)
|
||
{
|
||
case SHOOT_IDLE:
|
||
// 初始状态：钩子移动到顶部，钩住拉簧
|
||
if (shoot_thread & READY_TELL) // 如果收到等待通知
|
||
{
|
||
t_posSet = Tigger_ZERO;
|
||
if(trigger_Motor->total_angle > Tigger_ZERO-5 )
|
||
{
|
||
control_pos = TOP_POS;
|
||
limit_speed = TO_TOP; // 快速上去
|
||
|
||
|
||
}
|
||
if (feedback.fd_gopos < -209)
|
||
{
|
||
|
||
currentState = GO_TOP; // 切换到准备发射状态
|
||
}
|
||
}
|
||
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;
|
||
}
|
||
|
||
void Shoot::RemoveError()
|
||
{
|
||
currentState = SHOOT_IDLE;
|
||
control_pos = TOP_POS;
|
||
if (feedback.fd_gopos < -209)
|
||
{
|
||
t_posSet = Tigger_ZERO;
|
||
is_hooked = false;
|
||
}
|
||
if (trigger_Motor->total_angle >= Tigger_ZERO - 5)
|
||
{
|
||
// 认为完全松开
|
||
control_pos = INIT_POS;
|
||
BSP_Buzzer_Stop();
|
||
}
|
||
limit_speed = 5.0f; // 慢慢送上去
|
||
go1.Pos = control_pos;
|
||
}
|
||
|
||
#endif
|