89 lines
1.3 KiB
C
89 lines
1.3 KiB
C
|
#include "ball.h"
|
||
|
#include "main.h"
|
||
|
#include "cmsis_os.h"
|
||
|
#include "FreeRTOS.h"
|
||
|
#include "bsp_delay.h"
|
||
|
|
||
|
#define ballcome 1
|
||
|
#define balldown 0
|
||
|
|
||
|
//光电识别
|
||
|
int ball_in(void)
|
||
|
{
|
||
|
if (HAL_GPIO_ReadPin(ball_up_GPIO_Port, ball_up_Pin) == GPIO_PIN_SET)
|
||
|
{
|
||
|
return ballcome;
|
||
|
}
|
||
|
else
|
||
|
{
|
||
|
return balldown;
|
||
|
}
|
||
|
|
||
|
|
||
|
}
|
||
|
int pass_ball(void)
|
||
|
{
|
||
|
|
||
|
if (HAL_GPIO_ReadPin(ball_in1_GPIO_Port, ball_in1_Pin) == GPIO_PIN_SET)
|
||
|
{
|
||
|
return ballcome;
|
||
|
}
|
||
|
else
|
||
|
{
|
||
|
return balldown;
|
||
|
}
|
||
|
|
||
|
}
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
//运球
|
||
|
void handling_ball(int hand, int paw)
|
||
|
{
|
||
|
paw =ball_in();
|
||
|
if (hand == 1 && paw == 1)
|
||
|
{
|
||
|
//爪子
|
||
|
delay_us(100);
|
||
|
HAL_GPIO_WritePin(paw_GPIO_Port, paw_Pin, GPIO_PIN_RESET);
|
||
|
osDelay(2);
|
||
|
delay_us(100);
|
||
|
//上推
|
||
|
HAL_GPIO_WritePin(down_GPIO_Port, down_Pin, GPIO_PIN_RESET);
|
||
|
hand=0;
|
||
|
}
|
||
|
else
|
||
|
{
|
||
|
|
||
|
//爪子
|
||
|
HAL_GPIO_WritePin(paw_GPIO_Port, paw_Pin, GPIO_PIN_SET);
|
||
|
osDelay(2);
|
||
|
delay_us(100);
|
||
|
//下推
|
||
|
HAL_GPIO_WritePin(down_GPIO_Port, down_Pin, GPIO_PIN_SET);
|
||
|
}
|
||
|
|
||
|
|
||
|
|
||
|
|
||
|
}
|
||
|
|
||
|
|
||
|
|
||
|
//接球
|
||
|
int catch_ball(int inball)
|
||
|
{
|
||
|
inball =pass_ball();
|
||
|
if(inball==1)
|
||
|
{
|
||
|
return ballcome;
|
||
|
}
|
||
|
else
|
||
|
{
|
||
|
return balldown;
|
||
|
}
|
||
|
|
||
|
|
||
|
}
|