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_MIN 8.0f   //最小入仓长度,在最慢的速度下低于该长度也需要以SPEED_INSTORE缓慢入仓 cm  -------加了软减速后必须根据速度改变！！！！
#define INSTORE_LENGTH_SPEED  0.0002f   //最小入仓长度速度平方的(输出轴速度)比例,越快越长，加了软减速后必须根据速度改变！！！！



#define SPEED_MAX 200   //最快收放线速度--任何时候不应该超过这个速度
#define SPEED_INSTORE 5   //入仓速度,自动重量归零也是这个速度下降2cm
#define SPEED_BTN_SLOW 20   //按键慢收放线速度(没有设置顶点时只能按这个速度收线)
#define SPEED_BTN_MID 100   //按键中等收放线速度
#define SPEED_BTN_FAST 200   //按键最快收放线速度
#define SPEED_HOOK_FAST 100         //货物下放速度--有目标长度得情况下
#define SPEED_HOOK_SLOW 20   //货物快到地面速度--有目标长度得情况下
#define SPEED_HOOK_CHECK 50   //货物下放速度--没有目标长度得情况下能检测到脱钩的合适速度
#define SPEED_HOOK_UP 100     //空钩上升速度  
#define SPEED_UNBLOCK  0.1   //舵机堵转需要缓慢转的速度



#define TM_UNBLOCK 100   //舵机堵转需要缓慢转等待的时间ms
#define TM_SERVOLOCK 300   //舵机转到Lock和Unlock等待的时间ms
#define TM_ACC_HS 500    //货物快到地面减速时间--覆盖自动加减速时间--这个地方稍微长一些



#define SERVO_LOCKPOS  1890    //舵机锁定位置
#define SERVO_UNLOCKPOS  1780   //舵机解锁位置
#define SERVO_BLOCKUNLOCKPOS  1700   //舵机堵转需要转回的位置--可以和SERVO_UNLOCKPOS一样，为了速度快也可以更小

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

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

#define HOOK_SLOWDOWN_LENGTH  10   //下放物体时快到目标长度时慢速长度  (cm)
#define HOOK_UNHOOK_TIME  1000       //等待脱钩时间ms

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

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

} control_status_t;
// 锁定舵机状态
enum ServoStatus
{
  SS_WaitMotoStop,  //等待电机停止
  
  SS_ServoUnLocked,   // 

  SS_WaitUnLock,   // 
  SS_WaitServo,     //等待舵机不堵转
  SS_WaitUnBlock,      //等待舵机不堵转的位置--同时电机会缓慢旋转
  SS_ServoLocked   //成功停止
};







class Motocontrol
{
private:
  Servo* _lockservo;
  moto _moto_dji;
  float _start_cnt;
  float _target_cnt;
  control_status_t _controlstatus;
  bool _check_targetlength;
  ServoStatus  _servotatus;
  unsigned long _tm_servotatus;
  int _pullweight;
  HookStatus _hooksstatus;
  int _hook_targetlen;
  int _hook_currlen;
  float _curr_length;
  bool _weightalign;
  bool _autogoodsdown;
  uint8_t _overweightcount;

  uint8_t _notweightcount;
  uint8_t _unblocktimes;
  unsigned long  _tm_waitunhook;
  float _runspeed;

  void checkmotostatus();
  void checkhookstatus();
  void goodsstartup() ;
  void checkgoods();
  bool checkservoblock();
  void lockservo() ;
  void unlockservo() ; 
public:
    Motocontrol();                  // 构造函数
    ~Motocontrol();                 // 析构函数
    void setspeed(float motospeed,float acctime=-1); // 设置速度
    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

};