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

【类 型】：factor

Browse Source 
【原  因】：串口打印的日志 都发送到前端日志
【过  程】：
【影  响】：

# 类型 包含:
# feat：新功能（feature）
# fix：修补bug
# docs：文档（documentation）
# style： 格式（不影响代码运行的变动）
# refactor：重构（即不是新增功能，也不是修改bug的代码变动）
# test：增加测试
# chore：构建过程或辅助工具的变动
This commit is contained in:
air 2025-05-14 14:31:36 +08:00
parent d21b8d3b1f
commit 9f3f25acc2
5 changed files with 166 additions and 79 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
src/commser.cpp
View File

@ -49,10 +49,14 @@ const String topicPub[] = {"heartBeat", "voltagBattery", "currentBattery", "batt
*/ */
const int topicPubCount = sizeof(topicPub) / sizeof(topicPub[0]); // 登记 json成员总数 const int topicPubCount = sizeof(topicPub) / sizeof(topicPub[0]); // 登记 json成员总数
String topicPubMsg[topicPubCount] = {""}; // 登记 json成员的值 对应topicPub String topicPubMsg[topicPubCount] = {""}; // 登记 json成员的值 对应topicPub
/*日志队列*/
#define LOG_QUEUE_SIZE 50 // 日志队列大小
String logQueue[LOG_QUEUE_SIZE];
int logHead = 0;
int logTail = 0;
/*触发发送 主题*/ /*触发发送 主题*/
// 0:对频信息 // 0:对频信息
String topicHandle[] = {"crosFrequency"}; String topicHandle[] = {"crosFrequency"};
boolean isPush = false; // 记得删除 板子按钮状态 ps:D3引脚下拉微动开关
/*异步线程对象*/ /*异步线程对象*/
Ticker pubTicker; // 定时发布主题 线程 Ticker pubTicker; // 定时发布主题 线程
@ -266,7 +270,8 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length)
case 0: case 0:
{ {
// 收钩 // 收钩
ESP_LOGD(MOUDLENAME, "Mqtt_HOOK_UP"); // ESP_LOGD(MOUDLENAME, "Mqtt_HOOK_UP");
addLogMessage("Mqtt_HOOK_UP");
if (motocontrol.getcontrolstatus().is_autogoodsdown) if (motocontrol.getcontrolstatus().is_autogoodsdown)
up_action(SPEED_BTN_FAST); up_action(SPEED_BTN_FAST);
} }
@ -279,7 +284,8 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length)
case 2: case 2:
{ {
// 暂停 // 暂停
ESP_LOGD(MOUDLENAME, "Mqtt_HOOK_PAUSE"); // ESP_LOGD(MOUDLENAME, "Mqtt_HOOK_PAUSE");
addLogMessage("Mqtt_HOOK_PAUSE");
if (motocontrol.getcontrolstatus().is_autogoodsdown) if (motocontrol.getcontrolstatus().is_autogoodsdown)
Hook_stop(); Hook_stop();
} }
@ -287,7 +293,8 @@ void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length)
case 3: case 3:
{ {
// 继续 // 继续
ESP_LOGD(MOUDLENAME, "Mqtt_HOOK_resume"); // ESP_LOGD(MOUDLENAME, "Mqtt_HOOK_resume");
addLogMessage("Mqtt_HOOK_resume");
if (motocontrol.getcontrolstatus().is_autogoodsdown) if (motocontrol.getcontrolstatus().is_autogoodsdown)
Hook_resume(); Hook_resume();
} }
@ -446,7 +453,7 @@ void mavlink_receiveCallback(uint8_t c)
// mavlink_msg_rc_channels_decode(&msg, &rc_channels); // mavlink_msg_rc_channels_decode(&msg, &rc_channels);
// uint16_t throttle_value = rc_channels.chan3_raw; // ch3_raw 是油门通道的原始输 // 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); // 油门值 // 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 fc_hook_cmd = mavlink_msg_command_long_get_command(&msg);
uint16_t rngalt_cm = (uint16_t)mavlink_msg_command_long_get_param1(&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); // 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) switch (fc_hook_cmd)
{ {
// 自动放线 // 自动放线
@ -465,20 +472,20 @@ void mavlink_receiveCallback(uint8_t c)
{ {
HookStatus hss = motocontrol.gethooktatus(); HookStatus hss = motocontrol.gethooktatus();
// printf("MAV_CMD_FC_HOOK_AUTODOWN Hook:%d,rng:%dcm \n", hss, rngalt_cm); // 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) if (rngalt_cm == 0)
{ {
rngalt_cm == 500; rngalt_cm == 500;
// printf("exit rngalt_cm==0"); // printf("exit rngalt_cm==0");
topicPubMsg[18] = "exit rngalt_cm==0，激光高度异常"; addLogMessage("exit rngalt_cm==0，激光高度异常");
// break; // break;
} }
if (hss == HS_Locked) if (hss == HS_Locked)
{ {
Hook_autodown(rngalt_cm); Hook_autodown(rngalt_cm);
topicPubMsg[18] = "Hook_autodown"; addLogMessage("Hook_autodown");
// 语音播报3次 // 语音播报3次
if (fc.questVoiceStr != "") if (fc.questVoiceStr != "")
{ {
@ -491,7 +498,7 @@ void mavlink_receiveCallback(uint8_t c)
Hook_resume(); Hook_resume();
else else
// printf("exit hooktatus error"); // printf("exit hooktatus error");
topicPubMsg[18] = "exit hooktatus error"; addLogMessage("exit hooktatus error");
} }
break; break;
} }
@ -499,7 +506,7 @@ void mavlink_receiveCallback(uint8_t c)
case MAV_CMD_FC_HOOK_PAUSE: case MAV_CMD_FC_HOOK_PAUSE:
{ {
// printf("MAV_CMD_FC_HOOK_PAUSE \n"); // printf("MAV_CMD_FC_HOOK_PAUSE \n");
topicPubMsg[18] = "MAV_CMD_FC_HOOK_PAUSE"; addLogMessage("MAV_CMD_FC_HOOK_PAUSE");
Hook_stop(); Hook_stop();
break; break;
} }
@ -507,7 +514,7 @@ void mavlink_receiveCallback(uint8_t c)
case MAV_CMD_FC_HOOK_RECOVERY: case MAV_CMD_FC_HOOK_RECOVERY:
{ {
// printf("MAV_CMD_FC_HOOK_RECOVERY \n"); // printf("MAV_CMD_FC_HOOK_RECOVERY \n");
topicPubMsg[18] = "MAV_CMD_FC_HOOK_PAUSE"; addLogMessage("MAV_CMD_FC_HOOK_PAUSE");
Hook_recovery(); Hook_recovery();
break; break;
} }
@ -674,7 +681,7 @@ void mavlink_receiveCallback(uint8_t c)
break; break;
} }
fc.playText(topicPubMsg[12]); fc.playText(topicPubMsg[12]);
topicPubMsg[18] = "飞控模式已切换为" + topicPubMsg[12]; // 终端 打印日志 addLogMessage("飞控模式已切换为" + topicPubMsg[12]); // 终端 打印日志
} }
} }
break; break;
@ -743,10 +750,8 @@ void mavlink_receiveCallback(uint8_t c)
// 解析文本信息 // 解析文本信息
strncpy(buf, statustext.text, sizeof(statustext.text)); strncpy(buf, statustext.text, sizeof(statustext.text));
buf[sizeof(statustext.text)] = '\0'; // 确保字符串以null结尾 buf[sizeof(statustext.text)] = '\0'; // 确保字符串以null结尾
if (topicPubMsg[18] != buf)
{ addLogMessage(buf);
topicPubMsg[18] = buf;
}
// 解析加速度计校准状态 // 解析加速度计校准状态
if (fc.isAnalyzeAcce) if (fc.isAnalyzeAcce)
@ -953,6 +958,16 @@ void pubThread()
doc[topicPub[i]] = topicPubMsg[i]; 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 // 其余有新值就发 发出后重置 else // 其余有新值就发 发出后重置
{ {
if (topicPubMsg[i] != "") if (topicPubMsg[i] != "")
@ -967,6 +982,21 @@ void pubThread()
serializeJson(doc, jsonString); serializeJson(doc, jsonString);
fc.pubMQTTmsg("planeState", 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: 刷新各请求 * @description: 刷新各请求

1
src/commser.h
View File

@ -21,6 +21,7 @@ enum FlightMode
}; };
extern void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length); extern void mqtt_receiveCallback(char *topic, byte *payload, unsigned int length);
extern void pubThread(); extern void pubThread();
extern void addLogMessage(const String &msg);
extern void refreshRequest(); extern void refreshRequest();
extern void set_rc_channels(uint16_t channel_values[], uint8_t num_channels); extern void set_rc_channels(uint16_t channel_values[], uint8_t num_channels);
extern void set_mode(FlightMode mode); extern void set_mode(FlightMode mode);

95
src/main.cpp
View File

@ -99,7 +99,8 @@ void setup()
ESP_LOGI(MOUDLENAME, "Starting PullupDevice... Ver:%s", VERSION); ESP_LOGI(MOUDLENAME, "Starting PullupDevice... Ver:%s", VERSION);
preferences.begin("PullupDev", false); preferences.begin("PullupDev", false);
wei_offset = preferences.getLong("wei_offset", 0); 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); button_up.attachClick(upbtn_click);
@ -229,8 +230,8 @@ void showinfo()
// 校准称重 // 校准称重
void begin_tare() void begin_tare()
{ {
ESP_LOGD(MOUDLENAME, "begin_tare"); // ESP_LOGD(MOUDLENAME, "begin_tare");
topicPubMsg[18] = "begin_tare,校准称重"; addLogMessage("begin_tare,校准称重");
_weightalign_status = WAS_Aligning; _weightalign_status = WAS_Aligning;
_needweightalign = true; _needweightalign = true;
_weightalign_begintm = millis(); _weightalign_begintm = millis();
@ -257,7 +258,8 @@ bool check_tare()
wei_offset = scale.get_offset(); wei_offset = scale.get_offset();
preferences.putLong("wei_offset", wei_offset); preferences.putLong("wei_offset", wei_offset);
motocontrol.weightalign(true); 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; return true;
} }
else else
@ -266,7 +268,8 @@ bool check_tare()
else 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; _needweightalign = true;
pullweightoktimes = 0; pullweightoktimes = 0;
} }
@ -292,7 +295,8 @@ void checkinited()
if (motocontrol.gethooktatus() != HS_Locked) 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.setspeed(SPEED_BTN_SLOW);
motocontrol.moto_run(MotoStatus::MS_Up); motocontrol.moto_run(MotoStatus::MS_Up);
initstatus = IS_Wait_Locked; initstatus = IS_Wait_Locked;
@ -307,7 +311,8 @@ void checkinited()
{ {
if (motocontrol.gethooktatus() == HS_Locked) 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; initstatus = IS_begin_WeightZero;
} }
break; break;
@ -318,7 +323,8 @@ void checkinited()
// 如果没有保存的校准数据就需要校准 // 如果没有保存的校准数据就需要校准
if (wei_offset != 0) 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); scale.set_offset(wei_offset);
motocontrol.weightalign(true); motocontrol.weightalign(true);
_needweightalign = false; _needweightalign = false;
@ -327,7 +333,8 @@ void checkinited()
} }
else else
{ {
ESP_LOGD(MOUDLENAME, "begin_tare"); // ESP_LOGD(MOUDLENAME, "begin_tare");
addLogMessage("begin_tare");
begin_tare(); begin_tare();
initstatus = IS_CheckWeightZero; initstatus = IS_CheckWeightZero;
} }
@ -342,7 +349,8 @@ void checkinited()
} }
else if (_weightalign_status == WAS_Alignfault) 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; initstatus = IS_begin_WeightZero;
} }
break; break;
@ -587,7 +595,8 @@ void showledidel()
// 顶部按钮，检测是否到顶部 // 顶部按钮，检测是否到顶部
void ctbtn_pressstart() 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)) if ((motocontrol.gethooktatus() == HS_UnInit) || (motocontrol.gethooktatus() == HS_Up) || (motocontrol.gethooktatus() == HS_InStore))
{ {
@ -598,15 +607,16 @@ void ctbtn_pressstart()
// 顶部按钮，抬起 // 顶部按钮，抬起
void ctbtn_pressend() void ctbtn_pressend()
{ {
ESP_LOGD(MOUDLENAME, "Top_pressend"); // ESP_LOGD(MOUDLENAME, "Top_pressend");
addLogMessage("Top_pressend");
set_locked(false); set_locked(false);
_bengstop = false; _bengstop = false;
} }
void down_action(float motospeed, float length = 0.0f) 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) if (motocontrol.getcontrolstatus().motostatus != MotoStatus::MS_Stop)
{ {
motocontrol.stoprun(); motocontrol.stoprun();
@ -629,7 +639,8 @@ void down_action(float motospeed, float length = 0.0f)
void up_action(float motospeed) 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) if (motocontrol.getcontrolstatus().motostatus == MotoStatus::MS_Stop)
{ {
@ -644,7 +655,8 @@ void up_action(float motospeed)
// 放线按钮--单击 // 放线按钮--单击
void downbtn_click() void downbtn_click()
{ {
ESP_LOGD(MOUDLENAME, "Down_click"); // ESP_LOGD(MOUDLENAME, "Down_click");
addLogMessage("Down_click");
_checkweightcal = true; // 检测是否需要校准称重传感器--在下放停止时检测 _checkweightcal = true; // 检测是否需要校准称重传感器--在下放停止时检测
_checkweighttimes = 0; _checkweighttimes = 0;
down_action(SPEED_BTN_SLOW, 10); down_action(SPEED_BTN_SLOW, 10);
@ -652,13 +664,15 @@ void downbtn_click()
// 放线按钮--双击 // 放线按钮--双击
void downbtn_dbclick() void downbtn_dbclick()
{ {
ESP_LOGD(MOUDLENAME, "Down_dbclick"); // ESP_LOGD(MOUDLENAME, "Down_dbclick");
addLogMessage("Down_dbclick");
down_action(SPEED_BTN_MID); down_action(SPEED_BTN_MID);
} }
// 放线按钮--长按 // 放线按钮--长按
void downbtn_pressstart() void downbtn_pressstart()
{ {
ESP_LOGD(MOUDLENAME, "Down_pressstart"); // ESP_LOGD(MOUDLENAME, "Down_pressstart");
addLogMessage("Down_pressstart");
// 两个同时按用于对频 // 两个同时按用于对频
btn_pressatonce++; btn_pressatonce++;
if (btn_pressatonce > 2) if (btn_pressatonce > 2)
@ -673,7 +687,8 @@ void downbtn_pressstart()
// 放线按钮--长按抬起 // 放线按钮--长按抬起
void downbtn_pressend() void downbtn_pressend()
{ {
ESP_LOGD(MOUDLENAME, "Down_pressend"); // ESP_LOGD(MOUDLENAME, "Down_pressend");
addLogMessage("Down_pressend");
btn_pressatonce--; btn_pressatonce--;
if (btn_pressatonce < 0) if (btn_pressatonce < 0)
btn_pressatonce = 0; btn_pressatonce = 0;
@ -686,19 +701,22 @@ void downbtn_pressend()
void upbtn_click() void upbtn_click()
{ {
// fc.mav_send_text(MAV_SEVERITY_INFO,"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); up_action(SPEED_BTN_SLOW);
} }
// 收线按钮-双击 // 收线按钮-双击
void upbtn_dbclick() void upbtn_dbclick()
{ {
ESP_LOGD(MOUDLENAME, "UP_dbclick"); // ESP_LOGD(MOUDLENAME, "UP_dbclick");
addLogMessage("UP_dbclick");
up_action(SPEED_BTN_MID); up_action(SPEED_BTN_MID);
} }
// 两个按钮同时按下 // 两个按钮同时按下
void btn_presssatonce() void btn_presssatonce()
{ {
ESP_LOGD(MOUDLENAME, "UP_presssatonce"); // ESP_LOGD(MOUDLENAME, "UP_presssatonce");
addLogMessage("UP_presssatonce");
led_show(255, 255, 255); // 高亮一下 led_show(255, 255, 255); // 高亮一下
fc.playText("发送对频信息"); fc.playText("发送对频信息");
fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd()); fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd());
@ -706,7 +724,8 @@ void btn_presssatonce()
// 收线按钮-长按 // 收线按钮-长按
void upbtn_pressstart() void upbtn_pressstart()
{ {
ESP_LOGD(MOUDLENAME, "UP_pressstart"); // ESP_LOGD(MOUDLENAME, "UP_pressstart");
addLogMessage("UP_pressstart");
// 两个同时按用于对频 // 两个同时按用于对频
btn_pressatonce++; btn_pressatonce++;
if (btn_pressatonce > 2) if (btn_pressatonce > 2)
@ -721,7 +740,8 @@ void upbtn_pressstart()
// 收线按钮-长按抬起 // 收线按钮-长按抬起
void upbtn_pressend() void upbtn_pressend()
{ {
ESP_LOGD(MOUDLENAME, "UP_pressend"); // ESP_LOGD(MOUDLENAME, "UP_pressend");
addLogMessage("UP_pressend");
btn_pressatonce--; btn_pressatonce--;
if (btn_pressatonce < 0) if (btn_pressatonce < 0)
btn_pressatonce = 0; btn_pressatonce = 0;
@ -734,7 +754,8 @@ void Hook_autodown(float length_cm)
{ {
if (motocontrol.gethooktatus() == HS_Locked) 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(); fc.Camera_action_down();
motocontrol.moto_goodsdown(length_cm); // 二楼340 //桌子40 motocontrol.moto_goodsdown(length_cm); // 二楼340 //桌子40
} }
@ -743,22 +764,26 @@ void Hook_autodown(float length_cm)
} }
void Hook_stop() void Hook_stop()
{ {
ESP_LOGD(MOUDLENAME, "Hook_stop"); // ESP_LOGD(MOUDLENAME, "Hook_stop");
addLogMessage("Hook_stop");
motocontrol.stoprun(); motocontrol.stoprun();
} }
void Hook_resume() void Hook_resume()
{ {
if (motocontrol.gethooktatus() == HS_Stop) if (motocontrol.gethooktatus() == HS_Stop)
{ {
ESP_LOGD(MOUDLENAME, "Hook_resume"); // ESP_LOGD(MOUDLENAME, "Hook_resume");
addLogMessage("Hook_resume");
motocontrol.moto_goodsdownresume(); motocontrol.moto_goodsdownresume();
} }
else 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() void Hook_recovery()
{ {
ESP_LOGD(MOUDLENAME, "Hook_recovery"); // ESP_LOGD(MOUDLENAME, "Hook_recovery");
addLogMessage("Hook_recovery");
if (motocontrol.gethooktatus() != HS_Locked) if (motocontrol.gethooktatus() != HS_Locked)
{ {
motocontrol.stoprun(); motocontrol.stoprun();
@ -769,25 +794,29 @@ void Hook_recovery()
// 测试按钮 // 测试按钮
void testbtn_click() void testbtn_click()
{ {
ESP_LOGD(MOUDLENAME, "testbtn_click"); // ESP_LOGD(MOUDLENAME, "testbtn_click");
addLogMessage("testbtn_click");
switch (motocontrol.gethooktatus()) switch (motocontrol.gethooktatus())
{ {
case HS_Locked: // 在顶部--会自动检测是否有物体，有才下放 case HS_Locked: // 在顶部--会自动检测是否有物体，有才下放
{ {
ESP_LOGD(MOUDLENAME, "moto_goodsdown"); // ESP_LOGD(MOUDLENAME, "moto_goodsdown");
addLogMessage("moto_goodsdown");
motocontrol.moto_goodsdown(22); // 二楼340 //桌子40 motocontrol.moto_goodsdown(22); // 二楼340 //桌子40
break; break;
} }
case HS_Stop: case HS_Stop:
{ {
ESP_LOGD(MOUDLENAME, "moto_resume"); // ESP_LOGD(MOUDLENAME, "moto_resume");
addLogMessage("moto_resume");
motocontrol.moto_goodsdownresume(); motocontrol.moto_goodsdownresume();
break; break;
} }
default: default:
{ {
ESP_LOGD(MOUDLENAME, "stop"); // ESP_LOGD(MOUDLENAME, "stop");
addLogMessage("stop");
motocontrol.stoprun(); motocontrol.stoprun();
} }
} }

15
src/moto.cpp
View File

@ -1,24 +1,27 @@
#include "moto.h" #include "moto.h"
#include "Arduino.h" #include "Arduino.h"
#include "config.h" #include "config.h"
#include "commser.h"
uint8_t CaninBuffer[8]; // 接收指令缓冲区 uint8_t CaninBuffer[8]; // 接收指令缓冲区
moto_measure_t moto_chassis; moto_measure_t moto_chassis;
PID_TypeDef moto_pid; PID_TypeDef moto_pid;
static const char* MOUDLENAME = "MOTO"; static const char *MOUDLENAME = "MOTO";
moto::moto() moto::moto()
{ {
_closed = true; _closed = true;
} }
bool moto::init() bool moto::init()
{ {
ESP_LOGD(MOUDLENAME,"init moto"); // ESP_LOGD(MOUDLENAME,"init moto");
addLogMessage("init moto");
pid_init(); pid_init();
CAN.setPins(MOTO_CAN_RX, MOTO_CAN_TX); CAN.setPins(MOTO_CAN_RX, MOTO_CAN_TX);
// start the CAN bus at 500 kbps // start the CAN bus at 500 kbps
if (!CAN.begin(1000E3)) if (!CAN.begin(1000E3))
{ {
ESP_LOGE(MOUDLENAME,"Starting CAN failed!"); // ESP_LOGE(MOUDLENAME, "Starting CAN failed!");
addLogMessage("Starting CAN failed!");
return false; return false;
} }
CAN.onReceive(onReceive); CAN.onReceive(onReceive);
@ -30,7 +33,7 @@ moto::~moto()
void moto::update() void moto::update()
{ {
//printf("u1:%d\n",_closed); // printf("u1:%d\n",_closed);
if (!_closed) if (!_closed)
{ {
unsigned long dt = millis() - moto_chassis.starttime; // 时间差 unsigned long dt = millis() - moto_chassis.starttime; // 时间差
@ -45,7 +48,7 @@ void moto::update()
_msoftspeed = _start_speed + mspeed * _ds; _msoftspeed = _start_speed + mspeed * _ds;
} }
pid_cal(&moto_pid, moto_chassis.speed_rpm, _msoftspeed); pid_cal(&moto_pid, moto_chassis.speed_rpm, _msoftspeed);
// printf("u2\n"); // printf("u2\n");
set_moto_current(moto_pid.output); set_moto_current(moto_pid.output);
// printf("tar:%.2f,dt:%d,ds:%.2f,x:%.2f,mspeed:%.2f,motspeed:%.2f,out:%.2f\n", // 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); // 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; _start_speed = moto_chassis.speed_rpm;
_ds = moto_pid.target - _start_speed; // 速度差 _ds = moto_pid.target - _start_speed; // 速度差
_closed = false; _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) void moto::set_moto_current(int16_t iq1)

70
src/motocontrol.cpp
View File

@ -4,6 +4,7 @@
#include "moto.h" #include "moto.h"
#include <ESP32Servo.h> #include <ESP32Servo.h>
#include "config.h" #include "config.h"
#include "commser.h"
#define DEBUG_OUT false #define DEBUG_OUT false
static const char *MOUDLENAME = "MOTO_C"; static const char *MOUDLENAME = "MOTO_C";
@ -139,13 +140,15 @@ void Motocontrol::checkgoods() // 检测是否超重
} }
void Motocontrol::lockservo() // 锁定舵机 void Motocontrol::lockservo() // 锁定舵机
{ {
ESP_LOGD(MOUDLENAME, "start_lockservo"); // ESP_LOGD(MOUDLENAME, "start_lockservo");
addLogMessage("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");
addLogMessage("unlockservo");
// 解锁操作 // 解锁操作
_lockservo->write(SERVO_UNLOCKPOS); _lockservo->write(SERVO_UNLOCKPOS);
_moto_dji.setspeedtarget(-1.0f); // 解锁时逆向慢速转动 _moto_dji.setspeedtarget(-1.0f); // 解锁时逆向慢速转动
@ -155,7 +158,8 @@ 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);
addLogMessage("stoprun after time: " + String(acctime, 2));
_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;
@ -173,7 +177,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));
addLogMessage("Set HS: " + String(gethooktatus_str(false)));
} }
void Motocontrol::setlocked(bool locked) void Motocontrol::setlocked(bool locked)
{ {
@ -217,14 +222,16 @@ 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);
addLogMessage("Not goods wait " + String(HOOK_UNHOOK_TIME) + " ms for unhook");
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);
addLogMessage("HS_Down target is arrived curr: " + String(mf.output_round_cnt, 2));
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);
@ -236,7 +243,8 @@ 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);
addLogMessage("Not havegoods wait " + String(HOOK_UNHOOK_TIME) + " ms unhook");
sethooksstatus(HS_WaitUnhook); sethooksstatus(HS_WaitUnhook);
_tm_waitunhook = millis(); _tm_waitunhook = millis();
} }
@ -247,7 +255,8 @@ 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");
addLogMessage("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);
@ -313,14 +322,16 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
// 检测堵转,没堵转直接关闭电机,否则打开舵机，让电机旋转一点再锁定--最多重复3次 // 检测堵转,没堵转直接关闭电机,否则打开舵机，让电机旋转一点再锁定--最多重复3次
if (!checkservoblock() || (_unblocktimes > 2)) if (!checkservoblock() || (_unblocktimes > 2))
{ {
ESP_LOGD(MOUDLENAME, "Close moto"); // ESP_LOGD(MOUDLENAME, "Close moto");
addLogMessage("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");
addLogMessage("SS servoblock unlock servo and turn moto");
_tm_servotatus = millis(); _tm_servotatus = millis();
_servotatus = SS_WaitUnBlock; _servotatus = SS_WaitUnBlock;
// 写一个不会堵转的位置 // 写一个不会堵转的位置
@ -336,7 +347,8 @@ 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");
addLogMessage("SS lock servo");
// 继续锁定等待舵机检测 // 继续锁定等待舵机检测
_lockservo->write(SERVO_LOCKPOS); _lockservo->write(SERVO_LOCKPOS);
_servotatus = SS_WaitServo; _servotatus = SS_WaitServo;
@ -370,7 +382,8 @@ 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);
addLogMessage("Instorelen: " + String(instlen, 2) + ", speed: " + String(mf.output_speed_rpm, 2));
} }
} }
else else
@ -386,7 +399,8 @@ 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);
addLogMessage("stop down tar: " + String(mf.output_round_cnt, 2) + ", " + String(_target_cnt));
stoprun(1000); // 缓慢停止 stoprun(1000); // 缓慢停止
} }
} }
@ -394,7 +408,8 @@ 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);
addLogMessage("stop up tar: " + String(mf.output_round_cnt, 2) + ", " + String(_target_cnt));
stoprun(1000); // 缓慢停止 stoprun(1000); // 缓慢停止
} }
} }
@ -452,24 +467,28 @@ 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");
addLogMessage("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");
addLogMessage("not lengthzero fault");
return; return;
} }
if (!_weightalign) if (!_weightalign)
{ {
ESP_LOGD(MOUDLENAME, "weight not align fault"); // ESP_LOGD(MOUDLENAME, "weight not align fault");
addLogMessage("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);
addLogMessage("goods min fault: " + String(_pullweight));
return; return;
} }
@ -490,7 +509,8 @@ void Motocontrol::moto_goodsdown(float 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);
addLogMessage("start down " + String(tarleng, 2) + " cm, tar: " + String(_goods_down_target_cnt, 2));
setspeed(SPEED_HOOK_FAST, TM_ACC_HS); setspeed(SPEED_HOOK_FAST, TM_ACC_HS);
_goods_speed = SPEED_HOOK_FAST; _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)) 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; 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");
addLogMessage("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");
addLogMessage("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);
addLogMessage("overweight fault :" + String(_pullweight));
return; return;
} }
sethooksstatus(HS_Up); sethooksstatus(HS_Up);