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成员总数
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: 刷新各请求

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 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);

95
src/main.cpp
View File

@ -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();
}
}

7
src/moto.cpp
View File

@ -1,6 +1,7 @@
#include "moto.h"
#include "Arduino.h"
#include "config.h"
#include "commser.h"
uint8_t CaninBuffer[8]; // 接收指令缓冲区
moto_measure_t moto_chassis;
@ -12,13 +13,15 @@ moto::moto()
}
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);

70
src/motocontrol.cpp
View File

@ -4,6 +4,7 @@
#include "moto.h"
#include <ESP32Servo.h>
#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);