178 lines
4.4 KiB
C++
178 lines
4.4 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"
|
|
|
|
extern RC_ctrl_t rc_ctrl;
|
|
float vofa[8];
|
|
#define SHOOT_SPEED 30500
|
|
#define SHOOT_SPEED_BACK -2500
|
|
#define Error 50
|
|
|
|
int test_speed =30500;
|
|
|
|
#define STOP 0
|
|
|
|
#define Trigger_Torque -5000
|
|
|
|
//sw[7]👆 1694 中 1000 👇306
|
|
//sw[2]👆 1694 👇306
|
|
|
|
//F键 sw[0]👆 1800 中 1000 👇200
|
|
//E键 sw[1]👆 1800 👇200
|
|
|
|
const fp32 Shoot:: M2006_speed_PID[3] = {5, 0, 0};
|
|
int t=0;
|
|
|
|
Shoot::Shoot()
|
|
{
|
|
//扳机初始化
|
|
trigger_Motor= get_motor_point(4);
|
|
trigger_Motor->type=M2006;
|
|
PID_init(&speed_pid,PID_POSITION,M2006_speed_PID,6000, 1000);//pid初始化
|
|
speedSet = 0;
|
|
result = 0;
|
|
|
|
//发射摩擦轮
|
|
motor_5065[0] = get_vesc_point(0);//获取电机数据指针
|
|
motor_5065[1] = get_vesc_point(1);//获取电机数据指针
|
|
speed_5065=0;
|
|
currentState= SHOOT_IDLE;
|
|
}
|
|
|
|
void Shoot::trigger_control()
|
|
{
|
|
///speedSet=speed;
|
|
//result = PID_calc(&speed_pid, trigger_Motor->speed_rpm, speedSet);
|
|
//result = Trigger_Torque;
|
|
CAN_cmd_1FF(result,0,0,0,&hcan1);
|
|
|
|
}
|
|
|
|
void Shoot::shootThree()
|
|
{
|
|
if((rc_ctrl.sw[7]==1694))
|
|
{
|
|
|
|
speed_5065 = SHOOT_SPEED;
|
|
|
|
}
|
|
if((rc_ctrl.sw[7]==1000))
|
|
{
|
|
speed_5065=STOP;
|
|
//发一次会堵塞另一个
|
|
// CAN_VESC_HEAD(1);
|
|
// CAN_VESC_HEAD(2);
|
|
}
|
|
if(rc_ctrl.sw[7]==306)
|
|
{
|
|
speed_5065=SHOOT_SPEED_BACK;
|
|
|
|
}
|
|
CAN_VESC_Control(1,speed_5065,&hcan2);
|
|
// CAN_VESC_RPM(1, speed_5065);
|
|
// CAN_VESC_RPM(2, speed_5065);
|
|
|
|
// vofa[0] = motor_5065[1]->rotor_speed;
|
|
// vofa[1] = motor_5065[0]->rotor_speed; // 发送电机速度数据
|
|
// vofa_tx_main(vofa); // 发送数据到虚拟串口
|
|
}
|
|
|
|
void Shoot::shootBack()
|
|
{
|
|
speed_5065=SHOOT_SPEED_BACK;
|
|
CAN_VESC_Control(1,speed_5065,&hcan2);
|
|
|
|
}
|
|
|
|
void Shoot::shootStop()
|
|
{
|
|
speed_5065=STOP;
|
|
CAN_VESC_Control(1,speed_5065,&hcan2);
|
|
|
|
}
|
|
|
|
void Shoot::shootStateMachine() {
|
|
|
|
distance =3.5;
|
|
switch (currentState) {
|
|
case SHOOT_IDLE: {
|
|
speed_5065=STOP;
|
|
result=STOP;
|
|
// 空闲状态，等待遥控器输入
|
|
if((rc_ctrl.sw[0]==1800)) {
|
|
currentState = SHOOT_FIRE; // 切换到发射状态
|
|
}
|
|
break;
|
|
}
|
|
|
|
case SHOOT_FIRE: {
|
|
// 发射状态，控制电机发射
|
|
// speed_5065 = test_speed;
|
|
speed_5065 =distanceToSpeed(distance);
|
|
if(motor_5065[0]->rotor_speed>=speed_5065-Error && motor_5065[0]->rotor_speed<=speed_5065+Error &&
|
|
motor_5065[1]->rotor_speed>=speed_5065-Error && motor_5065[1]->rotor_speed<=speed_5065+Error)
|
|
{
|
|
result=Trigger_Torque;//恒力扳机
|
|
}
|
|
|
|
// 检测光电传感器是否触发
|
|
if (IS_PHOTOELECTRIC_TRIGGERED()) {
|
|
currentState = SHOOT_BACK; // 切换到光电触发状态
|
|
}
|
|
break;
|
|
}
|
|
|
|
case SHOOT_BACK: {
|
|
// 光电触发状态，停止发射
|
|
result=STOP;
|
|
speed_5065=STOP;
|
|
|
|
// 切换到返回状态
|
|
currentState = SHOOT_RETURN;
|
|
break;
|
|
}
|
|
|
|
case SHOOT_RETURN: {
|
|
// 自动返回状态，控制电机反转
|
|
speed_5065=SHOOT_SPEED_BACK;
|
|
result=-Trigger_Torque;
|
|
|
|
// 检测返回完成（可以通过时间或其他传感器判断）
|
|
if (rc_ctrl.sw[0]==1000) { // 假设遥控器开关控制返回完成
|
|
speed_5065=SHOOT_SPEED_BACK;
|
|
result=STOP;
|
|
currentState = SHOOT_IDLE; // 切换回空闲状态
|
|
}
|
|
break;
|
|
}
|
|
|
|
default: {
|
|
// 默认状态，回到空闲
|
|
currentState = SHOOT_IDLE;
|
|
break;
|
|
}
|
|
}
|
|
CAN_VESC_Control(1,speed_5065,&hcan2);
|
|
|
|
vofa[0] = motor_5065[1]->rotor_speed;
|
|
vofa[1] = motor_5065[0]->rotor_speed; // 发送电机速度数据
|
|
vofa[2] = speed_5065; // 发送电机速度数据
|
|
|
|
vofa_tx_main(vofa); // 发送数据到虚拟串口
|
|
|
|
}
|
|
|
|
//拟合函数
|
|
float distanceToSpeed(float x) {
|
|
return -2.3958f * x * x * x * x
|
|
+ 59.615f * x * x * x
|
|
- 525.63f * x * x
|
|
+ 2136.4f * x
|
|
+ 28001.0f;
|
|
}
|