#include "ball.h"
#include "main.h"
#include "cmsis_os.h"
#include "FreeRTOS.h"
#include "bsp_delay.h"
#include "go.h"

// PC6  ball_up_Pin读运球
// PE13 down_Pin 下推
// PE14 paw_Pin  爪子
// PI6 ball_in1_Pin 读接球
// PI7 ball_in2_Pin 读接球
// PE11 key_Pin 收网
#define ballcome 1
#define balldown 0

extern GO go;
//光电识别
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,int angle) 
{
    GO_TURN_ball(TURN,go,angle);//停止运球
    paw =ball_in();//获取爪子有无球状态
    if (hand == 1 && paw == 1)
    {
        //爪子
        delay_us(100);
        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);
        osDelay(2);
        HAL_GPIO_WritePin(down_GPIO_Port, down_Pin, GPIO_PIN_RESET);

        hand=0;//清除手指状态
    }
    else if(hand == 0 && paw == 1)
    {
        
        //闭合爪子
        HAL_GPIO_WritePin(paw_GPIO_Port, paw_Pin, GPIO_PIN_RESET);
        osDelay(2);

    }
    

}



//接球
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;
    }

    
}