146 lines
5.4 KiB
C++
146 lines
5.4 KiB
C++
#pragma once
|
||
#include "Arduino.h"
|
||
#include "config.h"
|
||
#include "moto.h"
|
||
#include <ESP32Servo.h>
|
||
|
||
#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
|
||
};
|