#ifndef BALL_HPP
#define BALL_HPP
#include "main.h"
#include "TopDefine.h"
#include "cmsis_os.h"
#include "FreeRTOS.h"
#include <cmsis_os2.h>
#include "bsp_delay.h"
#include "djiMotor.h"
#include "pid.h"
#include "filter.h"

// 定义状态枚举
typedef enum {
    BALL_IDLE,         // 空闲状态
    BALL_FORWARD,      // 正转状态
    BALL_DROP,         // 球下落状态
    BALL_REVERSE,      // 反转状态
    BALL_FLAG,
    BALL_CLOSE,         // 关闭状态
    BALL_FINISH,        // 完成状态

  //持球状态
    BALL_TAKE,
    BALL_LOSE

} BallState_t;

typedef enum
{
    MOTOR_1 = 0,
    MOTOR_2 = 1,
    MOTOR_3 = 2,
    MOTOR_DOWN = 3,
    MOTOR_NUM
}motorhangd_e;

  /* 滤波器 */
  typedef struct {
    LowPassFilter2p_t *in;  /* 反馈值滤波器 */
    LowPassFilter2p_t *out; /* 输出值滤波器 */
  } Filter;

// 定义光电传感器检测宏
#define IS_PHOTOELECTRIC_BALL() (HAL_GPIO_ReadPin(up_ball_GPIO_Port, up_ball_Pin) == GPIO_PIN_RESET)

class Ball
{
public:
    Ball();
    void Spin(float speed);
    void ballHadling(void);
    void ballDown(void);
    void Send_control(void);
    void ballStop(void);
    void ballTake(void);
    void Extend_control(int angle);

    BallState_t currentState1; // 当前状态
    int flag;//暂时还没用到
	  int ball_state ;//光电检测
    
    //伸缩6020
    int16_t e_result[2];
    motor_measure_t *  Extern_Motor[2];

    int16_t result[MOTOR_NUM];
    motor_measure_t *hand_Motor[MOTOR_NUM];
//==========================公共变量==========================//
    //用于传接球,运球后通知
    volatile BallState_t ballStatus;//是否有球
    volatile uint32_t flag_thread;//接收传回的线程通知

private:  
    //滤波一下
  

    //三个摩擦轮
    static const float M3508_speed_PID[3];
    static const float M3508_angle_PID[3];

    static const float Extend_speed_PID[3];
    static const float Extend_angle_PID[3];

    //电机速度pid结构体
    pid_type_def speed_pid[MOTOR_NUM];
    //位置环pid
    pid_type_def angle_pid[MOTOR_NUM];

    pid_type_def extend_speed_pid[2];
    pid_type_def extend_angle_pid[2];

    float angleSet[2];
    float speedSet[MOTOR_NUM];
};

#endif
