250 lines
6.3 KiB
C++
250 lines
6.3 KiB
C++
#include "ball.hpp"
|
||
#include "bsp_delay.h"
|
||
#include "remote_control.h"
|
||
#include "detect.h"
|
||
#include "userTask.h"
|
||
#include "user_math.h"
|
||
#include "shoot.hpp"
|
||
|
||
extern RC_ctrl_t rc_ctrl;
|
||
extern int key;
|
||
|
||
//伸缩
|
||
#define M_SPEED 4000
|
||
#define I_ANGLE1 180
|
||
#define I_ANGLE2 -75
|
||
#define O_ANGLE1 340
|
||
#define O_ANGLE2 -240
|
||
|
||
|
||
const fp32 Ball:: Extend_speed_PID[3] = { 40, 0.0, 0.1};
|
||
const fp32 Ball:: Extend_angle_PID[3]= { 25, 0.1, 0};
|
||
|
||
|
||
//PE11 气缸
|
||
|
||
Ball ::Ball()
|
||
{
|
||
detect_init();
|
||
|
||
//两个伸缩6020 左207 右208
|
||
Extern_Motor[0] = get_motor_point(6);
|
||
Extern_Motor[1] = get_motor_point(7);
|
||
|
||
Extern_Motor[0]->type = GM6020;
|
||
Extern_Motor[1]->type = GM6020;
|
||
PID_init(&extend_speed_pid[0],PID_POSITION,Extend_speed_PID,10000, 2000);
|
||
PID_init(&extend_angle_pid[0],PID_POSITION,Extend_angle_PID,20000, 1000);
|
||
|
||
PID_init(&extend_speed_pid[1],PID_POSITION,Extend_speed_PID,10000, 2000);
|
||
PID_init(&extend_angle_pid[1],PID_POSITION,Extend_angle_PID,20000, 1000);
|
||
|
||
e_result[0] = 0;
|
||
e_result[1] = 0;
|
||
angleSet[0] = 0;
|
||
angleSet[1] = 0;
|
||
|
||
//状态机状态初始化
|
||
currentState1= BALL_IDLE;
|
||
|
||
|
||
}
|
||
|
||
|
||
void Ball ::Extend_mcontrol(int angle1,int angle2)
|
||
{
|
||
int16_t delta[2];
|
||
angleSet[0] = angle1/2;
|
||
angleSet[1] = angle2/2;
|
||
|
||
fp32 angle_get[2];
|
||
angle_get[0] = motor_ecd_to_angle_change(Extern_Motor[0]->ecd, angleSet[0]);
|
||
angle_get[1] = motor_ecd_to_angle_change(Extern_Motor[1]->ecd, angleSet[1]);
|
||
|
||
delta[0] = PID_calc(&extend_angle_pid[0], angle_get[0] , angleSet[0]);
|
||
e_result[0] = PID_calc(&extend_speed_pid[0], Extern_Motor[0]->speed_rpm, delta[0]);
|
||
|
||
delta[1] = PID_calc(&extend_angle_pid[1], angle_get[1] , angleSet[1]);
|
||
e_result[1] = PID_calc(&extend_speed_pid[1], Extern_Motor[1]->speed_rpm, delta[1]);
|
||
|
||
}
|
||
|
||
|
||
|
||
// E键 sw[1] 👆 200 shoot 中 1000 stop sw[2]👇1800
|
||
// G键 sw[6]👆 1800 中 1000 👇200
|
||
// sw[5] 👆 200 👇1800
|
||
//左旋 sw[7] 200 --1800
|
||
void Ball::rc_mode()
|
||
{
|
||
if(rc_ctrl.sw[6]==1800)
|
||
{
|
||
rc_key=UP2;
|
||
}
|
||
if(rc_ctrl.sw[6]==1000)
|
||
{
|
||
rc_key=MIDDLE2;
|
||
}
|
||
if(rc_ctrl.sw[6]==200)
|
||
{
|
||
rc_key=DOWN2;
|
||
}
|
||
|
||
if(rc_ctrl.sw[5]==200)
|
||
{
|
||
extern_key=IN;
|
||
}
|
||
if(rc_ctrl.sw[5]==1800)
|
||
{
|
||
extern_key=OUT;
|
||
}
|
||
|
||
}
|
||
|
||
void Ball::Move_Extend(void)
|
||
{
|
||
if(extern_key==IN)
|
||
{
|
||
Extend_mcontrol(I_ANGLE1, I_ANGLE2); // 内伸
|
||
}
|
||
else if(extern_key==OUT)
|
||
{
|
||
Extend_mcontrol(O_ANGLE1, O_ANGLE2); // 外伸
|
||
}
|
||
else
|
||
{
|
||
Extend_mcontrol(O_ANGLE1, O_ANGLE2); // 外伸
|
||
}
|
||
}
|
||
|
||
void Ball::ballDown(void)
|
||
{
|
||
|
||
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_SET); // 打开气缸爪子
|
||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||
|
||
}
|
||
|
||
void Ball::ball_control()
|
||
{
|
||
ball_state = HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin); // 读取光电状态(有球 0，无球 1)
|
||
|
||
Move_Extend();
|
||
|
||
switch (rc_key){
|
||
case MIDDLE2:
|
||
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); //确保爪气缸关闭
|
||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||
if (currentState1 == BALL_FINISH)
|
||
{
|
||
currentState1 = BALL_IDLE;
|
||
}
|
||
else {
|
||
currentState1 = BALL_IDLE; // 默认回到空闲状态
|
||
}
|
||
break;
|
||
|
||
case UP2:
|
||
ballHadling();
|
||
break;
|
||
|
||
case DOWN2:
|
||
ballDown();
|
||
|
||
break;
|
||
|
||
}
|
||
|
||
Send_control();
|
||
|
||
}
|
||
|
||
|
||
void Ball::Send_control()
|
||
{
|
||
|
||
CAN_cmd_1FF(0,0,e_result[0],e_result[1],&hcan1);
|
||
osDelay(1);
|
||
|
||
}
|
||
|
||
|
||
int ball_state = 0;
|
||
int triggerCount = 0; // 光电传感器触发计数
|
||
int last_ball_state = 1; // 上一次的光电状态
|
||
//(有球 0,无球 1)
|
||
void Ball::ballHadling(void)
|
||
{
|
||
|
||
switch (currentState1)
|
||
{
|
||
case BALL_IDLE:
|
||
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); //确保爪气缸关闭
|
||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||
if (key > 0 ||rc_key == UP2) // 检测按键是否被按下
|
||
{
|
||
key = 0; // 重置按键状态
|
||
triggerCount = 0; // 重置触发计数
|
||
currentState1 = BALL_FORWARD;
|
||
}
|
||
break;
|
||
|
||
case BALL_FORWARD:
|
||
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_SET); // 打开气缸爪子
|
||
osDelay(5);
|
||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET); // 打开下气缸
|
||
currentState1 = BALL_DROP; // 切换到球下落状态
|
||
break;
|
||
|
||
case BALL_DROP:
|
||
osDelay(100); // 延时 100ms
|
||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 关闭下气缸
|
||
if (ball_state == 0 && last_ball_state == 1) // 检测到状态从无球变为有球
|
||
{
|
||
//osDelay(10); // 延时去抖动
|
||
triggerCount++; // 增加触发计数
|
||
if (triggerCount == 1) // 第一次触发
|
||
{
|
||
currentState1 = BALL_FLAG; // 切换到等待第二次触发状态
|
||
}
|
||
}
|
||
last_ball_state = ball_state; // 更新上一次的状态
|
||
break;
|
||
|
||
case BALL_FLAG:
|
||
osDelay(10); // 延时 50ms
|
||
if (triggerCount == 1 && ball_state == 0 && last_ball_state == 1) // 第二次检测到球
|
||
{
|
||
triggerCount++; // 增加触发计数
|
||
currentState1 = BALL_CLOSE; // 切换到闭合气缸状态
|
||
}
|
||
last_ball_state = ball_state; // 更新上一次的状态
|
||
break;
|
||
|
||
case BALL_CLOSE:
|
||
if (triggerCount == 2) // 确保是第二次触发
|
||
{
|
||
osDelay(100); // 延时去抖
|
||
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); // 闭合气缸爪子
|
||
currentState1 = BALL_FINISH; // 切换到反转状态
|
||
}
|
||
break;
|
||
|
||
case BALL_FINISH:
|
||
osDelay(50); // 延时 50ms
|
||
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); // 确保气缸爪子闭合
|
||
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 确保下气缸关闭
|
||
key = 0; // 重置按键状态
|
||
triggerCount = 0; // 重置触发计数
|
||
|
||
//currentState1 = BALL_IDLE; // 回到空闲状态
|
||
|
||
break;
|
||
|
||
default:
|
||
currentState1 = BALL_IDLE; // 默认回到空闲状态
|
||
break;
|
||
}
|
||
}
|
||
|