#pragma once
#include "Arduino.h"
#include "config.h"
#include "moto.h"
#define ROPE_MAXLENGTH 600   //最多能放600cm---实际绳子应该比这个长650之类的
#define WHEEL_DIAMETER 4.0   //轮子直径cm
#define INSTORE_LENGTH 15   //入仓长度,低于该长度要缓慢入仓 cm

#define SPEED_MAX 330   //最快收放线速度
#define SPEED_MIN 3   //入仓速度
#define SPEED_SLOW 20   //慢收放线速度
#define SPEED_FAST 100   //快收放线速度
#define SPEED_BTN_FAST 200   //最快收放线速度

#define WHEEL_PERIMETER  (WHEEL_DIAMETER * 3.1416)  //轮子周长
#define ROPE_MAXCOUNT (ROPE_MAXLENGTH/WHEEL_PERIMETER)   //最大圈数



// 挂钩状态
enum MotoStatus
{
  MS_Down,   // 自动下放中
  MS_Up,     // 回收中
  MS_Stop   //停止中
};

typedef struct
{
  bool is_instore;
  bool is_setzero;
  bool is_toplocked;
  float zero_cnt;
  float speed_targe;
  MotoStatus motostatus;

} control_status_t;





class Motocontrol
{
private:
  moto _moto_dji;
  float _start_cnt;
  float _target_cnt;
  control_status_t _controlstatus;
  bool _check_targetlength;
  
  void checkstatus();
public:
    Motocontrol();                  // 构造函数
    ~Motocontrol();                 // 析构函数
    void setspeed(float motospeed); // 设置速度
    void down(float length);        // 收线
    void up(float length);          // 放线
    void stop();                    // 停止
    void setzero();                 // 设置0长度位置
    int16_t getlength();            // 得到长度
    void update();                  // 更新
    bool init(); // 初始化
    void direct_speed(float motospeed);  //直接设置电机速度--立马生效,仅用于测试
    moto_measure_t getmotoinfo();        //得到电机信息   
    control_status_t getcontrolstatus();        //得到控制信息  
    void setlocked(bool locked);             //是否到顶锁定
    void moto_run(MotoStatus updown,float length= 0.0f) ;// 放线

};