From 5f414479f1bb18800f3fc929c342ffc0594fc58b Mon Sep 17 00:00:00 2001 From: pxzleo Date: Fri, 12 May 2023 00:12:07 +0800 Subject: [PATCH] =?UTF-8?q?=E5=8A=A0=E5=85=A5=E8=87=AA=E5=8A=A8=E6=94=BE?= =?UTF-8?q?=E7=BA=BF=E5=8F=AF=E6=9A=82=E5=81=9C=E5=92=8C=E6=81=A2=E5=A4=8D?= =?UTF-8?q?=E5=8A=9F=E8=83=BD=20=E5=BE=AE=E8=B0=83=E5=8F=82=E6=95=B0?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main.cpp | 22 +++++++++++++++++++++- src/motocontrol.cpp | 46 +++++++++++++++++++++++++++++++++++---------- src/motocontrol.h | 14 ++++++++------ 3 files changed, 65 insertions(+), 17 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index df4a1b7..da5aeaf 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -493,5 +493,25 @@ void upbtn_pressend() void testbtn_click() { Serial.println("testbtn_click"); - motocontrol.moto_goodsdown(22); //二楼340 //桌子40 + switch (motocontrol.gethooktatus()) + { + case HS_Locked: // 在顶部--会自动检测是否有物体，有才下放 + { + Serial.println("moto_goodsdown"); + motocontrol.moto_goodsdown(22); // 二楼340 //桌子40 + break; + } + case HS_Stop: + { + Serial.println("moto_resume"); + + motocontrol.moto_goodsdownresume(); + break; + } + default: + { + Serial.println("stop"); + motocontrol.stop(); + } + } } \ No newline at end of file diff --git a/src/motocontrol.cpp b/src/motocontrol.cpp index 15e8546..52a5d69 100644 --- a/src/motocontrol.cpp +++ b/src/motocontrol.cpp @@ -109,7 +109,11 @@ void Motocontrol::stop(float acctime) // 停止 _moto_dji.settime_acc(acctime); _moto_dji.setspeedtarget(0.0f); _controlstatus.motostatus = MS_Stop; - //_hooksstatus = HS_Stop; + if ((_hooksstatus != HS_UnInit) || (_hooksstatus != HS_Stop)) + { + _hooksstatus_prv = _hooksstatus; + _hooksstatus = HS_Stop; + } lockservo(); } void Motocontrol::setlocked(bool locked) @@ -168,8 +172,7 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态 { _hooksstatus = HS_DownSlow; _moto_dji.settime_acc(500); - _moto_dji.setspeedtarget(SPEED_HOOK_SLOW); - + set_hook_speed(SPEED_HOOK_SLOW); } break; } @@ -189,13 +192,10 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态 if ((millis() - _tm_waitunhook) > HOOK_UNHOOK_TIME) { printf("HS_WaitUnhook ok startup \n"); - _moto_dji.settime_acc(500); - _moto_dji.setspeedtarget(-SPEED_HOOK_UP); + _moto_dji.settime_acc(1000); + set_hook_speed(-SPEED_HOOK_UP); _hooksstatus = HS_Up; _controlstatus.motostatus = MS_Up; - // moto_run(MS_Stop); - // setspeed(SPEED_HOOK_UP); - // moto_run(MS_Up); } break; } @@ -311,10 +311,10 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候 if (!_controlstatus.is_instore) { _moto_dji.settime_acc(500); - _moto_dji.setspeedtarget(-SPEED_INSTORE); + set_hook_speed(-SPEED_INSTORE); _controlstatus.is_instore = true; _hooksstatus = HS_InStore; - printf("begin instore currlen:%.2f,instorelen:%.2f,speed:%.2f \n",_curr_length,instlen,mf.output_speed_rpm); + printf("begin instore currlen:%.2f,instorelen:%.2f,speed:%.2f \n", _curr_length, instlen, mf.output_speed_rpm); } } else @@ -341,6 +341,12 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候 } } +void Motocontrol::set_hook_speed(float hooksspeed) +{ + _goods_speed = hooksspeed; + _runspeed = hooksspeed; + _moto_dji.setspeedtarget(hooksspeed); +} void Motocontrol::update() // 更新 { _moto_dji.update(); @@ -361,6 +367,24 @@ control_status_t Motocontrol::getcontrolstatus() // 得到控制信息 return _controlstatus; } +void Motocontrol::moto_goodsdownresume() +{ + if (_hooksstatus == HS_Stop) + { + _runspeed = _goods_speed; + if (_hooksstatus_prv == HS_Up) + _controlstatus.motostatus = MS_Up; + if (_hooksstatus_prv == HS_InStore) + { + _controlstatus.motostatus = MS_Up; + _runspeed = -SPEED_INSTORE; + } + _hooksstatus = _hooksstatus_prv; + _moto_dji.settime_acc(1000); + unlockservo(); // 打开舵机 + // _moto_dji.setspeedtarget(_goods_speed); + } +} void Motocontrol::moto_goodsdown(float length) { if (length < 0.0) @@ -393,12 +417,14 @@ void Motocontrol::moto_goodsdown(float length) { _goods_down_target_cnt =0; setspeed(SPEED_HOOK_CHECK); + _goods_speed = SPEED_HOOK_CHECK; } else { _goods_down_start_cnt = _moto_dji.getmotoinfo().output_round_cnt; _goods_down_target_cnt = _goods_down_start_cnt + length / WHEEL_PERIMETER; setspeed(SPEED_HOOK_FAST, TM_ACC_HS); + _goods_speed = SPEED_HOOK_FAST; } moto_run(MS_Down); } diff --git a/src/motocontrol.h b/src/motocontrol.h index 1da6ab2..69ab4bf 100644 --- a/src/motocontrol.h +++ b/src/motocontrol.h @@ -12,7 +12,7 @@ #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.045f //0.0001f最小入仓长度速度平方的(输出轴速度)比例,越快越长，加了软减速后必须根据速度改变！！！！ +#define INSTORE_LENGTH_SPEED 0.05f // 0.0001f最小入仓长度速度平方的(输出轴速度)比例,越快越长，加了软减速后必须根据速度改变！！！！ @@ -44,7 +44,7 @@ #define HOOK_WEIHT_MAX 6000 //最大货物重量 大于这个认为超重不工作 (克) #define HOOK_SLOWDOWN_LENGTH 10 //下放物体时快到目标长度时慢速长度 (cm) -#define HOOK_UNHOOK_TIME 1000 //等待脱钩时间ms +#define HOOK_UNHOOK_TIME 1500 //等待脱钩时间ms // 挂钩状态 enum MotoStatus @@ -95,6 +95,7 @@ private: float _goods_down_start_cnt; float _goods_down_target_cnt; + float _goods_speed; control_status_t _controlstatus; bool _check_targetlength; @@ -102,7 +103,7 @@ private: unsigned long _tm_servotatus; int _pullweight; HookStatus _hooksstatus; - + HookStatus _hooksstatus_prv; int _hook_currlen; float _curr_length; bool _weightalign; @@ -121,6 +122,7 @@ private: bool checkservoblock(); void lockservo() ; void unlockservo() ; + void set_hook_speed(float hooksspeed); public: Motocontrol(); // 构造函数 ~Motocontrol(); // 析构函数 @@ -137,7 +139,7 @@ public: 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 - + void moto_goodsdownresume(); // 恢复自动放线 + HookStatus gethooktatus() { return _hooksstatus; } // 得到挂钩状态 + void weightalign(bool weightalign); // 重量传感器是否校准--必须校准才能执行moto_hookdown };