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

【类 型】：style

Browse Source 
【原  因】：
【过  程】：
【影  响】：
This commit is contained in:
air 2025-03-20 11:16:32 +08:00
parent c7a8907105
commit 9173d36cf5
2 changed files with 76 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
src/commser.cpp
View File

@ -18,10 +18,10 @@ static const char *MOUDLENAME = "COMMSER";
// char *password = "63587839ab"; // wifi密码 // char *password = "63587839ab"; // wifi密码
// char* ssid = "szdot"; //wifi帐号 // char* ssid = "szdot"; //wifi帐号
// char* password = "63587839ab"; //wifi密码 // char* password = "63587839ab"; //wifi密码
// char *ssid = "flicube"; // wifi帐号 char *ssid = "flicube"; // wifi帐号
// char *password = "fxmf0622"; // wifi密码 char *password = "fxmf0622"; // wifi密码
char *ssid = "fxmf_sc01"; // 4g wifi帐号 // char *ssid = "fxmf_sc01"; // 4g wifi帐号
char *password = "12345678"; // 4g wifi密码 // char *password = "12345678"; // 4g wifi密码
char *mqttServer = "szdot.top"; // mqtt地址 char *mqttServer = "szdot.top"; // mqtt地址
int mqttPort = 1883; // mqtt端口 int mqttPort = 1883; // mqtt端口
char *mqttName = "admin"; // mqtt帐号 char *mqttName = "admin"; // mqtt帐号

142
src/motocontrol.cpp
View File

@ -6,7 +6,7 @@
#include "config.h" #include "config.h"
#define DEBUG_OUT false #define DEBUG_OUT false
static const char* MOUDLENAME = "MOTO_C"; static const char *MOUDLENAME = "MOTO_C";
Motocontrol::Motocontrol() Motocontrol::Motocontrol()
{ {
@ -23,7 +23,7 @@ Motocontrol::Motocontrol()
_weightalign = false; _weightalign = false;
_overweightcount = 0; _overweightcount = 0;
_notweightcount = 0; _notweightcount = 0;
_controlstatus.is_autogoodsdown=false; _controlstatus.is_autogoodsdown = false;
_servotatus = SS_ServoUnLocked; // 停止状态，为了防止舵机堵转，和延时关闭电机需要一个停止状态机 _servotatus = SS_ServoUnLocked; // 停止状态，为了防止舵机堵转，和延时关闭电机需要一个停止状态机
_unblocktimes = 0; _unblocktimes = 0;
} }
@ -51,55 +51,53 @@ void Motocontrol::setweight(int pullweight) // 设置重量
_pullweight = pullweight; _pullweight = pullweight;
checkgoods(); checkgoods();
} }
String Motocontrol::gethooktatus_str(bool chstr) String Motocontrol::gethooktatus_str(bool chstr)
{ {
String hookstatusstr="未知"; String hookstatusstr = "未知";
String hookstatusstr_en="unknown"; String hookstatusstr_en = "unknown";
switch (_hooksstatus) switch (_hooksstatus)
{ {
case HS_UnInit: case HS_UnInit:
hookstatusstr="未初始化"; hookstatusstr = "未初始化";
hookstatusstr_en="HS_UnInit"; hookstatusstr_en = "HS_UnInit";
break; break;
case HS_Down: case HS_Down:
hookstatusstr="放钩"; hookstatusstr = "放钩";
hookstatusstr_en="HS_Down"; hookstatusstr_en = "HS_Down";
break; break;
case HS_DownSlow: case HS_DownSlow:
hookstatusstr="慢速放钩"; hookstatusstr = "慢速放钩";
hookstatusstr_en="HS_DownSlow"; hookstatusstr_en = "HS_DownSlow";
break; break;
case HS_WaitUnhook: case HS_WaitUnhook:
hookstatusstr="等待脱钩"; hookstatusstr = "等待脱钩";
hookstatusstr_en="HS_WaitUnhook"; hookstatusstr_en = "HS_WaitUnhook";
break; break;
case HS_Up: case HS_Up:
hookstatusstr="回收"; hookstatusstr = "回收";
hookstatusstr_en="HS_Up"; hookstatusstr_en = "HS_Up";
break; break;
case HS_InStore: case HS_InStore:
hookstatusstr="入仓中"; hookstatusstr = "入仓中";
hookstatusstr_en="HS_InStore"; hookstatusstr_en = "HS_InStore";
break; break;
case HS_Locked: case HS_Locked:
hookstatusstr="已锁定"; hookstatusstr = "已锁定";
hookstatusstr_en="HS_Locked"; hookstatusstr_en = "HS_Locked";
break; break;
case HS_Stop: case HS_Stop:
hookstatusstr="停止"; hookstatusstr = "停止";
hookstatusstr_en="HS_Stop"; hookstatusstr_en = "HS_Stop";
break; break;
default: default:
hookstatusstr="未知"; hookstatusstr = "未知";
hookstatusstr_en="HS_UnInit"; hookstatusstr_en = "HS_UnInit";
break; break;
} }
if (chstr) if (chstr)
return hookstatusstr; return hookstatusstr;
else else
return hookstatusstr_en; return hookstatusstr_en;
} }
void Motocontrol::checkgoods() // 检测是否超重 void Motocontrol::checkgoods() // 检测是否超重
@ -141,13 +139,13 @@ void Motocontrol::checkgoods() // 检测是否超重
} }
void Motocontrol::lockservo() // 锁定舵机 void Motocontrol::lockservo() // 锁定舵机
{ {
ESP_LOGD(MOUDLENAME,"start_lockservo"); ESP_LOGD(MOUDLENAME, "start_lockservo");
_servotatus = SS_WaitMotoStop; _servotatus = SS_WaitMotoStop;
_tm_servotatus = millis(); _tm_servotatus = millis();
} }
void Motocontrol::unlockservo() // 解锁舵机 void Motocontrol::unlockservo() // 解锁舵机
{ {
ESP_LOGD(MOUDLENAME,"unlockservo"); ESP_LOGD(MOUDLENAME, "unlockservo");
// 解锁操作 // 解锁操作
_lockservo->write(SERVO_UNLOCKPOS); _lockservo->write(SERVO_UNLOCKPOS);
_moto_dji.setspeedtarget(-1.0f); // 解锁时逆向慢速转动 _moto_dji.setspeedtarget(-1.0f); // 解锁时逆向慢速转动
@ -157,11 +155,11 @@ void Motocontrol::unlockservo() // 解锁舵机
} }
void Motocontrol::stoprun(float acctime) // 停止 void Motocontrol::stoprun(float acctime) // 停止
{ {
ESP_LOGD(MOUDLENAME,"stoprun after time:%.2f",acctime); ESP_LOGD(MOUDLENAME, "stoprun after time:%.2f", 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;
if (((_hooksstatus != HS_UnInit) || (_hooksstatus != HS_Stop)) &&(_hooksstatus != HS_Locked) ) if (((_hooksstatus != HS_UnInit) || (_hooksstatus != HS_Stop)) && (_hooksstatus != HS_Locked))
{ {
_hooksstatus_prv = _hooksstatus; _hooksstatus_prv = _hooksstatus;
sethooksstatus(HS_Stop); sethooksstatus(HS_Stop);
@ -174,8 +172,8 @@ void Motocontrol::stopautodown()
} }
void Motocontrol::sethooksstatus(HookStatus hookstatus) void Motocontrol::sethooksstatus(HookStatus hookstatus)
{ {
_hooksstatus=hookstatus; _hooksstatus = hookstatus;
ESP_LOGD(MOUDLENAME,"Set HS:%s",gethooktatus_str(false)); ESP_LOGD(MOUDLENAME, "Set HS:%s", gethooktatus_str(false));
} }
void Motocontrol::setlocked(bool locked) void Motocontrol::setlocked(bool locked)
{ {
@ -199,7 +197,6 @@ int16_t Motocontrol::getlength() // 得到长度
return 0; return 0;
} }
// 重量传感器已经校准 // 重量传感器已经校准
void Motocontrol::weightalign(bool weightalign) void Motocontrol::weightalign(bool weightalign)
{ {
@ -220,14 +217,14 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
// 如果没有货物--开始回收 // 如果没有货物--开始回收
if (!_controlstatus.is_havegoods) if (!_controlstatus.is_havegoods)
{ {
ESP_LOGD(MOUDLENAME,"Not goods wait %d ms for unhook",HOOK_UNHOOK_TIME); ESP_LOGD(MOUDLENAME, "Not goods wait %d ms for unhook", HOOK_UNHOOK_TIME);
sethooksstatus(HS_WaitUnhook); sethooksstatus(HS_WaitUnhook);
_tm_waitunhook = millis(); _tm_waitunhook = millis();
break; break;
} }
if ((_goods_down_target_cnt != 0) && (mf.output_round_cnt > _goods_down_target_cnt)) if ((_goods_down_target_cnt != 0) && (mf.output_round_cnt > _goods_down_target_cnt))
{ {
ESP_LOGD(MOUDLENAME,"HS_Down target is arrived curr: %.2f",mf.output_round_cnt); ESP_LOGD(MOUDLENAME, "HS_Down target is arrived curr: %.2f", mf.output_round_cnt);
sethooksstatus(HS_DownSlow); sethooksstatus(HS_DownSlow);
_moto_dji.settime_acc(500); _moto_dji.settime_acc(500);
set_hook_speed(SPEED_HOOK_SLOW); set_hook_speed(SPEED_HOOK_SLOW);
@ -239,7 +236,7 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
{ {
if (!_controlstatus.is_havegoods) if (!_controlstatus.is_havegoods)
{ {
ESP_LOGD(MOUDLENAME,"Not havegoods wait %d ms unhook",HOOK_UNHOOK_TIME); ESP_LOGD(MOUDLENAME, "Not havegoods wait %d ms unhook", HOOK_UNHOOK_TIME);
sethooksstatus(HS_WaitUnhook); sethooksstatus(HS_WaitUnhook);
_tm_waitunhook = millis(); _tm_waitunhook = millis();
} }
@ -250,7 +247,7 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
{ {
if ((millis() - _tm_waitunhook) > HOOK_UNHOOK_TIME) if ((millis() - _tm_waitunhook) > HOOK_UNHOOK_TIME)
{ {
ESP_LOGD(MOUDLENAME,"HS_WaitUnhook ok startup"); ESP_LOGD(MOUDLENAME, "HS_WaitUnhook ok startup");
_moto_dji.settime_acc(1000); _moto_dji.settime_acc(1000);
set_hook_speed(-SPEED_HOOK_UP); set_hook_speed(-SPEED_HOOK_UP);
sethooksstatus(HS_Up); sethooksstatus(HS_Up);
@ -316,14 +313,14 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
// 检测堵转,没堵转直接关闭电机,否则打开舵机，让电机旋转一点再锁定--最多重复3次 // 检测堵转,没堵转直接关闭电机,否则打开舵机，让电机旋转一点再锁定--最多重复3次
if (!checkservoblock() || (_unblocktimes > 2)) if (!checkservoblock() || (_unblocktimes > 2))
{ {
ESP_LOGD(MOUDLENAME,"Close moto"); ESP_LOGD(MOUDLENAME, "Close moto");
_moto_dji.close(); _moto_dji.close();
_servotatus = SS_ServoLocked; _servotatus = SS_ServoLocked;
} }
else else
// 堵转 // 堵转
{ {
ESP_LOGD(MOUDLENAME,"SS servoblock unlock servo and turn moto"); ESP_LOGD(MOUDLENAME, "SS servoblock unlock servo and turn moto");
_tm_servotatus = millis(); _tm_servotatus = millis();
_servotatus = SS_WaitUnBlock; _servotatus = SS_WaitUnBlock;
// 写一个不会堵转的位置 // 写一个不会堵转的位置
@ -339,7 +336,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
{ {
if ((millis() - _tm_servotatus) > TM_UNBLOCK) if ((millis() - _tm_servotatus) > TM_UNBLOCK)
{ {
ESP_LOGD(MOUDLENAME,"SS lock servo"); ESP_LOGD(MOUDLENAME, "SS lock servo");
// 继续锁定等待舵机检测 // 继续锁定等待舵机检测
_lockservo->write(SERVO_LOCKPOS); _lockservo->write(SERVO_LOCKPOS);
_servotatus = SS_WaitServo; _servotatus = SS_WaitServo;
@ -373,12 +370,12 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
_moto_dji.settime_acc(500); _moto_dji.settime_acc(500);
set_hook_speed(-SPEED_INSTORE); set_hook_speed(-SPEED_INSTORE);
_controlstatus.is_instore = true; _controlstatus.is_instore = true;
ESP_LOGD(MOUDLENAME,"Instorelen:%.2f,speed:%.2f", instlen, mf.output_speed_rpm); ESP_LOGD(MOUDLENAME, "Instorelen:%.2f,speed:%.2f", instlen, mf.output_speed_rpm);
} }
} }
else else
{ {
//已经在入仓了就不false了，因为instlen算法导致入仓后instlen还会变短进入这个分支 // 已经在入仓了就不false了，因为instlen算法导致入仓后instlen还会变短进入这个分支
if (_hooksstatus != HS_InStore) if (_hooksstatus != HS_InStore)
_controlstatus.is_instore = false; _controlstatus.is_instore = false;
} }
@ -389,7 +386,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
{ {
if (mf.output_round_cnt > _target_cnt) if (mf.output_round_cnt > _target_cnt)
{ {
ESP_LOGD(MOUDLENAME,"stop down tar:%.2f", mf.output_round_cnt, _target_cnt); ESP_LOGD(MOUDLENAME, "stop down tar:%.2f", mf.output_round_cnt, _target_cnt);
stoprun(1000); // 缓慢停止 stoprun(1000); // 缓慢停止
} }
} }
@ -397,7 +394,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
{ {
if (mf.output_round_cnt < _target_cnt) if (mf.output_round_cnt < _target_cnt)
{ {
ESP_LOGD(MOUDLENAME,"stop up tar:%.2f", mf.output_round_cnt, _target_cnt); ESP_LOGD(MOUDLENAME, "stop up tar:%.2f", mf.output_round_cnt, _target_cnt);
stoprun(1000); // 缓慢停止 stoprun(1000); // 缓慢停止
} }
} }
@ -432,7 +429,8 @@ control_status_t Motocontrol::getcontrolstatus() // 得到控制信息
void Motocontrol::moto_goodsdownresume() void Motocontrol::moto_goodsdownresume()
{ {
if (!_controlstatus.is_autogoodsdown) return; if (!_controlstatus.is_autogoodsdown)
return;
if (_hooksstatus == HS_Stop) if (_hooksstatus == HS_Stop)
{ {
_runspeed = _goods_speed; _runspeed = _goods_speed;
@ -449,29 +447,29 @@ void Motocontrol::moto_goodsdownresume()
// _moto_dji.setspeedtarget(_goods_speed); // _moto_dji.setspeedtarget(_goods_speed);
} }
} }
//长度cm // 长度cm
void Motocontrol::moto_goodsdown(float length) void Motocontrol::moto_goodsdown(float length)
{ {
if (length < 0.0) if (length < 0.0)
{ {
ESP_LOGD(MOUDLENAME,"length<0 fault"); ESP_LOGD(MOUDLENAME, "length<0 fault");
return; return;
} }
// 没设置零点 // 没设置零点
if (!_controlstatus.is_setzero) if (!_controlstatus.is_setzero)
{ {
ESP_LOGD(MOUDLENAME,"not lengthzero fault"); ESP_LOGD(MOUDLENAME, "not lengthzero fault");
return; return;
} }
if (!_weightalign) if (!_weightalign)
{ {
ESP_LOGD(MOUDLENAME,"weight not align fault"); ESP_LOGD(MOUDLENAME, "weight not align fault");
return; return;
} }
// 没挂东西 // 没挂东西
if (!_controlstatus.is_havegoods) if (!_controlstatus.is_havegoods)
{ {
ESP_LOGD(MOUDLENAME,"goods min fault:%d", _pullweight); ESP_LOGD(MOUDLENAME, "goods min fault:%d", _pullweight);
return; return;
} }
@ -487,12 +485,12 @@ void Motocontrol::moto_goodsdown(float length)
} }
else else
{ {
float tarleng=length; float tarleng = length;
if (length>HOOK_SLOWDOWN_LENGTH) if (length > HOOK_SLOWDOWN_LENGTH)
tarleng-=HOOK_SLOWDOWN_LENGTH; tarleng -= HOOK_SLOWDOWN_LENGTH;
_goods_down_start_cnt = _moto_dji.getmotoinfo().output_round_cnt; _goods_down_start_cnt = _moto_dji.getmotoinfo().output_round_cnt;
_goods_down_target_cnt = _goods_down_start_cnt + tarleng / WHEEL_PERIMETER; _goods_down_target_cnt = _goods_down_start_cnt + tarleng / WHEEL_PERIMETER;
ESP_LOGD(MOUDLENAME,"start down %.2f cm,tar:%.2f",tarleng,_goods_down_target_cnt); ESP_LOGD(MOUDLENAME, "start down %.2f cm,tar:%.2f", tarleng, _goods_down_target_cnt);
setspeed(SPEED_HOOK_FAST, TM_ACC_HS); setspeed(SPEED_HOOK_FAST, TM_ACC_HS);
_goods_speed = SPEED_HOOK_FAST; _goods_speed = SPEED_HOOK_FAST;
} }
@ -512,25 +510,25 @@ void Motocontrol::moto_run(MotoStatus updown, float length)
// 运动中暂时不管 // 运动中暂时不管
if ((_controlstatus.motostatus == MS_Down) || (_controlstatus.motostatus == MS_Up)) if ((_controlstatus.motostatus == MS_Down) || (_controlstatus.motostatus == MS_Up))
{ {
ESP_LOGD(MOUDLENAME,"motostatus is MS_Down\\MS_Up"); ESP_LOGD(MOUDLENAME, "motostatus is MS_Down\\MS_Up");
return; return;
} }
// 没设置速度不转 // 没设置速度不转
if (_controlstatus.speed_targe == 0) if (_controlstatus.speed_targe == 0)
{ {
ESP_LOGD(MOUDLENAME,"not set speed_targe"); ESP_LOGD(MOUDLENAME, "not set speed_targe");
return; return;
} }
if (updown == MS_Up) if (updown == MS_Up)
{ {
if (_controlstatus.is_toplocked) if (_controlstatus.is_toplocked)
{ {
ESP_LOGD(MOUDLENAME,"is_toplocked return"); ESP_LOGD(MOUDLENAME, "is_toplocked return");
return; return;
} }
if (_controlstatus.is_overweight) if (_controlstatus.is_overweight)
{ {
ESP_LOGD(MOUDLENAME,"overweight fault :%d", _pullweight); ESP_LOGD(MOUDLENAME, "overweight fault :%d", _pullweight);
return; return;
} }
sethooksstatus(HS_Up); sethooksstatus(HS_Up);
@ -567,7 +565,7 @@ void Motocontrol::moto_run(MotoStatus updown, float length)
runspeed = -_controlstatus.speed_targe; runspeed = -_controlstatus.speed_targe;
} }
// 开始转 // 开始转
// ESP_LOGD(MOUDLENAME,"run speed:%.0f,tarcnt:%.2f", runspeed, _target_cnt); // ESP_LOGD(MOUDLENAME,"run speed:%.0f,tarcnt:%.2f", runspeed, _target_cnt);
_runspeed = runspeed; _runspeed = runspeed;
// _moto_dji.setspeedtarget(runspeed); // _moto_dji.setspeedtarget(runspeed);
} }