diff --git a/src/commser.cpp b/src/commser.cpp index 0fb2a59..f576d90 100644 --- a/src/commser.cpp +++ b/src/commser.cpp @@ -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帐号 diff --git a/src/motocontrol.cpp b/src/motocontrol.cpp index 6ae3d2f..57f07fa 100644 --- a/src/motocontrol.cpp +++ b/src/motocontrol.cpp @@ -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); } \ No newline at end of file