129 lines
3.1 KiB
C++
129 lines
3.1 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"
|
|
extern RC_ctrl_t rc_ctrl;
|
|
#define SHOOT_SPEED 40000
|
|
#define SHOOT_SPEED_BACK -4000
|
|
#define STOP 0
|
|
|
|
// 定义状态机变量
|
|
ShootState_t currentState = SHOOT_IDLE;
|
|
|
|
Shoot::Shoot()
|
|
{
|
|
// motor_5065[0] = get_5065_motor_point(0);//获取电机数据指针
|
|
// motor_5065[1] = get_5065_motor_point(1);//获取电机数据指针
|
|
CAN_VESC_RPM(1, STOP);
|
|
CAN_VESC_RPM(2, STOP);
|
|
|
|
}
|
|
|
|
void Shoot::shootThree()
|
|
{
|
|
if((rc_ctrl.sw[1]>500))
|
|
{
|
|
speed_5065 = map((float)rc_ctrl.sw[1],500,1694,30000,45000);
|
|
|
|
// CAN_VESC_RPM(1, speed_5065);
|
|
// CAN_VESC_RPM(2, speed_5065);
|
|
}
|
|
else
|
|
{
|
|
speed_5065=STOP;
|
|
// CAN_VESC_Control(1,STOP,&hcan2);
|
|
// CAN_VESC_RPM(1, STOP);
|
|
// CAN_VESC_RPM(2, STOP);
|
|
}
|
|
if(rc_ctrl.sw[5]==1694)
|
|
{
|
|
speed_5065=SHOOT_SPEED_BACK;
|
|
// CAN_VESC_Control(1,SHOOT_SPEED_BACK,&hcan2);
|
|
// CAN_VESC_RPM(1, SHOOT_SPEED_BACK);
|
|
// CAN_VESC_RPM(2, SHOOT_SPEED_BACK);
|
|
|
|
}
|
|
//CAN_VESC_Control(1,speed_5065,&hcan2);
|
|
CAN_VESC_RPM(1, speed_5065);
|
|
CAN_VESC_RPM(2, speed_5065);
|
|
}
|
|
|
|
void Shoot::shootBack()
|
|
{
|
|
if(rc_ctrl.sw[5]==1694)
|
|
{
|
|
|
|
CAN_VESC_RPM(1, SHOOT_SPEED_BACK);
|
|
CAN_VESC_RPM(2, SHOOT_SPEED_BACK);
|
|
}
|
|
|
|
}
|
|
|
|
void Shoot::shootStop()
|
|
{
|
|
CAN_VESC_HEAD(1);
|
|
CAN_VESC_HEAD(2);
|
|
|
|
}
|
|
|
|
void Shoot::shootControl()
|
|
{
|
|
shootThree();
|
|
|
|
}
|
|
|
|
void Shoot::shootStateMachine() {
|
|
switch (currentState) {
|
|
case SHOOT_IDLE: {
|
|
// 空闲状态，等待遥控器输入
|
|
if (rc_ctrl.sw[1] > 500) {
|
|
currentState = SHOOT_FIRE; // 切换到发射状态
|
|
}
|
|
break;
|
|
}
|
|
|
|
case SHOOT_FIRE: {
|
|
// 发射状态，控制电机发射
|
|
speed_5065 = map((float)rc_ctrl.sw[1], 500, 1694, 30000, 45000);
|
|
CAN_VESC_RPM(1, speed_5065);
|
|
CAN_VESC_RPM(2, speed_5065);
|
|
|
|
// 检测光电传感器是否触发
|
|
if (IS_PHOTOELECTRIC_TRIGGERED()) {
|
|
currentState = SHOOT_TRIGGERED; // 切换到光电触发状态
|
|
}
|
|
break;
|
|
}
|
|
|
|
case SHOOT_TRIGGERED: {
|
|
// 光电触发状态，停止发射
|
|
CAN_VESC_HEAD(1);
|
|
CAN_VESC_HEAD(2);
|
|
// 切换到返回状态
|
|
currentState = SHOOT_RETURN;
|
|
break;
|
|
}
|
|
|
|
case SHOOT_RETURN: {
|
|
// 自动返回状态，控制电机反转
|
|
CAN_VESC_RPM(1, SHOOT_SPEED_BACK);
|
|
CAN_VESC_RPM(2, SHOOT_SPEED_BACK);
|
|
|
|
// 检测返回完成（可以通过时间或其他传感器判断）
|
|
if (rc_ctrl.sw[1] < 500) { // 假设遥控器开关控制返回完成
|
|
currentState = SHOOT_IDLE; // 切换回空闲状态
|
|
}
|
|
break;
|
|
}
|
|
|
|
default: {
|
|
// 默认状态，回到空闲
|
|
currentState = SHOOT_IDLE;
|
|
break;
|
|
}
|
|
}
|
|
}
|