141 lines
4.0 KiB
C++
141 lines
4.0 KiB
C++
#include "ball.hpp"
|
|
#include "bsp_delay.h"
|
|
#include "remote_control.h"
|
|
#include "detect.h"
|
|
|
|
extern RC_ctrl_t rc_ctrl;
|
|
#define KP 0.8
|
|
#define KD 0.008
|
|
//PE9 刹车光电
|
|
//PE11 接球气缸
|
|
//PE13 爪子气缸
|
|
//PE14 下球气缸
|
|
//PC6 接球光电
|
|
//PI6 运球光电
|
|
|
|
int aaa=0;
|
|
// 定义状态机变量
|
|
BallState_t currentBallState = BALL_IDLE;
|
|
|
|
Ball ::Ball()
|
|
{
|
|
aaa=10;
|
|
//ballState =HAL_GPIO_ReadPin(in_ball_GPIO_Port, in_ball_Pin);
|
|
detect_init();
|
|
|
|
}
|
|
|
|
void Ball ::ballCatch(void)
|
|
{
|
|
|
|
if(rc_ctrl.ch[1]>=300)
|
|
{
|
|
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET);//收网
|
|
}
|
|
else if(rc_ctrl.ch[1]<300)
|
|
{
|
|
HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);//放网
|
|
}
|
|
|
|
}
|
|
|
|
void Ball ::ballFlow(void)
|
|
{
|
|
GO_M8010_send_data(&huart6, 1,0,0,angleSet[1],0.5,KP,KD);
|
|
}
|
|
|
|
void Ball ::ballZero(void)
|
|
{
|
|
GO_M8010_send_data(&huart6, 1,0,0,0,0,0,0);
|
|
}
|
|
|
|
void Ball ::ballHadling(void)
|
|
{
|
|
if(rc_ctrl.sw[0]==306)
|
|
{
|
|
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_SET);//爪子气缸 开爪
|
|
osDelay(200);
|
|
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);//下球气缸 打出
|
|
osDelay(500);
|
|
angleSet[1] = 2;
|
|
ballFlow();
|
|
}
|
|
else
|
|
{
|
|
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET);//爪子气缸 关爪
|
|
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET);//下球气缸 打入
|
|
angleSet[1] = 0;
|
|
ballFlow();
|
|
}
|
|
|
|
}
|
|
|
|
void Ball::ballStateMachine() {
|
|
switch (currentBallState) {
|
|
case BALL_IDLE: {
|
|
// 空闲状态，等待遥控器输入
|
|
if (rc_ctrl.sw[0] == 306) { // 假设遥控器开关控制开始运球
|
|
// HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 收网
|
|
// osDelay(200); // 等待收网完成
|
|
currentBallState = BALL_RELEASE; // 切换到下球气缸弹出状态
|
|
|
|
}
|
|
break;
|
|
}
|
|
|
|
case BALL_RELEASE: {
|
|
// 下球气缸弹出，爪子张开
|
|
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET); // 下球气缸弹出
|
|
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_SET); // 爪子张开
|
|
osDelay(200); // 等待动作完成
|
|
currentBallState = BALL_JOINT_REVERSE; // 切换到关节电机反转状态
|
|
break;
|
|
}
|
|
|
|
case BALL_JOINT_REVERSE: {
|
|
// 关节电机反转置水平位置
|
|
angleSet[1] = 2; // 设置关节电机目标角度为水平位置
|
|
ballFlow(); // 控制关节电机动作
|
|
osDelay(500); // 等待电机动作完成
|
|
currentBallState = BALL_FALLING; // 切换到球加速落下状态
|
|
break;
|
|
}
|
|
|
|
case BALL_FALLING: {
|
|
// 球加速落下，等待光电检测
|
|
if (IS_PHOTOELECTRIC_BALL()) {
|
|
currentBallState = BALL_WAIT_BOUNCE; // 切换到等待反弹状态
|
|
}
|
|
break;
|
|
}
|
|
|
|
case BALL_WAIT_BOUNCE: {
|
|
// 等待光电检测反弹
|
|
osDelay(50); // 简单延时，避免误触发
|
|
if (IS_PHOTOELECTRIC_BALL()) {
|
|
currentBallState = BALL_NET_CLOSE; // 切换到收网状态
|
|
}
|
|
break;
|
|
}
|
|
|
|
case BALL_NET_CLOSE: {
|
|
// 收网状态
|
|
// HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET); // 收网
|
|
// osDelay(200); // 等待收网完成
|
|
HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); // 关闭爪子
|
|
HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET); // 下球气缸复位
|
|
angleSet[1] = 1; // 关节电机复位
|
|
ballFlow();
|
|
currentBallState = BALL_IDLE; // 切换回空闲状态
|
|
break;
|
|
}
|
|
|
|
default: {
|
|
|
|
currentBallState = BALL_IDLE; // 默认回到空闲状态
|
|
|
|
break;
|
|
}
|
|
}
|
|
}
|