#pragma once #include "Arduino.h" #include "config.h" #include "moto.h" #include #define ROPE_MAXLENGTH 700 //最多能放700cm---实际绳子应该比这个长750之类的 #define WHEEL_DIAMETER 3.8 //轮子直径cm #define WHEEL_PERIMETER (WHEEL_DIAMETER * 3.1416) //轮子周长 #define ROPE_MAXCOUNT (ROPE_MAXLENGTH/WHEEL_PERIMETER) //最大圈数 #define INSTORE_LENGTH_MIN_NONE 10.0f //最小入仓长度,没有挂东西的情况，在最慢的速度下低于该长度也需要以SPEED_INSTORE缓慢入仓 cm -------加了软减速后必须根据速度改变！！！！ #define INSTORE_LENGTH_MIN_GOODS 8.0f //最小入仓长度,有挂东西的情况 在最慢的速度下低于该长度也需要以SPEED_INSTORE缓慢入仓 cm -------加了软减速后必须根据速度改变！！！！ #define INSTORE_LENGTH_SPEED 0.05f // 0.0001f最小入仓长度速度平方的(输出轴速度)比例,越快越长，加了软减速后必须根据速度改变！！！！ #define SPEED_MAX 350 //最快收放线速度--任何时候不应该超过这个速度 #define SPEED_INSTORE 5 //入仓速度,自动重量归零也是这个速度下降2cm #define SPEED_BTN_SLOW 20 //按键慢收放线速度(没有设置顶点时只能按这个速度收线) #define SPEED_BTN_MID 100 //按键中等收放线速度 #define SPEED_BTN_FAST 200 //按键最快收放线速度 #define SPEED_HOOK_FAST SPEED_MAX //货物下放速度--有目标长度得情况下 #define SPEED_HOOK_SLOW 20 //货物快到地面速度--有目标长度得情况下 #define SPEED_HOOK_CHECK 50 //货物下放速度--没有目标长度得情况下能检测到脱钩的合适速度 #define SPEED_HOOK_UP SPEED_MAX //空钩上升速度 #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 1500 //等待脱钩时间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; float _goods_down_start_cnt; float _goods_down_target_cnt; float _goods_speed; control_status_t _controlstatus; bool _check_targetlength; ServoStatus _servotatus; unsigned long _tm_servotatus; int _pullweight; HookStatus _hooksstatus; HookStatus _hooksstatus_prv; 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() ; void set_hook_speed(float hooksspeed); public: Motocontrol(); // 构造函数 ~Motocontrol(); // 析构函数 void setspeed(float motospeed,float acctime=-1); // 设置速度 void stop(float acctime=0); // 停止 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);// 放下货物,自动回收 void moto_goodsdownresume(); // 恢复自动放线 HookStatus gethooktatus() { return _hooksstatus; } // 得到挂钩状态 void weightalign(bool weightalign); // 重量传感器是否校准--必须校准才能执行moto_hookdown };