From 9f3f25acc209f5dcfe940289d8981920f67c28ef Mon Sep 17 00:00:00 2001 From: air <30444667+sszdot@users.noreply.github.com> Date: Wed, 14 May 2025 14:31:36 +0800 Subject: [PATCH] =?UTF-8?q?=E3=80=90=E7=B1=BB=20=20=E5=9E=8B=E3=80=91?= =?UTF-8?q?=EF=BC=9Afactor=20=E3=80=90=E5=8E=9F=20=20=E5=9B=A0=E3=80=91?= =?UTF-8?q?=EF=BC=9A=E4=B8=B2=E5=8F=A3=E6=89=93=E5=8D=B0=E7=9A=84=E6=97=A5?= =?UTF-8?q?=E5=BF=97=20=E9=83=BD=E5=8F=91=E9=80=81=E5=88=B0=E5=89=8D?= =?UTF-8?q?=E7=AB=AF=E6=97=A5=E5=BF=97=20=E3=80=90=E8=BF=87=20=20=E7=A8=8B?= =?UTF-8?q?=E3=80=91=EF=BC=9A=20=E3=80=90=E5=BD=B1=20=20=E5=93=8D=E3=80=91?= =?UTF-8?q?=EF=BC=9A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit # 类型 包含: # feat：新功能（feature） # fix：修补bug # docs：文档（documentation） # style： 格式（不影响代码运行的变动） # refactor：重构（即不是新增功能，也不是修改bug的代码变动） # test：增加测试 # chore：构建过程或辅助工具的变动 --- src/commser.cpp | 64 ++++++++++++++++++++++-------- src/commser.h | 1 + src/main.cpp | 95 +++++++++++++++++++++++++++++---------------- src/moto.cpp | 15 ++++--- src/motocontrol.cpp | 70 ++++++++++++++++++++++----------- 5 files changed, 166 insertions(+), 79 deletions(-) diff --git a/src/commser.cpp b/src/commser.cpp index e7043fb..3e4c396 100644 --- a/src/commser.cpp +++ b/src/commser.cpp @@ -49,10 +49,14 @@ const String topicPub[] = {"heartBeat", "voltagBattery", "currentBattery", "batt */ const int topicPubCount = sizeof(topicPub) / sizeof(topicPub[0]); // 登记 json成员总数 String topicPubMsg[topicPubCount] = {""}; // 登记 json成员的值 对应topicPub +/*日志队列*/ +#define LOG_QUEUE_SIZE 50 // 日志队列大小 +String logQueue[LOG_QUEUE_SIZE]; +int logHead = 0; +int logTail = 0; /*触发发送 主题*/ // 0:对频信息 String topicHandle[] = {"crosFrequency"}; -boolean isPush = false; // 记得删除 板子按钮状态 ps:D3引脚下拉微动开关 /*异步线程对象*/ Ticker pubTicker; // 定时发布主题 线程 @@ -266,7 +270,8 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length) case 0: { // 收钩 - ESP_LOGD(MOUDLENAME, "Mqtt_HOOK_UP"); + // ESP_LOGD(MOUDLENAME, "Mqtt_HOOK_UP"); + addLogMessage("Mqtt_HOOK_UP"); if (motocontrol.getcontrolstatus().is_autogoodsdown) up_action(SPEED_BTN_FAST); } @@ -279,7 +284,8 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length) case 2: { // 暂停 - ESP_LOGD(MOUDLENAME, "Mqtt_HOOK_PAUSE"); + // ESP_LOGD(MOUDLENAME, "Mqtt_HOOK_PAUSE"); + addLogMessage("Mqtt_HOOK_PAUSE"); if (motocontrol.getcontrolstatus().is_autogoodsdown) Hook_stop(); } @@ -287,7 +293,8 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length) case 3: { // 继续 - ESP_LOGD(MOUDLENAME, "Mqtt_HOOK_resume"); + // ESP_LOGD(MOUDLENAME, "Mqtt_HOOK_resume"); + addLogMessage("Mqtt_HOOK_resume"); if (motocontrol.getcontrolstatus().is_autogoodsdown) Hook_resume(); } @@ -446,7 +453,7 @@ void mavlink_receiveCallback(uint8_t c) // mavlink_msg_rc_channels_decode(&msg, &rc_channels); // uint16_t throttle_value = rc_channels.chan3_raw; // ch3_raw 是油门通道的原始输 - // if (topicPubMsg[18] != "油门：" + String(throttle_value)) + // if (topicPubMsg[18] != "油门：" + String(throttle_value))//注意日常添加队列方式改变 // { // topicPubMsg[18] = "油门：" + String(throttle_value); // 油门值 // } @@ -457,7 +464,7 @@ void mavlink_receiveCallback(uint8_t c) uint16_t fc_hook_cmd = mavlink_msg_command_long_get_command(&msg); uint16_t rngalt_cm = (uint16_t)mavlink_msg_command_long_get_param1(&msg); // printf("COMMAND_LONG ID:%d,rng:%dcm \n", fc_hook_cmd, rngalt_cm); - topicPubMsg[18] = "COMMAND_LONG ID:" + String(fc_hook_cmd) + ",rng:" + String(rngalt_cm) + "cm"; // 终端 打印日志 + addLogMessage("COMMAND_LONG ID:" + String(fc_hook_cmd) + ",rng:" + String(rngalt_cm) + "cm"); // 终端 打印日志 switch (fc_hook_cmd) { // 自动放线 @@ -465,20 +472,20 @@ void mavlink_receiveCallback(uint8_t c) { HookStatus hss = motocontrol.gethooktatus(); // printf("MAV_CMD_FC_HOOK_AUTODOWN Hook:%d,rng:%dcm \n", hss, rngalt_cm); - topicPubMsg[18] = "MAV_CMD_FC_HOOK_AUTODOWN Hook:" + String(hss) + ",rng:" + String(rngalt_cm) + "cm"; // 终端 打印日志 + addLogMessage("MAV_CMD_FC_HOOK_AUTODOWN Hook:" + String(hss) + ",rng:" + String(rngalt_cm) + "cm"); // 终端 打印日志 // 没有激光高度直接退出 if (rngalt_cm == 0) { rngalt_cm == 500; // printf("exit rngalt_cm==0"); - topicPubMsg[18] = "exit rngalt_cm==0，激光高度异常"; + addLogMessage("exit rngalt_cm==0，激光高度异常"); // break; } if (hss == HS_Locked) { Hook_autodown(rngalt_cm); - topicPubMsg[18] = "Hook_autodown"; + addLogMessage("Hook_autodown"); // 语音播报3次 if (fc.questVoiceStr != "") { @@ -491,7 +498,7 @@ void mavlink_receiveCallback(uint8_t c) Hook_resume(); else // printf("exit hooktatus error"); - topicPubMsg[18] = "exit hooktatus error"; + addLogMessage("exit hooktatus error"); } break; } @@ -499,7 +506,7 @@ void mavlink_receiveCallback(uint8_t c) case MAV_CMD_FC_HOOK_PAUSE: { // printf("MAV_CMD_FC_HOOK_PAUSE \n"); - topicPubMsg[18] = "MAV_CMD_FC_HOOK_PAUSE"; + addLogMessage("MAV_CMD_FC_HOOK_PAUSE"); Hook_stop(); break; } @@ -507,7 +514,7 @@ void mavlink_receiveCallback(uint8_t c) case MAV_CMD_FC_HOOK_RECOVERY: { // printf("MAV_CMD_FC_HOOK_RECOVERY \n"); - topicPubMsg[18] = "MAV_CMD_FC_HOOK_PAUSE"; + addLogMessage("MAV_CMD_FC_HOOK_PAUSE"); Hook_recovery(); break; } @@ -674,7 +681,7 @@ void mavlink_receiveCallback(uint8_t c) break; } fc.playText(topicPubMsg[12]); - topicPubMsg[18] = "飞控模式已切换为" + topicPubMsg[12]; // 终端 打印日志 + addLogMessage("飞控模式已切换为" + topicPubMsg[12]); // 终端 打印日志 } } break; @@ -743,10 +750,8 @@ void mavlink_receiveCallback(uint8_t c) // 解析文本信息 strncpy(buf, statustext.text, sizeof(statustext.text)); buf[sizeof(statustext.text)] = '\0'; // 确保字符串以null结尾 - if (topicPubMsg[18] != buf) - { - topicPubMsg[18] = buf; - } + + addLogMessage(buf); // 解析加速度计校准状态 if (fc.isAnalyzeAcce) @@ -953,6 +958,16 @@ void pubThread() doc[topicPub[i]] = topicPubMsg[i]; } } + else if (i == 18) // 从日志队列里 拿日志信息 + { + if (logHead != logTail) + { + topicPubMsg[i] = logQueue[logHead]; + logHead = (logHead + 1) % LOG_QUEUE_SIZE; + doc[topicPub[i]] = topicPubMsg[i]; + topicPubMsg[i] = ""; // 清空，避免重复发 + } + } else // 其余有新值就发 发出后重置 { if (topicPubMsg[i] != "") @@ -967,6 +982,21 @@ void pubThread() serializeJson(doc, jsonString); fc.pubMQTTmsg("planeState", jsonString); } +// 添加日志 到日志队列 +void addLogMessage(const String &msg) +{ + int nextTail = (logTail + 1) % LOG_QUEUE_SIZE; + if (nextTail != logHead) + { // 队列未满 + logQueue[logTail] = msg; + logTail = nextTail; + } + else + { + // 队列已满，这里可以选择丢弃或覆盖最旧的日志 + // logHead = (logHead + 1) % LOG_QUEUE_SIZE; // 开启这行表示覆盖最旧的 + } +} /** * @description: 刷新各请求 diff --git a/src/commser.h b/src/commser.h index 745c464..088c945 100644 --- a/src/commser.h +++ b/src/commser.h @@ -21,6 +21,7 @@ enum FlightMode }; extern void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length); extern void pubThread(); +extern void addLogMessage(const String &msg); extern void refreshRequest(); extern void set_rc_channels(uint16_t channel_values[], uint8_t num_channels); extern void set_mode(FlightMode mode); diff --git a/src/main.cpp b/src/main.cpp index a61942c..b65d380 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -99,7 +99,8 @@ void setup() ESP_LOGI(MOUDLENAME, "Starting PullupDevice... Ver:%s", VERSION); preferences.begin("PullupDev", false); wei_offset = preferences.getLong("wei_offset", 0); - ESP_LOGD(MOUDLENAME, "wei_offset:%d", wei_offset); + // ESP_LOGD(MOUDLENAME, "wei_offset:%d", wei_offset); + addLogMessage("wei_offset: " + String(wei_offset)); // 初始化按钮 button_up.attachClick(upbtn_click); @@ -229,8 +230,8 @@ void showinfo() // 校准称重 void begin_tare() { - ESP_LOGD(MOUDLENAME, "begin_tare"); - topicPubMsg[18] = "begin_tare,校准称重"; + // ESP_LOGD(MOUDLENAME, "begin_tare"); + addLogMessage("begin_tare,校准称重"); _weightalign_status = WAS_Aligning; _needweightalign = true; _weightalign_begintm = millis(); @@ -257,7 +258,8 @@ bool check_tare() wei_offset = scale.get_offset(); preferences.putLong("wei_offset", wei_offset); motocontrol.weightalign(true); - ESP_LOGD(MOUDLENAME, "check_tare ok: %d,offset:%d", pullweight, wei_offset); + // ESP_LOGD(MOUDLENAME, "check_tare ok: %d,offset:%d", pullweight, wei_offset); + addLogMessage("check_tare ok: " + String(pullweight) + ", offset: " + String(wei_offset)); return true; } else @@ -266,7 +268,8 @@ bool check_tare() else { // 没成功继续校准 - ESP_LOGD(MOUDLENAME, "weight not zero: %d", pullweight); + // ESP_LOGD(MOUDLENAME, "weight not zero: %d", pullweight); + addLogMessage("weight not zero: " + String(pullweight)); _needweightalign = true; pullweightoktimes = 0; } @@ -292,7 +295,8 @@ void checkinited() if (motocontrol.gethooktatus() != HS_Locked) { // 开始自动慢速上升，直到顶部按钮按下 - ESP_LOGD(MOUDLENAME, "IS_Start: startup wait locked"); + // ESP_LOGD(MOUDLENAME, "IS_Start: startup wait locked"); + addLogMessage("IS_Start: startup wait locked"); motocontrol.setspeed(SPEED_BTN_SLOW); motocontrol.moto_run(MotoStatus::MS_Up); initstatus = IS_Wait_Locked; @@ -307,7 +311,8 @@ void checkinited() { if (motocontrol.gethooktatus() == HS_Locked) { - ESP_LOGD(MOUDLENAME, "IS_Wait_LengthZero: is locked"); + // ESP_LOGD(MOUDLENAME, "IS_Wait_LengthZero: is locked"); + addLogMessage("IS_Wait_LengthZero: is locked"); initstatus = IS_begin_WeightZero; } break; @@ -318,7 +323,8 @@ void checkinited() // 如果没有保存的校准数据就需要校准 if (wei_offset != 0) { - ESP_LOGD(MOUDLENAME, "offset is loaded weight: %d,offset:%d", pullweight, wei_offset); + // ESP_LOGD(MOUDLENAME, "offset is loaded weight: %d,offset:%d", pullweight, wei_offset); + addLogMessage("offset is loaded weight: " + String(pullweight) + ", offset: " + String(wei_offset)); scale.set_offset(wei_offset); motocontrol.weightalign(true); _needweightalign = false; @@ -327,7 +333,8 @@ void checkinited() } else { - ESP_LOGD(MOUDLENAME, "begin_tare"); + // ESP_LOGD(MOUDLENAME, "begin_tare"); + addLogMessage("begin_tare"); begin_tare(); initstatus = IS_CheckWeightZero; } @@ -342,7 +349,8 @@ void checkinited() } else if (_weightalign_status == WAS_Alignfault) { - ESP_LOGD(MOUDLENAME, "weightalign fault! again"); + // ESP_LOGD(MOUDLENAME, "weightalign fault! again"); + addLogMessage("weightalign fault! again"); initstatus = IS_begin_WeightZero; } break; @@ -587,7 +595,8 @@ void showledidel() // 顶部按钮，检测是否到顶部 void ctbtn_pressstart() { - ESP_LOGD(MOUDLENAME, "Top_pressstart"); + // ESP_LOGD(MOUDLENAME, "Top_pressstart"); + addLogMessage("Top_pressstart"); // 只在上升时停止 if ((motocontrol.gethooktatus() == HS_UnInit) || (motocontrol.gethooktatus() == HS_Up) || (motocontrol.gethooktatus() == HS_InStore)) { @@ -598,15 +607,16 @@ void ctbtn_pressstart() // 顶部按钮，抬起 void ctbtn_pressend() { - ESP_LOGD(MOUDLENAME, "Top_pressend"); + // ESP_LOGD(MOUDLENAME, "Top_pressend"); + addLogMessage("Top_pressend"); set_locked(false); _bengstop = false; } void down_action(float motospeed, float length = 0.0f) { - ESP_LOGD(MOUDLENAME, "Down_action sp:%.2f len:%.2f cm", motospeed, length); - + // ESP_LOGD(MOUDLENAME, "Down_action sp:%.2f len:%.2f cm", motospeed, length); + addLogMessage("Down_action sp:" + String(motospeed, 2) + " len:" + String(length, 2) + " cm"); if (motocontrol.getcontrolstatus().motostatus != MotoStatus::MS_Stop) { motocontrol.stoprun(); @@ -629,7 +639,8 @@ void down_action(float motospeed, float length = 0.0f) void up_action(float motospeed) { - ESP_LOGD(MOUDLENAME, "Up_action sp:%.2f", motospeed); + // ESP_LOGD(MOUDLENAME, "Up_action sp:%.2f", motospeed); + addLogMessage("Up_action sp:" + String(motospeed, 2)); if (motocontrol.getcontrolstatus().motostatus == MotoStatus::MS_Stop) { @@ -644,7 +655,8 @@ void up_action(float motospeed) // 放线按钮--单击 void downbtn_click() { - ESP_LOGD(MOUDLENAME, "Down_click"); + // ESP_LOGD(MOUDLENAME, "Down_click"); + addLogMessage("Down_click"); _checkweightcal = true; // 检测是否需要校准称重传感器--在下放停止时检测 _checkweighttimes = 0; down_action(SPEED_BTN_SLOW, 10); @@ -652,13 +664,15 @@ void downbtn_click() // 放线按钮--双击 void downbtn_dbclick() { - ESP_LOGD(MOUDLENAME, "Down_dbclick"); + // ESP_LOGD(MOUDLENAME, "Down_dbclick"); + addLogMessage("Down_dbclick"); down_action(SPEED_BTN_MID); } // 放线按钮--长按 void downbtn_pressstart() { - ESP_LOGD(MOUDLENAME, "Down_pressstart"); + // ESP_LOGD(MOUDLENAME, "Down_pressstart"); + addLogMessage("Down_pressstart"); // 两个同时按用于对频 btn_pressatonce++; if (btn_pressatonce > 2) @@ -673,7 +687,8 @@ void downbtn_pressstart() // 放线按钮--长按抬起 void downbtn_pressend() { - ESP_LOGD(MOUDLENAME, "Down_pressend"); + // ESP_LOGD(MOUDLENAME, "Down_pressend"); + addLogMessage("Down_pressend"); btn_pressatonce--; if (btn_pressatonce < 0) btn_pressatonce = 0; @@ -686,19 +701,22 @@ void downbtn_pressend() void upbtn_click() { // fc.mav_send_text(MAV_SEVERITY_INFO,"upbtn_click"); - ESP_LOGD(MOUDLENAME, "UP_click"); + // ESP_LOGD(MOUDLENAME, "UP_click"); + addLogMessage("UP_click"); up_action(SPEED_BTN_SLOW); } // 收线按钮-双击 void upbtn_dbclick() { - ESP_LOGD(MOUDLENAME, "UP_dbclick"); + // ESP_LOGD(MOUDLENAME, "UP_dbclick"); + addLogMessage("UP_dbclick"); up_action(SPEED_BTN_MID); } // 两个按钮同时按下 void btn_presssatonce() { - ESP_LOGD(MOUDLENAME, "UP_presssatonce"); + // ESP_LOGD(MOUDLENAME, "UP_presssatonce"); + addLogMessage("UP_presssatonce"); led_show(255, 255, 255); // 高亮一下 fc.playText("发送对频信息"); fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd()); @@ -706,7 +724,8 @@ void btn_presssatonce() // 收线按钮-长按 void upbtn_pressstart() { - ESP_LOGD(MOUDLENAME, "UP_pressstart"); + // ESP_LOGD(MOUDLENAME, "UP_pressstart"); + addLogMessage("UP_pressstart"); // 两个同时按用于对频 btn_pressatonce++; if (btn_pressatonce > 2) @@ -721,7 +740,8 @@ void upbtn_pressstart() // 收线按钮-长按抬起 void upbtn_pressend() { - ESP_LOGD(MOUDLENAME, "UP_pressend"); + // ESP_LOGD(MOUDLENAME, "UP_pressend"); + addLogMessage("UP_pressend"); btn_pressatonce--; if (btn_pressatonce < 0) btn_pressatonce = 0; @@ -734,7 +754,8 @@ void Hook_autodown(float length_cm) { if (motocontrol.gethooktatus() == HS_Locked) { - ESP_LOGD(MOUDLENAME, "Hook_autodown %.2f cm", length_cm); + // ESP_LOGD(MOUDLENAME, "Hook_autodown %.2f cm", length_cm); + addLogMessage("Hook_autodown " + String(length_cm, 2) + " cm"); fc.Camera_action_down(); motocontrol.moto_goodsdown(length_cm); // 二楼340 //桌子40 } @@ -743,22 +764,26 @@ void Hook_autodown(float length_cm) } void Hook_stop() { - ESP_LOGD(MOUDLENAME, "Hook_stop"); + // ESP_LOGD(MOUDLENAME, "Hook_stop"); + addLogMessage("Hook_stop"); motocontrol.stoprun(); } void Hook_resume() { if (motocontrol.gethooktatus() == HS_Stop) { - ESP_LOGD(MOUDLENAME, "Hook_resume"); + // ESP_LOGD(MOUDLENAME, "Hook_resume"); + addLogMessage("Hook_resume"); motocontrol.moto_goodsdownresume(); } else - ESP_LOGE(MOUDLENAME, "resume fault, not HS_Stop "); + // ESP_LOGE(MOUDLENAME, "resume fault, not HS_Stop "); + addLogMessage("resume fault, not HS_Stop "); } void Hook_recovery() { - ESP_LOGD(MOUDLENAME, "Hook_recovery"); + // ESP_LOGD(MOUDLENAME, "Hook_recovery"); + addLogMessage("Hook_recovery"); if (motocontrol.gethooktatus() != HS_Locked) { motocontrol.stoprun(); @@ -769,25 +794,29 @@ void Hook_recovery() // 测试按钮 void testbtn_click() { - ESP_LOGD(MOUDLENAME, "testbtn_click"); + // ESP_LOGD(MOUDLENAME, "testbtn_click"); + addLogMessage("testbtn_click"); switch (motocontrol.gethooktatus()) { case HS_Locked: // 在顶部--会自动检测是否有物体，有才下放 { - ESP_LOGD(MOUDLENAME, "moto_goodsdown"); + // ESP_LOGD(MOUDLENAME, "moto_goodsdown"); + addLogMessage("moto_goodsdown"); motocontrol.moto_goodsdown(22); // 二楼340 //桌子40 break; } case HS_Stop: { - ESP_LOGD(MOUDLENAME, "moto_resume"); + // ESP_LOGD(MOUDLENAME, "moto_resume"); + addLogMessage("moto_resume"); motocontrol.moto_goodsdownresume(); break; } default: { - ESP_LOGD(MOUDLENAME, "stop"); + // ESP_LOGD(MOUDLENAME, "stop"); + addLogMessage("stop"); motocontrol.stoprun(); } } diff --git a/src/moto.cpp b/src/moto.cpp index 39fe1ae..08ee4fe 100644 --- a/src/moto.cpp +++ b/src/moto.cpp @@ -1,24 +1,27 @@ #include "moto.h" #include "Arduino.h" #include "config.h" +#include "commser.h" uint8_t CaninBuffer[8]; // 接收指令缓冲区 moto_measure_t moto_chassis; PID_TypeDef moto_pid; -static const char* MOUDLENAME = "MOTO"; +static const char *MOUDLENAME = "MOTO"; moto::moto() { _closed = true; } bool moto::init() { - ESP_LOGD(MOUDLENAME,"init moto"); + // ESP_LOGD(MOUDLENAME,"init moto"); + addLogMessage("init moto"); pid_init(); CAN.setPins(MOTO_CAN_RX, MOTO_CAN_TX); // start the CAN bus at 500 kbps if (!CAN.begin(1000E3)) { - ESP_LOGE(MOUDLENAME,"Starting CAN failed!"); + // ESP_LOGE(MOUDLENAME, "Starting CAN failed!"); + addLogMessage("Starting CAN failed!"); return false; } CAN.onReceive(onReceive); @@ -30,7 +33,7 @@ moto::~moto() void moto::update() { - //printf("u1:%d\n",_closed); + // printf("u1:%d\n",_closed); if (!_closed) { unsigned long dt = millis() - moto_chassis.starttime; // 时间差 @@ -45,7 +48,7 @@ void moto::update() _msoftspeed = _start_speed + mspeed * _ds; } pid_cal(&moto_pid, moto_chassis.speed_rpm, _msoftspeed); - // printf("u2\n"); + // printf("u2\n"); set_moto_current(moto_pid.output); // printf("tar:%.2f,dt:%d,ds:%.2f,x:%.2f,mspeed:%.2f,motspeed:%.2f,out:%.2f\n", // moto_pid.target,dt,_ds,x,mspeed,_msoftspeed,moto_pid.output); @@ -66,7 +69,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, new_target); + // printf("speedtarget:tm:%d,target:%.2f\n", moto_chassis.starttime, new_target); } void moto::set_moto_current(int16_t iq1) diff --git a/src/motocontrol.cpp b/src/motocontrol.cpp index 57f07fa..6c1b066 100644 --- a/src/motocontrol.cpp +++ b/src/motocontrol.cpp @@ -4,6 +4,7 @@ #include "moto.h" #include #include "config.h" +#include "commser.h" #define DEBUG_OUT false static const char *MOUDLENAME = "MOTO_C"; @@ -139,13 +140,15 @@ void Motocontrol::checkgoods() // 检测是否超重 } void Motocontrol::lockservo() // 锁定舵机 { - ESP_LOGD(MOUDLENAME, "start_lockservo"); + // ESP_LOGD(MOUDLENAME, "start_lockservo"); + addLogMessage("start_lockservo"); _servotatus = SS_WaitMotoStop; _tm_servotatus = millis(); } void Motocontrol::unlockservo() // 解锁舵机 { - ESP_LOGD(MOUDLENAME, "unlockservo"); + // ESP_LOGD(MOUDLENAME, "unlockservo"); + addLogMessage("unlockservo"); // 解锁操作 _lockservo->write(SERVO_UNLOCKPOS); _moto_dji.setspeedtarget(-1.0f); // 解锁时逆向慢速转动 @@ -155,7 +158,8 @@ void Motocontrol::unlockservo() // 解锁舵机 } void Motocontrol::stoprun(float acctime) // 停止 { - ESP_LOGD(MOUDLENAME, "stoprun after time:%.2f", acctime); + // ESP_LOGD(MOUDLENAME, "stoprun after time:%.2f", acctime); + addLogMessage("stoprun after time: " + String(acctime, 2)); _moto_dji.settime_acc(acctime); _moto_dji.setspeedtarget(0.0f); _controlstatus.motostatus = MS_Stop; @@ -173,7 +177,8 @@ void Motocontrol::stopautodown() void Motocontrol::sethooksstatus(HookStatus hookstatus) { _hooksstatus = hookstatus; - ESP_LOGD(MOUDLENAME, "Set HS:%s", gethooktatus_str(false)); + // ESP_LOGD(MOUDLENAME, "Set HS:%s", gethooktatus_str(false)); + addLogMessage("Set HS: " + String(gethooktatus_str(false))); } void Motocontrol::setlocked(bool locked) { @@ -217,14 +222,16 @@ 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); + addLogMessage("Not goods wait " + String(HOOK_UNHOOK_TIME) + " ms for unhook"); 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); + addLogMessage("HS_Down target is arrived curr: " + String(mf.output_round_cnt, 2)); sethooksstatus(HS_DownSlow); _moto_dji.settime_acc(500); set_hook_speed(SPEED_HOOK_SLOW); @@ -236,7 +243,8 @@ 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); + addLogMessage("Not havegoods wait " + String(HOOK_UNHOOK_TIME) + " ms unhook"); sethooksstatus(HS_WaitUnhook); _tm_waitunhook = millis(); } @@ -247,7 +255,8 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态 { if ((millis() - _tm_waitunhook) > HOOK_UNHOOK_TIME) { - ESP_LOGD(MOUDLENAME, "HS_WaitUnhook ok startup"); + // ESP_LOGD(MOUDLENAME, "HS_WaitUnhook ok startup"); + addLogMessage("HS_WaitUnhook ok startup"); _moto_dji.settime_acc(1000); set_hook_speed(-SPEED_HOOK_UP); sethooksstatus(HS_Up); @@ -313,14 +322,16 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候 // 检测堵转,没堵转直接关闭电机,否则打开舵机，让电机旋转一点再锁定--最多重复3次 if (!checkservoblock() || (_unblocktimes > 2)) { - ESP_LOGD(MOUDLENAME, "Close moto"); + // ESP_LOGD(MOUDLENAME, "Close moto"); + addLogMessage("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"); + addLogMessage("SS servoblock unlock servo and turn moto"); _tm_servotatus = millis(); _servotatus = SS_WaitUnBlock; // 写一个不会堵转的位置 @@ -336,7 +347,8 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候 { if ((millis() - _tm_servotatus) > TM_UNBLOCK) { - ESP_LOGD(MOUDLENAME, "SS lock servo"); + // ESP_LOGD(MOUDLENAME, "SS lock servo"); + addLogMessage("SS lock servo"); // 继续锁定等待舵机检测 _lockservo->write(SERVO_LOCKPOS); _servotatus = SS_WaitServo; @@ -370,7 +382,8 @@ 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); + addLogMessage("Instorelen: " + String(instlen, 2) + ", speed: " + String(mf.output_speed_rpm, 2)); } } else @@ -386,7 +399,8 @@ 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); + addLogMessage("stop down tar: " + String(mf.output_round_cnt, 2) + ", " + String(_target_cnt)); stoprun(1000); // 缓慢停止 } } @@ -394,7 +408,8 @@ 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); + addLogMessage("stop up tar: " + String(mf.output_round_cnt, 2) + ", " + String(_target_cnt)); stoprun(1000); // 缓慢停止 } } @@ -452,24 +467,28 @@ void Motocontrol::moto_goodsdown(float length) { if (length < 0.0) { - ESP_LOGD(MOUDLENAME, "length<0 fault"); + // ESP_LOGD(MOUDLENAME, "length<0 fault"); + addLogMessage("length<0 fault"); return; } // 没设置零点 if (!_controlstatus.is_setzero) { - ESP_LOGD(MOUDLENAME, "not lengthzero fault"); + // ESP_LOGD(MOUDLENAME, "not lengthzero fault"); + addLogMessage("not lengthzero fault"); return; } if (!_weightalign) { - ESP_LOGD(MOUDLENAME, "weight not align fault"); + // ESP_LOGD(MOUDLENAME, "weight not align fault"); + addLogMessage("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); + addLogMessage("goods min fault: " + String(_pullweight)); return; } @@ -490,7 +509,8 @@ void Motocontrol::moto_goodsdown(float 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); + addLogMessage("start down " + String(tarleng, 2) + " cm, tar: " + String(_goods_down_target_cnt, 2)); setspeed(SPEED_HOOK_FAST, TM_ACC_HS); _goods_speed = SPEED_HOOK_FAST; } @@ -510,25 +530,29 @@ 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"); + addLogMessage("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"); + addLogMessage("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"); + addLogMessage("is_toplocked return"); return; } if (_controlstatus.is_overweight) { - ESP_LOGD(MOUDLENAME, "overweight fault :%d", _pullweight); + // ESP_LOGD(MOUDLENAME, "overweight fault :%d", _pullweight); + addLogMessage("overweight fault :" + String(_pullweight)); return; } sethooksstatus(HS_Up);