#include "ball.hpp"
#include "bsp_delay.h"
#include "remote_control.h"
#include "detect.h"

extern RC_ctrl_t rc_ctrl;
extern int key;

// 定义状态机变量
BallState_t currentState1 = BALL_IDLE; // 当前状态
uint32_t startTime = 0;             // 用于记录延时的起始时间
int speedm=0;
int speedm1=0;

#define MOTOR_SPEED 1000

const fp32 Ball:: M3508_speed_PID[3] = {5, 0.01, 0};

//PE11 气缸

//三摩擦轮运球！！！

Ball ::Ball()
{  
   detect_init();
   for(int i = 0;i < MOTOR_NUM;i ++)
 {
    hand_Motor[i] = get_motor_point(i);  
    if(i <=3)
    {
        hand_Motor[i]->type = M3508;//设置电机类型
        PID_init(&speed_pid[i],PID_POSITION,M3508_speed_PID,16000, 10000);//pid初始化
    }  
    result[i] = 0;
    speedSet[i] = 0;    
 }
     
}

#if HANDLING3 == 1

void Ball ::Spin(float speed,float speed2)
{

    speedSet[MOTOR_1] = -speed;
    result[MOTOR_1] = PID_calc(&speed_pid[MOTOR_1],hand_Motor[MOTOR_1]->speed_rpm,speedSet[MOTOR_1]);

    speedSet[MOTOR_2] = speed;
    result[MOTOR_2] = PID_calc(&speed_pid[MOTOR_2],hand_Motor[MOTOR_2]->speed_rpm,speedSet[MOTOR_2]);

    speedSet[MOTOR_3] = speed2;
    result[MOTOR_3] = PID_calc(&speed_pid[MOTOR_3],hand_Motor[MOTOR_3]->speed_rpm,speedSet[MOTOR_3]);

}

	int flag =0;
	int ball_state = 0;
   
void Ball::ballHadling(void)
{

    ball_state =HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);//有球 0 无球 1
    switch (currentState1)
    {
    case BALL_IDLE:
       HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);//确保闭合气缸
        if (key > 0) // 检测按键是否被按下
        {
            speedm=-4500;
            speedm1=-4500;
            currentState1 = BALL_FORWARD; // 切换到正转状态
        }
        break;

    case BALL_FORWARD:

        if (  hand_Motor[MOTOR_1]->speed_rpm >= 4480&&hand_Motor[MOTOR_1]->speed_rpm <= 4530 &&
              hand_Motor[MOTOR_2]->speed_rpm <= -4480&&hand_Motor[MOTOR_2]->speed_rpm >= -4530 &&          
              hand_Motor[MOTOR_3]->speed_rpm <= -4480&&hand_Motor[MOTOR_3]->speed_rpm >= -4530 )
        {
            HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_SET);// 打开气缸
            currentState1 = BALL_DROP; // 切换到球下落状态
        }
        break;

    case BALL_DROP:

        if (ball_state == 1) //读光电 有球 0  无球 1
        {
            osDelay(200); // 延时200ms
            speedm=4500;
            speedm1=4500;
            currentState1 = BALL_REVERSE; // 切换到反转状态
        }
        
        break;

    case BALL_REVERSE:
       
        if (ball_state == 0) 
        {
            
            //flag=2;   //抽象的计次
            speedm=0; // 停止电机
            speedm1=0;
            currentState1 = BALL_CLOSE; // 切换到完成状态
        }
     
     break;

    case BALL_CLOSE:
       // osDelay(200); // 延时50ms
       if(ball_state == 0)
       {
           HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);
           currentState1 = BALL_FINISH; // 切换到完成状态
       }
        // if (flag == 2) 
        // {
        //     if(ball_state == 0)
        //     {
        //         HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET);
        //         currentState1 = BALL_FINISH; // 切换到完成状态
        //     }
          
        // }
        break;    

    case BALL_FINISH:
        osDelay(200); // 延时50ms
        key=0; // 重置按键状态
        flag=0;
        speedm=0;
        speedm1=0;
        currentState1 = BALL_IDLE; // 回到空闲状态
        break;

    default:
       currentState1 = BALL_IDLE; // 默认回到空闲状态
        break;
    }

}

void Ball::Send_control()
{
    CAN_cmd_200(result[MOTOR_1],result[MOTOR_2],result[MOTOR_3],0,&hcan1);
    osDelay(2);

}

#else
int flag =0;
int ball_state = 0;
// void Ball::ballHadling(void)
// {
//     ball_state =HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin);//有球 0 无球 1
//     switch (currentState1)
//     {
//     case BALL_IDLE:
    
//     if (key > 0) // 检测按键是否被按下
//      {
//         currentState1 = BALL_FORWARD; // 切换到正转状态
        
//      }
//         break;

//     case BALL_FORWARD:
//     HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_SET);
//     osDelay(50); 
//     HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_SET);

//     currentState1 = BALL_DROP; // 切换到球下落状态
//         break;

//     case BALL_DROP:
//     osDelay(100); // 
//     HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);
//     currentState1 = BALL_CLOSE; // 切换到反转状态
//      break;

//     case BALL_CLOSE:
//         osDelay(100);
//         if(ball_state == 0)
//         {
//            HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); 
//            currentState1 = BALL_REVERSE; // 切换到反转状态 
//         }
     
     

//     break;

//     case BALL_REVERSE:
//      osDelay(50); // 延时200ms
//      HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET);
//      HAL_GPIO_WritePin(DOWN_GPIO_Port, DOWN_Pin, GPIO_PIN_RESET);
//      key=0; // 重置按键状态
//      currentState1 = BALL_IDLE; // 回到空闲状态
//      break;
    
//      default:
//       currentState1 = BALL_IDLE; // 默认回到空闲状态
//       break;
//   }
    
// }    
 int triggerCount = 0; // 光电传感器触发计数
 int last_ball_state = 1; // 上一次的光电状态
void Ball::ballHadling(void)
{
  
    ball_state = HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin); // 读取光电状态（有球 0，无球 1）

    switch (currentState1)
    {
    case BALL_IDLE:
        HAL_GPIO_WritePin(CLOSE_GPIO_Port, CLOSE_Pin, GPIO_PIN_RESET); // 确保气缸闭合
        if (key > 0) // 检测按键是否被按下
        {
            key = 0; // 重置按键状态
            triggerCount = 0; // 重置触发计数
            currentState1 = BALL_FORWARD; // 切换到正转状态
        }
        break;

    case BALL_FORWARD:
        HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_SET); // 打开气缸爪子
        osDelay(10); 
        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(10); // 延时去抖
            HAL_GPIO_WritePin(PAW_GPIO_Port, PAW_Pin, GPIO_PIN_RESET); // 闭合气缸爪子
            currentState1 = BALL_REVERSE; // 切换到反转状态
        }
        break;

    case BALL_REVERSE:
        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;
    }
}   
 #endif