Food/PullupDev
3
0
Fork 0
Code Issues 8 Pull Requests 1 Actions Packages Projects Releases Wiki Activity

加入自动放线可暂停和恢复功能

Browse Source 
微调参数
This commit is contained in:
pxzleo 2023-05-12 00:12:07 +08:00
parent ff434cc3df
commit 5f414479f1
3 changed files with 65 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
src/main.cpp
View File

@ -493,5 +493,25 @@ void upbtn_pressend()
void testbtn_click()
{
Serial.println("testbtn_click");
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();
}
}
}

44
src/motocontrol.cpp
View File

@ -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,7 +311,7 @@ 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);
@ -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);
}

10
src/motocontrol.h
View File

@ -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);// 放下货物,自动回收
void moto_goodsdownresume(); // 恢复自动放线
HookStatus gethooktatus() { return _hooksstatus; } // 得到挂钩状态
void weightalign(bool weightalign); // 重量传感器是否校准--必须校准才能执行moto_hookdown
};