【类 型】：style
【原 因】： 【过 程】： 【影 响】：
This commit is contained in:
air
parent
c7a8907105
commit
9173d36cf5
@ -18,10 +18,10 @@ static const char *MOUDLENAME = "COMMSER";
|
||||
// char *password = "63587839ab"; // wifi密码
|
||||
// char* ssid = "szdot"; //wifi帐号
|
||||
// char* password = "63587839ab"; //wifi密码
|
||||
// char *ssid = "flicube"; // wifi帐号
|
||||
// char *password = "fxmf0622"; // wifi密码
|
||||
char *ssid = "fxmf_sc01"; // 4g wifi帐号
|
||||
char *password = "12345678"; // 4g wifi密码
|
||||
char *ssid = "flicube"; // wifi帐号
|
||||
char *password = "fxmf0622"; // wifi密码
|
||||
// char *ssid = "fxmf_sc01"; // 4g wifi帐号
|
||||
// char *password = "12345678"; // 4g wifi密码
|
||||
char *mqttServer = "szdot.top"; // mqtt地址
|
||||
int mqttPort = 1883; // mqtt端口
|
||||
char *mqttName = "admin"; // mqtt帐号
|
||||
|
||||
@ -6,7 +6,7 @@
|
||||
#include "config.h"
|
||||
|
||||
#define DEBUG_OUT false
|
||||
static const char* MOUDLENAME = "MOTO_C";
|
||||
static const char *MOUDLENAME = "MOTO_C";
|
||||
|
||||
Motocontrol::Motocontrol()
|
||||
{
|
||||
@ -23,7 +23,7 @@ Motocontrol::Motocontrol()
|
||||
_weightalign = false;
|
||||
_overweightcount = 0;
|
||||
_notweightcount = 0;
|
||||
_controlstatus.is_autogoodsdown=false;
|
||||
_controlstatus.is_autogoodsdown = false;
|
||||
_servotatus = SS_ServoUnLocked; // 停止状态，为了防止舵机堵转，和延时关闭电机需要一个停止状态机
|
||||
_unblocktimes = 0;
|
||||
}
|
||||
@ -51,55 +51,53 @@ void Motocontrol::setweight(int pullweight) // 设置重量
|
||||
_pullweight = pullweight;
|
||||
checkgoods();
|
||||
}
|
||||
String Motocontrol::gethooktatus_str(bool chstr)
|
||||
String Motocontrol::gethooktatus_str(bool chstr)
|
||||
{
|
||||
String hookstatusstr="未知";
|
||||
String hookstatusstr_en="unknown";
|
||||
switch (_hooksstatus)
|
||||
{
|
||||
String hookstatusstr = "未知";
|
||||
String hookstatusstr_en = "unknown";
|
||||
switch (_hooksstatus)
|
||||
{
|
||||
case HS_UnInit:
|
||||
hookstatusstr="未初始化";
|
||||
hookstatusstr_en="HS_UnInit";
|
||||
break;
|
||||
case HS_Down:
|
||||
hookstatusstr="放钩";
|
||||
hookstatusstr_en="HS_Down";
|
||||
break;
|
||||
hookstatusstr = "未初始化";
|
||||
hookstatusstr_en = "HS_UnInit";
|
||||
break;
|
||||
case HS_Down:
|
||||
hookstatusstr = "放钩";
|
||||
hookstatusstr_en = "HS_Down";
|
||||
break;
|
||||
case HS_DownSlow:
|
||||
hookstatusstr="慢速放钩";
|
||||
hookstatusstr_en="HS_DownSlow";
|
||||
break;
|
||||
hookstatusstr = "慢速放钩";
|
||||
hookstatusstr_en = "HS_DownSlow";
|
||||
break;
|
||||
case HS_WaitUnhook:
|
||||
hookstatusstr="等待脱钩";
|
||||
hookstatusstr_en="HS_WaitUnhook";
|
||||
break;
|
||||
hookstatusstr = "等待脱钩";
|
||||
hookstatusstr_en = "HS_WaitUnhook";
|
||||
break;
|
||||
case HS_Up:
|
||||
hookstatusstr="回收";
|
||||
hookstatusstr_en="HS_Up";
|
||||
break;
|
||||
hookstatusstr = "回收";
|
||||
hookstatusstr_en = "HS_Up";
|
||||
break;
|
||||
case HS_InStore:
|
||||
hookstatusstr="入仓中";
|
||||
hookstatusstr_en="HS_InStore";
|
||||
break;
|
||||
hookstatusstr = "入仓中";
|
||||
hookstatusstr_en = "HS_InStore";
|
||||
break;
|
||||
case HS_Locked:
|
||||
hookstatusstr="已锁定";
|
||||
hookstatusstr_en="HS_Locked";
|
||||
break;
|
||||
hookstatusstr = "已锁定";
|
||||
hookstatusstr_en = "HS_Locked";
|
||||
break;
|
||||
case HS_Stop:
|
||||
hookstatusstr="停止";
|
||||
hookstatusstr_en="HS_Stop";
|
||||
break;
|
||||
default:
|
||||
hookstatusstr="未知";
|
||||
hookstatusstr_en="HS_UnInit";
|
||||
break;
|
||||
}
|
||||
hookstatusstr = "停止";
|
||||
hookstatusstr_en = "HS_Stop";
|
||||
break;
|
||||
default:
|
||||
hookstatusstr = "未知";
|
||||
hookstatusstr_en = "HS_UnInit";
|
||||
break;
|
||||
}
|
||||
if (chstr)
|
||||
return hookstatusstr;
|
||||
else
|
||||
else
|
||||
return hookstatusstr_en;
|
||||
|
||||
|
||||
}
|
||||
|
||||
void Motocontrol::checkgoods() // 检测是否超重
|
||||
@ -141,13 +139,13 @@ void Motocontrol::checkgoods() // 检测是否超重
|
||||
}
|
||||
void Motocontrol::lockservo() // 锁定舵机
|
||||
{
|
||||
ESP_LOGD(MOUDLENAME,"start_lockservo");
|
||||
ESP_LOGD(MOUDLENAME, "start_lockservo");
|
||||
_servotatus = SS_WaitMotoStop;
|
||||
_tm_servotatus = millis();
|
||||
}
|
||||
void Motocontrol::unlockservo() // 解锁舵机
|
||||
{
|
||||
ESP_LOGD(MOUDLENAME,"unlockservo");
|
||||
ESP_LOGD(MOUDLENAME, "unlockservo");
|
||||
// 解锁操作
|
||||
_lockservo->write(SERVO_UNLOCKPOS);
|
||||
_moto_dji.setspeedtarget(-1.0f); // 解锁时逆向慢速转动
|
||||
@ -157,25 +155,25 @@ void Motocontrol::unlockservo() // 解锁舵机
|
||||
}
|
||||
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.setspeedtarget(0.0f);
|
||||
_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;
|
||||
sethooksstatus(HS_Stop);
|
||||
}
|
||||
lockservo();
|
||||
}
|
||||
void Motocontrol::stopautodown()
|
||||
void Motocontrol::stopautodown()
|
||||
{
|
||||
_controlstatus.is_autogoodsdown = false;
|
||||
}
|
||||
void Motocontrol::sethooksstatus(HookStatus hookstatus)
|
||||
{
|
||||
_hooksstatus=hookstatus;
|
||||
ESP_LOGD(MOUDLENAME,"Set HS:%s",gethooktatus_str(false));
|
||||
_hooksstatus = hookstatus;
|
||||
ESP_LOGD(MOUDLENAME, "Set HS:%s", gethooktatus_str(false));
|
||||
}
|
||||
void Motocontrol::setlocked(bool locked)
|
||||
{
|
||||
@ -199,7 +197,6 @@ int16_t Motocontrol::getlength() // 得到长度
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
||||
// 重量传感器已经校准
|
||||
void Motocontrol::weightalign(bool weightalign)
|
||||
{
|
||||
@ -220,14 +217,14 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
|
||||
// 如果没有货物--开始回收
|
||||
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);
|
||||
_tm_waitunhook = millis();
|
||||
break;
|
||||
}
|
||||
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);
|
||||
_moto_dji.settime_acc(500);
|
||||
set_hook_speed(SPEED_HOOK_SLOW);
|
||||
@ -239,7 +236,7 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
|
||||
{
|
||||
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);
|
||||
_tm_waitunhook = millis();
|
||||
}
|
||||
@ -250,7 +247,7 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
|
||||
{
|
||||
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);
|
||||
set_hook_speed(-SPEED_HOOK_UP);
|
||||
sethooksstatus(HS_Up);
|
||||
@ -316,14 +313,14 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
|
||||
// 检测堵转,没堵转直接关闭电机,否则打开舵机，让电机旋转一点再锁定--最多重复3次
|
||||
if (!checkservoblock() || (_unblocktimes > 2))
|
||||
{
|
||||
ESP_LOGD(MOUDLENAME,"Close moto");
|
||||
ESP_LOGD(MOUDLENAME, "Close moto");
|
||||
_moto_dji.close();
|
||||
_servotatus = SS_ServoLocked;
|
||||
}
|
||||
else
|
||||
// 堵转
|
||||
{
|
||||
ESP_LOGD(MOUDLENAME,"SS servoblock unlock servo and turn moto");
|
||||
ESP_LOGD(MOUDLENAME, "SS servoblock unlock servo and turn moto");
|
||||
_tm_servotatus = millis();
|
||||
_servotatus = SS_WaitUnBlock;
|
||||
// 写一个不会堵转的位置
|
||||
@ -339,7 +336,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
|
||||
{
|
||||
if ((millis() - _tm_servotatus) > TM_UNBLOCK)
|
||||
{
|
||||
ESP_LOGD(MOUDLENAME,"SS lock servo");
|
||||
ESP_LOGD(MOUDLENAME, "SS lock servo");
|
||||
// 继续锁定等待舵机检测
|
||||
_lockservo->write(SERVO_LOCKPOS);
|
||||
_servotatus = SS_WaitServo;
|
||||
@ -373,12 +370,12 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
|
||||
_moto_dji.settime_acc(500);
|
||||
set_hook_speed(-SPEED_INSTORE);
|
||||
_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
|
||||
{
|
||||
//已经在入仓了就不false了，因为instlen算法导致入仓后instlen还会变短进入这个分支
|
||||
// 已经在入仓了就不false了，因为instlen算法导致入仓后instlen还会变短进入这个分支
|
||||
if (_hooksstatus != HS_InStore)
|
||||
_controlstatus.is_instore = false;
|
||||
}
|
||||
@ -389,7 +386,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
|
||||
{
|
||||
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); // 缓慢停止
|
||||
}
|
||||
}
|
||||
@ -397,7 +394,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
|
||||
{
|
||||
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); // 缓慢停止
|
||||
}
|
||||
}
|
||||
@ -432,7 +429,8 @@ control_status_t Motocontrol::getcontrolstatus() // 得到控制信息
|
||||
|
||||
void Motocontrol::moto_goodsdownresume()
|
||||
{
|
||||
if (!_controlstatus.is_autogoodsdown) return;
|
||||
if (!_controlstatus.is_autogoodsdown)
|
||||
return;
|
||||
if (_hooksstatus == HS_Stop)
|
||||
{
|
||||
_runspeed = _goods_speed;
|
||||
@ -449,29 +447,29 @@ void Motocontrol::moto_goodsdownresume()
|
||||
// _moto_dji.setspeedtarget(_goods_speed);
|
||||
}
|
||||
}
|
||||
//长度cm
|
||||
// 长度cm
|
||||
void Motocontrol::moto_goodsdown(float length)
|
||||
{
|
||||
if (length < 0.0)
|
||||
{
|
||||
ESP_LOGD(MOUDLENAME,"length<0 fault");
|
||||
ESP_LOGD(MOUDLENAME, "length<0 fault");
|
||||
return;
|
||||
}
|
||||
// 没设置零点
|
||||
if (!_controlstatus.is_setzero)
|
||||
{
|
||||
ESP_LOGD(MOUDLENAME,"not lengthzero fault");
|
||||
ESP_LOGD(MOUDLENAME, "not lengthzero fault");
|
||||
return;
|
||||
}
|
||||
if (!_weightalign)
|
||||
{
|
||||
ESP_LOGD(MOUDLENAME,"weight not align fault");
|
||||
ESP_LOGD(MOUDLENAME, "weight not align fault");
|
||||
return;
|
||||
}
|
||||
// 没挂东西
|
||||
if (!_controlstatus.is_havegoods)
|
||||
{
|
||||
ESP_LOGD(MOUDLENAME,"goods min fault:%d", _pullweight);
|
||||
ESP_LOGD(MOUDLENAME, "goods min fault:%d", _pullweight);
|
||||
return;
|
||||
}
|
||||
|
||||
@ -487,12 +485,12 @@ void Motocontrol::moto_goodsdown(float length)
|
||||
}
|
||||
else
|
||||
{
|
||||
float tarleng=length;
|
||||
if (length>HOOK_SLOWDOWN_LENGTH)
|
||||
tarleng-=HOOK_SLOWDOWN_LENGTH;
|
||||
float tarleng = length;
|
||||
if (length > HOOK_SLOWDOWN_LENGTH)
|
||||
tarleng -= HOOK_SLOWDOWN_LENGTH;
|
||||
_goods_down_start_cnt = _moto_dji.getmotoinfo().output_round_cnt;
|
||||
_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);
|
||||
_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))
|
||||
{
|
||||
ESP_LOGD(MOUDLENAME,"motostatus is MS_Down\\MS_Up");
|
||||
ESP_LOGD(MOUDLENAME, "motostatus is MS_Down\\MS_Up");
|
||||
return;
|
||||
}
|
||||
// 没设置速度不转
|
||||
if (_controlstatus.speed_targe == 0)
|
||||
{
|
||||
ESP_LOGD(MOUDLENAME,"not set speed_targe");
|
||||
ESP_LOGD(MOUDLENAME, "not set speed_targe");
|
||||
return;
|
||||
}
|
||||
if (updown == MS_Up)
|
||||
{
|
||||
if (_controlstatus.is_toplocked)
|
||||
{
|
||||
ESP_LOGD(MOUDLENAME,"is_toplocked return");
|
||||
ESP_LOGD(MOUDLENAME, "is_toplocked return");
|
||||
return;
|
||||
}
|
||||
if (_controlstatus.is_overweight)
|
||||
{
|
||||
ESP_LOGD(MOUDLENAME,"overweight fault :%d", _pullweight);
|
||||
ESP_LOGD(MOUDLENAME, "overweight fault :%d", _pullweight);
|
||||
return;
|
||||
}
|
||||
sethooksstatus(HS_Up);
|
||||
@ -567,7 +565,7 @@ void Motocontrol::moto_run(MotoStatus updown, float length)
|
||||
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;
|
||||
// _moto_dji.setspeedtarget(runspeed);
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user