87 lines
1.9 KiB
C
87 lines
1.9 KiB
C
#include "ball.h"
|
|
#include "main.h"
|
|
#include "cmsis_os.h"
|
|
#include "FreeRTOS.h"
|
|
#include "bsp_delay.h"
|
|
#include "go.h"
|
|
|
|
// PC6 ball_up_Pin读运球 检测到输出0,未检测到输出1
|
|
// PE13 down_Pin 下推
|
|
// PE14 paw_Pin 爪子
|
|
// PI6 ball_in2_Pin 读接球
|
|
// PI7 ball_in1_Pin 读接球
|
|
// PE11 key_Pin 收网
|
|
#define ballcome 1
|
|
#define balldown 0
|
|
|
|
extern GO go;
|
|
//光电识别
|
|
int ball_in(void)
|
|
{
|
|
return (HAL_GPIO_ReadPin(ball_up_GPIO_Port, ball_up_Pin));
|
|
}
|
|
|
|
int pass_ball(void)
|
|
{
|
|
|
|
|
|
return (HAL_GPIO_ReadPin(ball_in1_GPIO_Port, ball_in1_Pin));
|
|
}
|
|
|
|
//运球
|
|
void handling_ball(int hand, int angle)
|
|
{
|
|
|
|
int ball_state =0;//有球
|
|
int paw_state =0;//获取爪子开合状态
|
|
if( hand==1)
|
|
{
|
|
HAL_GPIO_WritePin(paw_GPIO_Port, paw_Pin, GPIO_PIN_SET);
|
|
paw_state =1;//爪子开
|
|
// osDelay(5);
|
|
HAL_GPIO_WritePin(down_GPIO_Port, down_Pin, GPIO_PIN_RESET);//打出
|
|
osDelay(500);
|
|
angle=80;
|
|
GO_TURN_ball(TURN,go,angle);//停止运球
|
|
ball_state =1;//无球
|
|
osDelay(500);
|
|
HAL_GPIO_WritePin(down_GPIO_Port, down_Pin, GPIO_PIN_SET);//收回
|
|
ball_state =ball_in();//获取爪子有无球状态
|
|
}
|
|
else if (hand==0)
|
|
{
|
|
angle=20;
|
|
GO_TURN_ball(TURN,go,angle);//停止运球
|
|
}
|
|
if (paw_state==1&&ball_state==0)
|
|
{
|
|
HAL_GPIO_WritePin(paw_GPIO_Port, paw_Pin, GPIO_PIN_RESET);//爪子合
|
|
osDelay(500);
|
|
ball_state=1;//有球清除标志
|
|
paw_state=0;//爪子合清除标志
|
|
hand=0;
|
|
|
|
}
|
|
|
|
|
|
}
|
|
|
|
//接球
|
|
int catch_ball(int inball)
|
|
{
|
|
inball =pass_ball();
|
|
if(inball==1)
|
|
{
|
|
//接球
|
|
HAL_GPIO_WritePin(key_GPIO_Port, key_Pin, GPIO_PIN_SET);
|
|
return ballcome;
|
|
}
|
|
else
|
|
{
|
|
HAL_GPIO_WritePin(key_GPIO_Port, key_Pin, GPIO_PIN_RESET);
|
|
return balldown;
|
|
}
|
|
|
|
|
|
}
|