aa22993490
【原 因】：1.指点模式 的mask参数 移位bug 2.修改挂在仓 轮子直径参数 轮子实际该外1.7cm参数设置位2cm 【过 程】： 【影 响】： # 类型 包含: # feat：新功能（feature） # fix：修补bug # docs：文档（documentation） # style： 格式（不影响代码运行的变动） # refactor：重构（即不是新增功能，也不是修改bug的代码变动） # test：增加测试 # chore：构建过程或辅助工具的变动
138 lines
6.1 KiB
C++
138 lines
6.1 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_DIAMETER 2.3 // 轮子直径cm 小轮子直径（实际2cm）
|
||
#define WHEEL_DIAMETER 2 // 轮子直径cm 小轮子直径(实际1.7cm)
|
||
#define WHEEL_PERIMETER (WHEEL_DIAMETER * 3.1416) // 轮子周长
|
||
#define ROPE_MAXCOUNT (ROPE_MAXLENGTH / WHEEL_PERIMETER) // 最大圈数
|
||
|
||
#define INSTORE_LENGTH_MIN_NONE 12.0f // 最小入仓长度,没有挂东西的情况，在最慢的速度下低于该长度也需要以SPEED_INSTORE缓慢入仓 cm -------加了软减速后必须根据速度改变！！！！
|
||
#define INSTORE_LENGTH_MIN_GOODS 10.0f // 最小入仓长度,有挂东西的情况 在最慢的速度下低于该长度也需要以SPEED_INSTORE缓慢入仓 cm -------加了软减速后必须根据速度改变！！！！
|
||
|
||
#define INSTORE_LENGTH_SPEED 0.05f // 0.0001f最小入仓长度速度平方的(输出轴速度)比例,越快越长，加了软减速后必须根据速度改变！！！！
|
||
|
||
#define SPEED_MAX 400 // 最快收放线速度--任何时候不应该超过这个速度def:350
|
||
#define SPEED_INSTORE 10 // 入仓速度,自动重量归零也是这个速度下降2cm
|
||
#define SPEED_BTN_SLOW 30 // 按键慢收放线速度(没有设置顶点时只能按这个速度收线)
|
||
#define SPEED_BTN_MID 100 // 按键中等收放线速度
|
||
#define SPEED_BTN_FAST 200 // 按键最快收放线速度
|
||
#define SPEED_HOOK_FAST SPEED_MAX // 货物下放速度--有目标长度得情况下
|
||
#define SPEED_HOOK_SLOW 100 // 货物快到地面速度--有目标长度得情况下
|
||
#define SPEED_HOOK_CHECK 100 // 货物下放速度--没有目标长度得情况下能检测到脱钩的合适速度
|
||
#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 1000 // 1920 // 舵机锁定位置
|
||
#define SERVO_UNLOCKPOS 1120 // 1800 // 舵机解锁位置
|
||
#define SERVO_BLOCKUNLOCKPOS 1100 // 舵机堵转需要转回的位置--可以和SERVO_UNLOCKPOS一样，为了速度快也可以更小
|
||
|
||
#define HOOK_WEIHT_MIN 300 // 最小货物重量 小于这个认为没挂东西 (克)
|
||
|
||
#define HOOK_WEIHT_MAX 12000 // 最大货物重量 大于这个认为超重不工作 (克)
|
||
|
||
#define HOOK_SLOWDOWN_LENGTH 50 // 下放物体时快到目标长度时慢速长度 (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; // 是否有货物
|
||
bool is_autogoodsdown; // 正在自动放线中
|
||
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;
|
||
|
||
uint8_t _unblocktimes;
|
||
unsigned long _tm_waitunhook;
|
||
float _runspeed;
|
||
|
||
void checkmotostatus();
|
||
void checkhookstatus();
|
||
void checkgoods();
|
||
bool checkservoblock();
|
||
void lockservo();
|
||
void unlockservo();
|
||
void set_hook_speed(float hooksspeed);
|
||
void sethooksstatus(HookStatus hookstatus);
|
||
|
||
public:
|
||
Motocontrol(); // 构造函数
|
||
~Motocontrol(); // 析构函数
|
||
void setspeed(float motospeed, float acctime = -1); // 设置速度
|
||
void stoprun(float acctime = 0); // 停止运行
|
||
void stopautodown(); // 停止自动下放模式
|
||
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; } // 得到挂钩状态
|
||
String gethooktatus_str(bool chstr = true); // 得到挂钩状态(中英文）
|
||
|
||
void weightalign(bool weightalign); // 重量传感器是否校准--必须校准才能执行moto_hookdown
|
||
};
|