PullupDev/src/motocontrol.h

#pragma once
#include "Arduino.h"
#include "config.h"
#include "moto.h"
#include <ESP32Servo.h>

#define ROPE_MAXLENGTH 600   //最多能放600cm---实际绳子应该比这个长650之类的
#define WHEEL_DIAMETER 4.0   //轮子直径cm
#define WHEEL_PERIMETER  (WHEEL_DIAMETER * 3.1416)  //轮子周长
#define ROPE_MAXCOUNT (ROPE_MAXLENGTH/WHEEL_PERIMETER)   //最大圈数

#define INSTORE_LENGTH 10   //入仓长度,低于该长度要缓慢入仓 cm

#define SPEED_MAX 330   //最快收放线速度--任何时候不应该超过这个速度
#define SPEED_INSTORE 5   //入仓速度,自动重量归零也是这个速度下降2cm
#define SPEED_BTN_SLOW 20   //按键慢收放线速度(没有设置顶点时只能按这个速度收线)
#define SPEED_BTN_MID 100   //按键中等收放线速度
#define SPEED_BTN_FAST 200   //按键最快收放线速度

#define SPEED_HOOK_FAST 200         //货物下放速度--有目标长度得情况下
#define SPEED_HOOK_SLOW 20   //货物快到地面速度--有目标长度得情况下
#define SPEED_HOOK_CHECK 50   //货物下放速度--没有目标长度得情况下能检测到脱钩的合适速度
#define SPEED_HOOK_UP 50     //空钩上升速度  



#define SERVO_LOCKPOS  1900    //舵机锁定位置
#define SERVO_UNLOCKPOS  1500   //舵机解锁位置

#define HOOK_WEIHT_MIN  10   //最小货物重量 小于这个认为没挂东西  (克)

#define HOOK_WEIHT_MAX  5000   //最大货物重量 大于这个认为超重不工作  (克)

#define HOOK_SLOWDOWN_LENGTH  50   //下放物体时快到目标长度时慢速长度  (cm)

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

typedef struct
{
  bool is_instore;        //正在进仓
  bool is_setzero;        //已设置0长度位置
  bool is_toplocked;      //已到顶锁住
  bool is_overweight;     //是否超重
  float zero_cnt;         //0长度位置
  float speed_targe;      //当前目标速度
  MotoStatus motostatus;  //电机运行状态

} control_status_t;





class Motocontrol
{
private:
  Servo* _lockservo;
  moto _moto_dji;
  float _start_cnt;
  float _target_cnt;
  control_status_t _controlstatus;
  bool _check_targetlength;
  bool _need_closemoto;
  unsigned long _tm_closemoto;
  int _pullweight;
  HookStatus _hooksstatus;
  int _hook_targetlen;
  int _hook_currlen;
  float _curr_length;
  bool _weightalign;
  bool _autogoodsdown;
  u_int8_t _overweightcount;
  void checkmotostatus();
  void checkhookstatus();
  bool havegoods();
  void goodsstartup() ;
  void checkoverweight();
public:
    Motocontrol();                  // 构造函数
    ~Motocontrol();                 // 析构函数
    void setspeed(float motospeed); // 设置速度
    void stop();                    // 停止
    void setzero();                 // 设置0长度位置
    int16_t getlength();            // 得到长度
    void update();                  // 更新
    bool init(Servo* lockservo); // 初始化
    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) ;// 收放线
    void setweight(int pullweight);//设置货物实时重量
    void moto_goodsdown(float length= 0.0f);// 放下货物,自动回收
    HookStatus gethooktatus(){return _hooksstatus;}  //得到挂钩状态
    void weightalign(bool weightalign);         //重量传感器是否校准--必须校准才能执行moto_hookdown

};