#ifndef TAKE_HPP
#define TAKE_HPP
#include "djiMotor.h"
#include "pid.h"

typedef enum
{
    //上下3508顶端
    POINT_TOP = -2280,
    //上下3508暂停点
    POINT_MID = -1720,
    //上下3508取球点
    POINT_BUTTOM = 0,
    //前后2006取球点
    POINT_TAKE = -730,
    //前后2006放球点
    POINT_PUT = 0,
    //翻转3508放球点
    POINT_TURN_LEFT = -350,
    POINT_TURN_RIGHT = 350,
    POINT_TURN_ZERO = 0,
    //归零死区，没有柔性控制，防止堵转
    POINT_DEAD = 5,
    
}point_e;//存放位置宏定义

typedef enum
{
    MOTOR_UP = 0,
    MOTOR_TURN = 1,
    MOTOR_RING_RIGHT = 2,
    MOTOR_RING_LEFT = 3,
    MOTOR_NUM
}motorput_e;

class Take
{
public:
    Take();
    //下降到取球点
    void fall();
    void mid();//平常停在中间
    void top();
    void putRight();
    void putLeft();
    void takeRight();
    void takeLeft();
    void left();
    void right();
    void turnMid();
		void turnFlow();
    //暂存要发送的电流
    int16_t result[MOTOR_NUM]; 

    //电机状态
    
private:
    motor_measure_t* putMotor[MOTOR_NUM];
    //上下3508pid
    static const float TakeRing_speed_PID[3];
    static const float TakeRing_angle_PID[3];
    //翻转3508pid
    static const float M3508_speed_PID[3];
    static const float M3508_angle_PID[3];
    //前后与夹球2006pid
    static const float Turn_speed_PID[3];
    static const float Turn_angle_PID[3];
    //电机速度pid结构体
    pid_type_def speed_pid[MOTOR_NUM];
    //位置环pid
    pid_type_def angle_pid[MOTOR_NUM];
    //暂存目标位置
    //0上下，1翻转，2前后，3夹球
    float angleSet[MOTOR_NUM];
};

#endif