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

重构自动放货逻辑，直接控制电机，不通过moto_run(MS_Down);

Browse Source 
修改入仓中暂停再收速度不正确的bug
This commit is contained in:
pxzleo 2023-05-10 01:54:38 +08:00
parent c1dd4a6d49
commit ff434cc3df
4 changed files with 69 additions and 79 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/main.cpp
View File

@ -493,5 +493,5 @@ void upbtn_pressend()
void testbtn_click() void testbtn_click()
{ {
Serial.println("testbtn_click"); Serial.println("testbtn_click");
motocontrol.moto_goodsdown(40); //二楼340 //桌子40 motocontrol.moto_goodsdown(22); //二楼340 //桌子40
} }

2
src/moto.cpp
View File

@ -66,7 +66,7 @@ void moto::setspeedtarget(float new_target)
_start_speed = moto_chassis.speed_rpm; _start_speed = moto_chassis.speed_rpm;
_ds = moto_pid.target - _start_speed; // 速度差 _ds = moto_pid.target - _start_speed; // 速度差
_closed = false; _closed = false;
printf("speedtarget:tm:%d,target:%.2f\n", moto_chassis.starttime, moto_pid.target); printf("speedtarget:tm:%d,target:%.2f\n", moto_chassis.starttime, new_target);
} }
void moto::set_moto_current(int16_t iq1) void moto::set_moto_current(int16_t iq1)

104
src/motocontrol.cpp
View File

@ -38,7 +38,7 @@ bool Motocontrol::init(Servo *lockservo) // 初始化
return _moto_dji.init(); return _moto_dji.init();
} }
void Motocontrol::setspeed(float motospeed,float acctime) // 设置速度 void Motocontrol::setspeed(float motospeed, float acctime) // 设置速度
{ {
_controlstatus.speed_targe = motospeed; _controlstatus.speed_targe = motospeed;
_moto_dji.settime_acc(acctime); _moto_dji.settime_acc(acctime);
@ -98,7 +98,7 @@ void Motocontrol::unlockservo() // 解锁舵机
printf("unlockservo\n"); printf("unlockservo\n");
// 解锁操作 // 解锁操作
_lockservo->write(SERVO_UNLOCKPOS); _lockservo->write(SERVO_UNLOCKPOS);
_moto_dji.setspeedtarget(-1.0f);//解锁时逆向慢速转动 _moto_dji.setspeedtarget(-1.0f); // 解锁时逆向慢速转动
_servotatus = SS_WaitUnLock; _servotatus = SS_WaitUnLock;
_tm_servotatus = millis(); _tm_servotatus = millis();
_unblocktimes = 0; _unblocktimes = 0;
@ -109,7 +109,7 @@ void Motocontrol::stop(float acctime) // 停止
_moto_dji.settime_acc(acctime); _moto_dji.settime_acc(acctime);
_moto_dji.setspeedtarget(0.0f); _moto_dji.setspeedtarget(0.0f);
_controlstatus.motostatus = MS_Stop; _controlstatus.motostatus = MS_Stop;
_hooksstatus = HS_Stop; //_hooksstatus = HS_Stop;
lockservo(); lockservo();
} }
void Motocontrol::setlocked(bool locked) void Motocontrol::setlocked(bool locked)
@ -150,7 +150,7 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
// 需要在自动放货物中 // 需要在自动放货物中
if (!_autogoodsdown) if (!_autogoodsdown)
return; return;
moto_measure_t mf = _moto_dji.getmotoinfo();
_hook_currlen = _curr_length; _hook_currlen = _curr_length;
switch (_hooksstatus) switch (_hooksstatus)
{ {
@ -164,38 +164,16 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
goodsstartup(); goodsstartup();
break; break;
} }
if ((_goods_down_target_cnt!=0)&&(mf.output_round_cnt > _goods_down_target_cnt))
break;
}
case HookStatus::HS_Stop:
{ {
// 如果有长度目标
if ((_hook_targetlen > 0) && (_hook_targetlen <= HOOK_SLOWDOWN_LENGTH))
{
printf("_hook_targetlen>0 \n");
// 如果停了就开始慢速
{
printf("begin hook slow down \n");
// 慢速一直下放，直到货物卸下--没有重量
setspeed(SPEED_HOOK_SLOW);
moto_run(MS_Down);
_hooksstatus = HS_DownSlow; _hooksstatus = HS_DownSlow;
} _moto_dji.settime_acc(500);
} _moto_dji.setspeedtarget(SPEED_HOOK_SLOW);
else
{
// 没有长度目标,如果停了并且货物还在，一般是线到最大了，所有开始回收
{
printf("not settargetlen arrived wait unhook \n");
goodsstartup();
} }
}
break; break;
} }
//开始慢速下放
case HookStatus::HS_DownSlow: case HookStatus::HS_DownSlow:
{ {
if (!_controlstatus.is_havegoods) if (!_controlstatus.is_havegoods)
@ -205,25 +183,31 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
} }
break; break;
} }
//等待脱钩
case HookStatus::HS_WaitUnhook: case HookStatus::HS_WaitUnhook:
{ {
if ((millis() - _tm_waitunhook) > HOOK_UNHOOK_TIME) if ((millis() - _tm_waitunhook) > HOOK_UNHOOK_TIME)
{ {
printf("HS_WaitUnhook ok startup \n"); printf("HS_WaitUnhook ok startup \n");
moto_run(MS_Stop); _moto_dji.settime_acc(500);
setspeed(SPEED_HOOK_UP); _moto_dji.setspeedtarget(-SPEED_HOOK_UP);
moto_run(MS_Up); _hooksstatus = HS_Up;
_controlstatus.motostatus = MS_Up;
// moto_run(MS_Stop);
// setspeed(SPEED_HOOK_UP);
// moto_run(MS_Up);
} }
break; break;
} }
//开始收回
case HookStatus::HS_Up: case HookStatus::HS_Up:
{ {
// 会自动设置HS_InStore状态 // 会自动设置HS_Up状态
break; break;
} }
case HookStatus::HS_InStore: case HookStatus::HS_InStore:
{ {
// 会自动设置HS_InStore状态
// 如果到顶部，会触发setlocked,不用在这里设置 // 如果到顶部，会触发setlocked,不用在这里设置
break; break;
} }
@ -241,25 +225,25 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
// 停止状态---1防止舵机堵转，2延时关闭电机供电 // 停止状态---1防止舵机堵转，2延时关闭电机供电
switch (_servotatus) switch (_servotatus)
{ {
case SS_WaitUnLock: //舵机解锁 case SS_WaitUnLock: // 舵机解锁
{ {
if ((millis() - _tm_servotatus) > TM_SERVOLOCK) if ((millis() - _tm_servotatus) > TM_SERVOLOCK)
{ {
//入仓时会自动设置速度，原速度不可用
if (!_controlstatus.is_instore)
_moto_dji.setspeedtarget(_runspeed); _moto_dji.setspeedtarget(_runspeed);
_servotatus=SS_ServoUnLocked; _servotatus = SS_ServoUnLocked;
} }
break; break;
} }
// 等待电机停止
//等待电机停止
case SS_WaitMotoStop: case SS_WaitMotoStop:
{ {
if (abs(mf.speed_rpm)<3) if (abs(mf.speed_rpm) < 3)
{ {
_lockservo->write(SERVO_LOCKPOS); _lockservo->write(SERVO_LOCKPOS);
_servotatus=SS_WaitServo; _servotatus = SS_WaitServo;
_tm_servotatus = millis(); _tm_servotatus = millis();
} }
break; break;
@ -307,7 +291,6 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
} }
} }
// 已设置零点 // 已设置零点
if (_controlstatus.is_setzero) if (_controlstatus.is_setzero)
{ {
@ -317,14 +300,21 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
if ((_curr_length > ROPE_MAXLENGTH) && (_controlstatus.motostatus == MS_Down)) if ((_curr_length > ROPE_MAXLENGTH) && (_controlstatus.motostatus == MS_Down))
stop(); stop();
// 开始入仓 // 开始入仓
if ((_curr_length < (INSTORE_LENGTH_MIN+(mf.output_speed_rpm*mf.output_speed_rpm)*INSTORE_LENGTH_SPEED)) && (_controlstatus.motostatus == MotoStatus::MS_Up)) //if ((_curr_length < (INSTORE_LENGTH_MIN + (mf.output_speed_rpm * mf.output_speed_rpm) * INSTORE_LENGTH_SPEED)) && (_controlstatus.motostatus == MotoStatus::MS_Up))
float instlen;
if (_controlstatus.is_havegoods)
instlen=INSTORE_LENGTH_MIN_GOODS + abs(mf.output_speed_rpm)*INSTORE_LENGTH_SPEED;
else
instlen=INSTORE_LENGTH_MIN_NONE + abs(mf.output_speed_rpm)*INSTORE_LENGTH_SPEED;
if ((_curr_length < instlen) && (_controlstatus.motostatus == MotoStatus::MS_Up))
{ {
if (!_controlstatus.is_instore) if (!_controlstatus.is_instore)
{ {
_moto_dji.settime_acc(500);
_moto_dji.setspeedtarget(-SPEED_INSTORE); _moto_dji.setspeedtarget(-SPEED_INSTORE);
_controlstatus.is_instore = true; _controlstatus.is_instore = true;
_hooksstatus = HS_InStore; _hooksstatus = HS_InStore;
printf("begin instore \n"); printf("begin instore currlen:%.2f,instorelen:%.2f,speed:%.2f \n",_curr_length,instlen,mf.output_speed_rpm);
} }
} }
else else
@ -337,7 +327,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
if (mf.output_round_cnt > _target_cnt) if (mf.output_round_cnt > _target_cnt)
{ {
printf("stop--cnt:%.2f ,tar:%.2f\n", mf.output_round_cnt, _target_cnt); printf("stop--cnt:%.2f ,tar:%.2f\n", mf.output_round_cnt, _target_cnt);
stop(1000);//缓慢停止 stop(1000); // 缓慢停止
} }
} }
else if (_controlstatus.motostatus == MS_Up) else if (_controlstatus.motostatus == MS_Up)
@ -345,7 +335,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
if (mf.output_round_cnt < _target_cnt) if (mf.output_round_cnt < _target_cnt)
{ {
printf("cnt:%.2f ,tar:%.2f\n", mf.output_round_cnt, _target_cnt); printf("cnt:%.2f ,tar:%.2f\n", mf.output_round_cnt, _target_cnt);
stop(1000);//缓慢停止 stop(1000); // 缓慢停止
} }
} }
} }
@ -398,25 +388,19 @@ void Motocontrol::moto_goodsdown(float length)
// 开始自动放货物状态 // 开始自动放货物状态
_autogoodsdown = true; _autogoodsdown = true;
_hook_targetlen = length;
// 长度=0，直接中慢速下降， // 长度=0，直接中慢速下降，
if (length == 0.0) if (length == 0.0)
{
{ _goods_down_target_cnt =0;
setspeed(SPEED_HOOK_CHECK); setspeed(SPEED_HOOK_CHECK);
moto_run(MS_Down);
}
else if (length > HOOK_SLOWDOWN_LENGTH) // 长度大于慢速长度
{
setspeed(SPEED_HOOK_FAST,TM_ACC_HS);
moto_run(MS_Down, length - HOOK_SLOWDOWN_LENGTH);
_hook_targetlen = HOOK_SLOWDOWN_LENGTH;
} }
else else
{ {
setspeed(SPEED_HOOK_SLOW); // 长度小于慢速长度 _goods_down_start_cnt = _moto_dji.getmotoinfo().output_round_cnt;
moto_run(MS_Down, length); _goods_down_target_cnt = _goods_down_start_cnt + length / WHEEL_PERIMETER;
setspeed(SPEED_HOOK_FAST, TM_ACC_HS);
} }
moto_run(MS_Down);
} }
// 按指定速度收放线 放线为+，收线为- 单位cm // 按指定速度收放线 放线为+，收线为- 单位cm
@ -488,6 +472,6 @@ void Motocontrol::moto_run(MotoStatus updown, float length)
} }
// 开始转 // 开始转
printf("run speed:%.0f,tarcnt:%.2f\n", runspeed, _target_cnt); printf("run speed:%.0f,tarcnt:%.2f\n", runspeed, _target_cnt);
_runspeed=runspeed; _runspeed = runspeed;
// _moto_dji.setspeedtarget(runspeed); // _moto_dji.setspeedtarget(runspeed);
} }

18
src/motocontrol.h
View File

@ -5,20 +5,22 @@
#include <ESP32Servo.h> #include <ESP32Servo.h>
#define ROPE_MAXLENGTH 700 //最多能放700cm---实际绳子应该比这个长750之类的 #define ROPE_MAXLENGTH 700 //最多能放700cm---实际绳子应该比这个长750之类的
#define WHEEL_DIAMETER 4.0 //轮子直径cm #define WHEEL_DIAMETER 3.8 //轮子直径cm
#define WHEEL_PERIMETER (WHEEL_DIAMETER * 3.1416) //轮子周长 #define WHEEL_PERIMETER (WHEEL_DIAMETER * 3.1416) //轮子周长
#define ROPE_MAXCOUNT (ROPE_MAXLENGTH/WHEEL_PERIMETER) //最大圈数 #define ROPE_MAXCOUNT (ROPE_MAXLENGTH/WHEEL_PERIMETER) //最大圈数
#define INSTORE_LENGTH_MIN 8.0f //最小入仓长度,在最慢的速度下低于该长度也需要以SPEED_INSTORE缓慢入仓 cm -------加了软减速后必须根据速度改变！！！！ #define INSTORE_LENGTH_MIN_NONE 10.0f //最小入仓长度,没有挂东西的情况，在最慢的速度下低于该长度也需要以SPEED_INSTORE缓慢入仓 cm -------加了软减速后必须根据速度改变！！！！
#define INSTORE_LENGTH_SPEED 0.0002f //最小入仓长度速度平方的(输出轴速度)比例,越快越长，加了软减速后必须根据速度改变！！！！ #define INSTORE_LENGTH_MIN_GOODS 8.0f //最小入仓长度,有挂东西的情况 在最慢的速度下低于该长度也需要以SPEED_INSTORE缓慢入仓 cm -------加了软减速后必须根据速度改变！！！！
#define INSTORE_LENGTH_SPEED 0.045f //0.0001f最小入仓长度速度平方的(输出轴速度)比例,越快越长，加了软减速后必须根据速度改变！！！！
#define SPEED_MAX 330 //最快收放线速度--任何时候不应该超过这个速度 #define SPEED_MAX 350 //最快收放线速度--任何时候不应该超过这个速度
#define SPEED_INSTORE 5 //入仓速度,自动重量归零也是这个速度下降2cm #define SPEED_INSTORE 5 //入仓速度,自动重量归零也是这个速度下降2cm
#define SPEED_BTN_SLOW 20 //按键慢收放线速度(没有设置顶点时只能按这个速度收线) #define SPEED_BTN_SLOW 20 //按键慢收放线速度(没有设置顶点时只能按这个速度收线)
#define SPEED_BTN_MID 100 //按键中等收放线速度 #define SPEED_BTN_MID 100 //按键中等收放线速度
#define SPEED_BTN_FAST SPEED_MAX //按键最快收放线速度 #define SPEED_BTN_FAST 200 //按键最快收放线速度
#define SPEED_HOOK_FAST SPEED_MAX //货物下放速度--有目标长度得情况下 #define SPEED_HOOK_FAST SPEED_MAX //货物下放速度--有目标长度得情况下
#define SPEED_HOOK_SLOW 20 //货物快到地面速度--有目标长度得情况下 #define SPEED_HOOK_SLOW 20 //货物快到地面速度--有目标长度得情况下
#define SPEED_HOOK_CHECK 50 //货物下放速度--没有目标长度得情况下能检测到脱钩的合适速度 #define SPEED_HOOK_CHECK 50 //货物下放速度--没有目标长度得情况下能检测到脱钩的合适速度
@ -90,13 +92,17 @@ private:
moto _moto_dji; moto _moto_dji;
float _start_cnt; float _start_cnt;
float _target_cnt; float _target_cnt;
float _goods_down_start_cnt;
float _goods_down_target_cnt;
control_status_t _controlstatus; control_status_t _controlstatus;
bool _check_targetlength; bool _check_targetlength;
ServoStatus _servotatus; ServoStatus _servotatus;
unsigned long _tm_servotatus; unsigned long _tm_servotatus;
int _pullweight; int _pullweight;
HookStatus _hooksstatus; HookStatus _hooksstatus;
int _hook_targetlen;
int _hook_currlen; int _hook_currlen;
float _curr_length; float _curr_length;
bool _weightalign; bool _weightalign;