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()
{
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;
_ds = moto_pid.target - _start_speed; // 速度差
_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)

104
src/motocontrol.cpp
View File

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

18
src/motocontrol.h
View File

@ -5,20 +5,22 @@
#include <ESP32Servo.h>
#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 ROPE_MAXCOUNT (ROPE_MAXLENGTH/WHEEL_PERIMETER) //最大圈数
#define INSTORE_LENGTH_MIN 8.0f //最小入仓长度,在最慢的速度下低于该长度也需要以SPEED_INSTORE缓慢入仓 cm -------加了软减速后必须根据速度改变！！！！
#define INSTORE_LENGTH_SPEED 0.0002f //最小入仓长度速度平方的(输出轴速度)比例,越快越长，加了软减速后必须根据速度改变！！！！
#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 SPEED_MAX 330 //最快收放线速度--任何时候不应该超过这个速度
#define SPEED_MAX 350 //最快收放线速度--任何时候不应该超过这个速度
#define SPEED_INSTORE 5 //入仓速度,自动重量归零也是这个速度下降2cm
#define SPEED_BTN_SLOW 20 //按键慢收放线速度(没有设置顶点时只能按这个速度收线)
#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_SLOW 20 //货物快到地面速度--有目标长度得情况下
#define SPEED_HOOK_CHECK 50 //货物下放速度--没有目标长度得情况下能检测到脱钩的合适速度
@ -90,13 +92,17 @@ private:
moto _moto_dji;
float _start_cnt;
float _target_cnt;
float _goods_down_start_cnt;
float _goods_down_target_cnt;
control_status_t _controlstatus;
bool _check_targetlength;
ServoStatus _servotatus;
unsigned long _tm_servotatus;
int _pullweight;
HookStatus _hooksstatus;
int _hook_targetlen;
int _hook_currlen;
float _curr_length;
bool _weightalign;