266 lines
7.2 KiB
C++
266 lines
7.2 KiB
C++
#include "TopDefine.h"
|
||
#include "shoot.hpp"
|
||
#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"
|
||
|
||
extern RC_ctrl_t rc_ctrl;
|
||
NUC_t nuc_v;
|
||
|
||
//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
|
||
|
||
const fp32 Shoot:: M2006_speed_PID[3] = {5, 0, 0};
|
||
const fp32 Shoot:: M2006_angle_PID[3] = { 25, 0, 0.1};
|
||
|
||
#define INIT_POS -100
|
||
#define TOP_POS -210
|
||
#define Tigger_DO -5
|
||
#define Tigger_ZERO 130
|
||
|
||
float knob_increment;
|
||
|
||
Shoot::Shoot()
|
||
{
|
||
//扳机初始化
|
||
trigger_Motor= get_motor_point(4);//id 205
|
||
trigger_Motor->type=M2006;
|
||
PID_init(&speed_pid,PID_POSITION,M2006_speed_PID,3000, 2000);//pid初始化
|
||
PID_init(&angle_pid,PID_POSITION,M2006_angle_PID,3000, 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,
|
||
|
||
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;
|
||
|
||
|
||
}
|
||
|
||
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
|
||
// result = PID_calc(&speed_pid,trigger_Motor->speed_rpm,t_speedSet);
|
||
CAN_cmd_1FF(result,0,0,0,&hcan1);
|
||
}
|
||
|
||
|
||
void Shoot :: distanceGet(const NUC_t &nuc_v)
|
||
{
|
||
|
||
//test_distance=nuc_v.vision.x; // 获取自瞄数据
|
||
|
||
}
|
||
|
||
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;
|
||
}
|
||
|
||
//E键 sw[1]👆 200 shoot 中 1000 stop 👇1800 error
|
||
void Shoot::rc_mode()
|
||
{
|
||
if(rc_ctrl.sw[1]==200)
|
||
{
|
||
rc_key=UP1;
|
||
}
|
||
if(rc_ctrl.sw[1]==1000)
|
||
{
|
||
rc_key=MIDDLE1;
|
||
}
|
||
if(rc_ctrl.sw[1]==1800)
|
||
{
|
||
rc_key=DOWN1;
|
||
}
|
||
|
||
if(rc_ctrl.sw[7]==200)
|
||
{
|
||
trigger_key=WAIT;
|
||
}
|
||
if(rc_ctrl.sw[7]==1800)
|
||
{
|
||
trigger_key=SHOOT;
|
||
}
|
||
|
||
//knob_increment=rc_ctrl.ch[2]/150;
|
||
|
||
//旋钮增量
|
||
static int last_knob_value = 0; // 记录旋钮的上一次值
|
||
int current_knob_value = rc_ctrl.sw[4]; // 获取当前旋钮值
|
||
|
||
// 计算旋钮增量
|
||
if (current_knob_value >= 200 && current_knob_value <= 1800) {
|
||
knob_increment = (current_knob_value - last_knob_value) / 50.0f; // 每80单位对应一个增量
|
||
} else {
|
||
knob_increment = 0; // 如果旋钮值超出范围，不产生增量
|
||
}
|
||
|
||
}
|
||
|
||
void Shoot::shoot_control() {
|
||
|
||
//方便调试
|
||
feedback.fd_gopos = go1_data->Pos;
|
||
feedback.fd_tpos = trigger_Motor->total_angle;
|
||
|
||
switch (rc_key) {
|
||
case DOWN1:
|
||
// 中间挡位：调节拉簧蓄力电机位置
|
||
control_pos = INIT_POS; // 默认中间挡位位置
|
||
//fire_pos = control_pos + 10; // 发射位置（可调节）
|
||
fire_pos = control_pos + knob_increment; // 根据旋钮值调整发射位置
|
||
//fire_pos +=knob_increment;
|
||
go1.Pos = fire_pos; // 设置蓄力电机位置
|
||
|
||
if (currentState == SHOOT_READY) {
|
||
// 如果当前状态是准备发射，松开钩子发射
|
||
t_posSet = Tigger_ZERO; // 扳机扣动
|
||
if (t_posSet >= 120) { // 假设120度为发射完成角度
|
||
currentState = SHOOT_IDLE; // 切换到空闲状态
|
||
is_hooked = false; // 重置钩子状态
|
||
}
|
||
} else {
|
||
currentState = SHOOT_IDLE; // 默认回到空闲状态
|
||
}
|
||
break;
|
||
|
||
case MIDDLE1:
|
||
shoot_Current();
|
||
break;
|
||
|
||
case UP1:
|
||
RemoveError();
|
||
break;
|
||
|
||
default:
|
||
break;
|
||
}
|
||
|
||
// 发送数据到蓄力电机
|
||
GO_SendData(go1.Pos, 5.0f);
|
||
|
||
// 控制扳机电机
|
||
trigger_control();
|
||
}
|
||
|
||
void Shoot :: shoot_Current()
|
||
{
|
||
|
||
switch (currentState) {
|
||
case SHOOT_IDLE:
|
||
// 初始状态：钩子移动到顶部，钩住拉簧
|
||
control_pos = TOP_POS; // 顶部位置
|
||
go1.Pos = control_pos;
|
||
t_posSet = Tigger_ZERO; // 扳机松开
|
||
if (feedback.fd_gopos <-209) {
|
||
currentState = SHOOT_TOP; // 切换到准备发射状态
|
||
}
|
||
break;
|
||
|
||
case SHOOT_TOP:
|
||
t_posSet = Tigger_DO; // 扳机扣动钩住
|
||
osDelay(500); // 等待一段时间，确保扳机动作完成
|
||
if (fd_tpos <1)
|
||
{
|
||
//判定为钩住
|
||
is_hooked = true; // 标记钩子已钩住
|
||
currentState = SHOOT_READY; // 切换到准备发射状态
|
||
}
|
||
break;
|
||
|
||
case SHOOT_READY:
|
||
if (is_hooked)
|
||
{
|
||
go1.Pos = fire_pos; // 下拉到中间挡位位置
|
||
if (feedback.fd_gopos >= fire_pos - 0.3f && feedback.fd_gopos <= fire_pos + 0.3f) {
|
||
//currentState = SHOOT_WAIT; // 等待发射信号
|
||
//在拨杆切换时触发了
|
||
}
|
||
}
|
||
break;
|
||
// case SHOOT_WAIT:
|
||
// // 等待发射信号
|
||
// if (trigger_key == SHOOT) {
|
||
// currentState = SHOOT_FIRE; // 切换到发射状态
|
||
// }
|
||
// break;
|
||
|
||
// case SHOOT_FIRE:
|
||
// // 发射逻辑
|
||
// t_posSet = Tigger_ZERO; // 扳机扣动
|
||
// if (t_posSet <= 50) { // 假设250度为发射完成角度
|
||
// currentState = SHOOT_IDLE; // 切换到空闲状态
|
||
// is_hooked = false; // 重置钩子状态
|
||
// }
|
||
// break;
|
||
|
||
default:
|
||
currentState = SHOOT_IDLE; // 默认回到空闲状态
|
||
break;
|
||
}
|
||
}
|
||
|
||
void Shoot::RemoveError() {
|
||
currentState = SHOOT_IDLE;
|
||
control_pos = TOP_POS;
|
||
go1.Pos = control_pos;
|
||
if(feedback.fd_gopos >= control_pos - 0.5f && feedback.fd_gopos <= control_pos + 0.5f)
|
||
{
|
||
t_posSet = Tigger_ZERO;
|
||
is_hooked = false;
|
||
}
|
||
}
|