848 lines
23 KiB
C++
848 lines
23 KiB
C++
/// 主程序，硬件及线程控制
|
||
/// @file main.cpp
|
||
#include <Arduino.h>
|
||
#include "HX711.h"
|
||
#include "OneButton.h"
|
||
#include "Serialcommand.h"
|
||
#include "config.h"
|
||
#include "Ticker.h" //调用Ticker.h库
|
||
#include <soc/rtc_cntl_reg.h>
|
||
#include "motocontrol.h"
|
||
#include "moto.h"
|
||
#include <FastLED.h>
|
||
#include <ESP32Servo.h>
|
||
#include "FoodDeliveryBase.h"
|
||
#include <Preferences.h>
|
||
#include "commser.h"
|
||
|
||
// 角度传感器
|
||
// 收放线电机控制
|
||
// 控制串口直接使用Serial2，用法和Serial一样，如需要还可以用Serial1，但需要映射引脚
|
||
|
||
//---------------------------------
|
||
OneButton button_up(BTN_UP, true);
|
||
OneButton button_down(BTN_DOWN, true);
|
||
OneButton button_checktop(BTN_CT, true);
|
||
|
||
OneButton button_test(BTN_TEST, true);
|
||
|
||
HX711 scale;
|
||
Serialcommand sercomm;
|
||
Servo myservo;
|
||
#define NUM_LEDS 1
|
||
Preferences preferences;
|
||
long wei_offset;
|
||
|
||
CRGB leds[NUM_LEDS];
|
||
Motocontrol motocontrol;
|
||
void led_show(uint8_t cr, uint8_t cg, uint8_t cb);
|
||
void sendinfo();
|
||
Ticker tksendinfo; // 发送状态
|
||
|
||
// 收
|
||
void upbtn_click();
|
||
void upbtn_dbclick();
|
||
void upbtn_pressstart();
|
||
void upbtn_pressend();
|
||
// 放
|
||
void downbtn_click();
|
||
void downbtn_dbclick();
|
||
void downbtn_pressstart();
|
||
void downbtn_pressend();
|
||
// 顶抬起
|
||
void ctbtn_pressend();
|
||
// 顶按下
|
||
void ctbtn_pressstart();
|
||
|
||
void testbtn_click();
|
||
void btn_presssatonce();
|
||
|
||
int get_pullweight();
|
||
void Task1(void *pvParameters);
|
||
// void led_show(CRGB ledcolor);
|
||
// void Task1code( void *pvParameters );
|
||
void showledidel();
|
||
int pullweight = 0; // 检测重量-克
|
||
int pullweightoktimes = 0; // 校准成功次数
|
||
int8_t btn_pressatonce = 0; // 是否同时按下
|
||
unsigned long _tm_bengstop;
|
||
bool _bengstop = false;
|
||
bool _needweightalign = false;
|
||
|
||
HookStatus _lasthooktatus = HS_UnInit; // 前一个钩子状态
|
||
bool curr_armed = false;
|
||
uint8_t curr_mode = 0;
|
||
bool _checkweightcal = false; // 检测是否要检测称重传感器是否需要校准
|
||
uint8_t _checkweighttimes = 0; //
|
||
unsigned long _tm_checkweigh;
|
||
static const char *MOUDLENAME = "MAIN";
|
||
unsigned long _tm_core1; // 主核心1 循环开始时间
|
||
unsigned long _tm_core0; // 核心0 循环开始时间
|
||
unsigned long _looptm_core1 = 0; // 主核心1单次循环时间--评估是否超时卡顿
|
||
unsigned long _looptm_core0 = 0; // 核心0单次循环时间--评估是否超时卡顿
|
||
|
||
// 称重校准状态
|
||
enum Weightalign_status
|
||
{
|
||
WAS_NotAlign,
|
||
WAS_Aligning,
|
||
WAS_AlignOK,
|
||
WAS_Alignfault
|
||
} _weightalign_status;
|
||
unsigned long _weightalign_begintm; // 校准开始时间
|
||
// 需要做的初始化工作
|
||
|
||
Initstatus initstatus;
|
||
unsigned long _tm_waitinit;
|
||
|
||
void setup()
|
||
{
|
||
WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0); // 关闭低电压检测,避免无限重启
|
||
// 启用Esp32双核第二个核心
|
||
xTaskCreatePinnedToCore(Task1, "Task1", 10000, NULL, 1, NULL, 0); // 最后一个参数至关重要，决定这个任务创建在哪个核上.PRO_CPU 为 0, APP_CPU 为 1,或者 tskNO_AFFINITY 允许任务在两者上运行.
|
||
|
||
// 调试串口
|
||
Serial.begin(57600);
|
||
// 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);
|
||
// addLogMessage("wei_offset: " + String(wei_offset));
|
||
|
||
// 初始化按钮
|
||
button_up.attachClick(upbtn_click);
|
||
button_up.attachDoubleClick(upbtn_dbclick);
|
||
button_up.attachLongPressStart(upbtn_pressstart);
|
||
button_up.attachLongPressStop(upbtn_pressend);
|
||
|
||
button_down.attachDoubleClick(downbtn_dbclick);
|
||
button_down.attachClick(downbtn_click);
|
||
button_down.attachLongPressStart(downbtn_pressstart);
|
||
button_down.attachLongPressStop(downbtn_pressend);
|
||
|
||
button_checktop.setPressTicks(10); // 10毫秒就产生按下事件，用于顶部按钮检测
|
||
button_checktop.attachLongPressStart(ctbtn_pressstart); // 按下马上产生事件，
|
||
button_checktop.attachLongPressStop(ctbtn_pressend); // 抬起
|
||
|
||
button_test.attachClick(testbtn_click);
|
||
|
||
ESP32PWM::allocateTimer(0);
|
||
ESP32PWM::allocateTimer(1);
|
||
ESP32PWM::allocateTimer(2);
|
||
ESP32PWM::allocateTimer(3);
|
||
myservo.setPeriodHertz(50); // standard 50 hz servo
|
||
myservo.attach(SERVO_PIN, 1000, 2000); // attaches the servo on pin 18 to the servo object
|
||
|
||
// 初始化RGB LED 显示黄色灯表示需要注意 LED
|
||
FastLED.addLeds<WS2812, LED_DATA_PIN, RGB>(leds, NUM_LEDS); // GRB ordering is typical
|
||
if (!motocontrol.init(&myservo)) // 初始化电机控制
|
||
// ESP_LOGE(MOUDLENAME, "motocontrol init fault");
|
||
|
||
initstatus = IS_WaitStart;
|
||
_tm_waitinit = millis();
|
||
_needweightalign = (wei_offset == 0);
|
||
|
||
led_show(255, 255, 255); // 连接wifi中
|
||
|
||
/////////////////////////////////MQTT_语音_MAVLINK 部分
|
||
/*初始化*/
|
||
Serial1.begin(115200, SERIAL_8N1, SERIAL_REPORT_RX, SERIAL_REPORT_TX); // 声音模块引串口脚映射
|
||
fc.playText("开始启动");
|
||
fc.connectWifi(); // 连接wifi
|
||
// fc.playText("正在连接服务器");
|
||
// fc.connectMqtt("cmd"); // 连接mqtt
|
||
fc.mqttClient->setCallback(mqtt_receiveCallback); // 设置订阅成功 回调
|
||
fc.mav_request_data(); // 指定飞控串口返回的数据类别(飞控启动之后发送才有意义)
|
||
|
||
/*异步线程，在核心0上回调*/
|
||
tksendinfo.attach(1, sendinfo); // 发送状态
|
||
pubTicker.attach(1, pubThread); // 定时 发布主题
|
||
/////////////////////////////////MQTT_语音_MAVLINK 部分结束
|
||
|
||
// if (motocontrol.getstatus()==MS_Stop)
|
||
// {
|
||
// //slowup();
|
||
|
||
// }
|
||
ESP_LOGI(MOUDLENAME, "PullupDevice is ready!");
|
||
// fc.mav_send_text(MAV_SEVERITY_INFO,"PullupDevice is ready!");
|
||
}
|
||
void slowup()
|
||
{
|
||
// motocontrol.setspeed(SPEED_UP_SLOW);
|
||
// motocontrol.up(0); //一直收线直到到顶部
|
||
}
|
||
void checkstatus()
|
||
{
|
||
HookStatus vhooktatus = motocontrol.gethooktatus();
|
||
// 入仓把云台回中
|
||
if ((_lasthooktatus != vhooktatus) && (vhooktatus == HS_InStore))
|
||
fc.Camera_action_ret();
|
||
|
||
if (_checkweightcal && (vhooktatus == HS_Stop))
|
||
{
|
||
// 第一次进来
|
||
if (_lasthooktatus != vhooktatus)
|
||
_tm_checkweigh = millis();
|
||
else
|
||
{ // 1秒内检测连续大于5次就认为重了
|
||
if ((millis() - _tm_checkweigh) < 1000)
|
||
{
|
||
if (abs(pullweight) > 100)
|
||
_checkweighttimes++;
|
||
else
|
||
_checkweighttimes = 0;
|
||
// ESP_LOGD(MOUDLENAME,"check neet tare %d ",pullweight);
|
||
if (_checkweighttimes > 10)
|
||
{
|
||
_checkweightcal = false;
|
||
// ESP_LOGD(MOUDLENAME,"check neet tare %d ",pullweight);
|
||
fc.playText("检测到有货物请确认，如果是空钩请在pad上校准重量");
|
||
}
|
||
}
|
||
else
|
||
{
|
||
_checkweightcal = false;
|
||
}
|
||
}
|
||
}
|
||
_lasthooktatus = vhooktatus;
|
||
// printf("msend: 1:%.2f,2:%.2f\n",msg_cmd.param1,msg_cmd.param2);
|
||
}
|
||
|
||
// 1秒调用一次了发mqtt到地面
|
||
void showinfo()
|
||
{
|
||
// moto_measure_t mf=motocontrol.getmotoinfo() ;
|
||
// printf("total_cnt:%.3f;speed:%.2f,curr:%.2fA \n ", mf.output_round_cnt, mf.output_speed_rpm,(float)mf.real_current/1000.0f);
|
||
// if (pullweight > 10)
|
||
// printf("PullWeight:%d\n", pullweight); //发送重量到mqtt
|
||
|
||
topicPubMsg[14] = motocontrol.gethooktatus_str();
|
||
topicPubMsg[13] = pullweight;
|
||
|
||
// control_status_t cs=motocontrol.getcontrolstatus() ;
|
||
|
||
// printf("hooktatus:%d motostatus:%d \n ", hs,cs.motostatus);
|
||
HookStatus vhooktatus = motocontrol.gethooktatus();
|
||
mavlink_command_long_t msg_cmd;
|
||
msg_cmd.command = MAV_CMD_FC_HOOK_STATUS;
|
||
msg_cmd.param1 = vhooktatus;
|
||
msg_cmd.param2 = pullweight;
|
||
msg_cmd.target_system = 1;
|
||
msg_cmd.target_component = 1;
|
||
msg_cmd.confirmation = 0;
|
||
fc.mav_send_command(msg_cmd);
|
||
}
|
||
// 校准称重
|
||
void begin_tare()
|
||
{
|
||
// ESP_LOGD(MOUDLENAME, "begin_tare");
|
||
// addLogMessage("begin_tare,校准称重");
|
||
_weightalign_status = WAS_Aligning;
|
||
_needweightalign = true;
|
||
_weightalign_begintm = millis();
|
||
}
|
||
|
||
// 检查校准结果
|
||
bool check_tare()
|
||
{
|
||
if (_weightalign_status != WAS_Aligning)
|
||
return false;
|
||
// 超时失败
|
||
if ((millis() - _weightalign_begintm) > 2000)
|
||
{
|
||
_weightalign_status = WAS_Alignfault;
|
||
return false;
|
||
}
|
||
if ((pullweight < 10) && ((pullweight > -10)))
|
||
{
|
||
pullweightoktimes++;
|
||
if (pullweightoktimes > 20)
|
||
{
|
||
_needweightalign = false;
|
||
_weightalign_status = WAS_AlignOK;
|
||
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);
|
||
// addLogMessage("check_tare ok: " + String(pullweight) + ", offset: " + String(wei_offset));
|
||
return true;
|
||
}
|
||
else
|
||
_needweightalign = true;
|
||
}
|
||
else
|
||
{
|
||
// 没成功继续校准
|
||
// ESP_LOGD(MOUDLENAME, "weight not zero: %d", pullweight);
|
||
// addLogMessage("weight not zero: " + String(pullweight));
|
||
_needweightalign = true;
|
||
pullweightoktimes = 0;
|
||
}
|
||
return false;
|
||
}
|
||
// 初始化过程--
|
||
// 先收线到顶，确定线的0点位置，再下降2cm开始称重归零（到顶部后有压力，重量不准，需要降一点）,再上升到顶，初始化完成
|
||
//
|
||
void checkinited()
|
||
{
|
||
// 根据初始化状态initstatus操作
|
||
switch (initstatus)
|
||
{
|
||
// 刚开机，等待500ms
|
||
case Initstatus::IS_WaitStart:
|
||
{
|
||
if ((millis() - _tm_waitinit) > 500)
|
||
initstatus = IS_Start;
|
||
break;
|
||
}
|
||
// 开始初始化
|
||
case Initstatus::IS_Start:
|
||
{
|
||
// 如果没有在锁定状态，先慢速收购直到锁定
|
||
if (motocontrol.gethooktatus() != HS_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;
|
||
}
|
||
else // 开机就是锁定状态--开始校准称重传感器
|
||
{
|
||
initstatus = IS_begin_WeightZero;
|
||
}
|
||
break;
|
||
}
|
||
case Initstatus::IS_Wait_Locked:
|
||
{
|
||
if (motocontrol.gethooktatus() == HS_Locked)
|
||
{
|
||
// ESP_LOGD(MOUDLENAME, "IS_Wait_LengthZero: is locked");
|
||
addLogMessage("IS_Wait_LengthZero: is locked");
|
||
initstatus = IS_begin_WeightZero;
|
||
}
|
||
break;
|
||
}
|
||
|
||
case Initstatus::IS_begin_WeightZero:
|
||
{
|
||
// 如果没有保存的校准数据就需要校准
|
||
if (wei_offset != 0)
|
||
{
|
||
// 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;
|
||
// fc.playText("挂钩已锁定");
|
||
initstatus = IS_OK;
|
||
}
|
||
else
|
||
{
|
||
// ESP_LOGD(MOUDLENAME, "begin_tare");
|
||
addLogMessage("begin_tare");
|
||
begin_tare();
|
||
initstatus = IS_CheckWeightZero;
|
||
}
|
||
break;
|
||
}
|
||
case Initstatus::IS_CheckWeightZero:
|
||
{
|
||
if (_weightalign_status == WAS_AlignOK)
|
||
{
|
||
// fc.playText("挂钩已锁定");
|
||
initstatus = IS_OK;
|
||
}
|
||
else if (_weightalign_status == WAS_Alignfault)
|
||
{
|
||
// ESP_LOGD(MOUDLENAME, "weightalign fault! again");
|
||
addLogMessage("weightalign fault! again");
|
||
initstatus = IS_begin_WeightZero;
|
||
}
|
||
break;
|
||
}
|
||
|
||
/*
|
||
case Initstatus::IS_LengthZero:
|
||
{
|
||
// 开始自动慢速下降2cm
|
||
printf("IS_LengthZero: Down 2cm \n");
|
||
|
||
motocontrol.setspeed(SPEED_INSTORE);
|
||
motocontrol.moto_run(MotoStatus::MS_Down, 10);
|
||
initstatus = IS_Wait_WeightZero;
|
||
break;
|
||
}
|
||
case Initstatus::IS_Wait_WeightZero:
|
||
{
|
||
if (motocontrol.gethooktatus() == HS_Stop)
|
||
{
|
||
printf("IS_Wait_WeightZero: Down Stop start tare and up \n");
|
||
scale.tare();
|
||
motocontrol.weightalign(true);
|
||
motocontrol.moto_run(MotoStatus::MS_Up);
|
||
initstatus = IS_WeightZero;
|
||
}
|
||
break;
|
||
}
|
||
|
||
case Initstatus::IS_WeightZero:
|
||
{
|
||
if (motocontrol.gethooktatus() == HS_Locked)
|
||
{
|
||
printf("IS_WeightZero: Locked \n");
|
||
initstatus = IS_InStoreLocked;
|
||
}
|
||
break;
|
||
}
|
||
case Initstatus::IS_InStoreLocked:
|
||
{
|
||
printf("IS_InStoreLocked: IS_OK \n");
|
||
initstatus = IS_OK;
|
||
break;
|
||
}
|
||
*/
|
||
}
|
||
}
|
||
void set_locked(bool locked)
|
||
{
|
||
motocontrol.setlocked(locked);
|
||
if (locked)
|
||
fc.playText("挂钩已锁定");
|
||
}
|
||
// 在核心1上执行，重要的延迟低的
|
||
void loop()
|
||
{
|
||
_tm_core1 = millis();
|
||
// sercomm.getcommand(); // 得到控制命令
|
||
button_checktop.tick(); // 按钮
|
||
button_down.tick(); // 按钮
|
||
button_up.tick(); // 按钮
|
||
button_test.tick();
|
||
motocontrol.setweight(pullweight); // 告诉电机拉的重量
|
||
motocontrol.update(); // 电机控制
|
||
|
||
showledidel(); // 显示LED灯光
|
||
checkstatus(); // 检测状态，执行一些和状态有关的功能
|
||
// showinfo(); // 显示一些调试用信息-
|
||
// 到顶后延迟关闭动力电和舵机
|
||
if (_bengstop)
|
||
{
|
||
if ((initstatus == IS_OK) && (pullweight > TM_INSTORE_DELAY_WEIGHT))
|
||
{
|
||
if ((millis() - _tm_bengstop) > TM_INSTORE_WEIGHT_DELAY)
|
||
{
|
||
_bengstop = false;
|
||
set_locked(true);
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if ((millis() - _tm_bengstop) > TM_INSTORE_NOWEIGHT_DELAY)
|
||
{
|
||
_bengstop = false;
|
||
set_locked(true);
|
||
}
|
||
}
|
||
}
|
||
check_tare(); // 检查看是否需要校准称重
|
||
checkinited(); // 检测执行初始化工作
|
||
delay(1);
|
||
_looptm_core1 = millis() - _tm_core1;
|
||
// 如果循环时间超过1000ms,则打印错误日志
|
||
|
||
if (_looptm_core1 > 10)
|
||
{
|
||
// ESP_LOGE(MOUDLENAME, "main loop time out %d ms", _looptm_core1);
|
||
addLogMessage("core1 timeout: " + String(_looptm_core1));
|
||
}
|
||
}
|
||
// 在核心0上执行耗时长的低优先级的
|
||
void Task1(void *pvParameters)
|
||
{
|
||
// 初始化称重传感器
|
||
scale.begin(LOADCELL_DOUT_PIN, LOADCELL_SCK_PIN, HX711_GAIN); // 2号机用的B通道,1号用的A通道
|
||
scale.set_scale(WEIGHT_SCALE); // 这是缩放值，根据砝码实测516.f
|
||
scale.tare(); // 重置为0
|
||
|
||
// 在这里可以添加一些代码，这样的话这个任务执行时会先执行一次这里的内容（当然后面进入while循环之后不会再执行这部分了）
|
||
while (1)
|
||
{
|
||
_tm_core0 = millis();
|
||
/*重量*/
|
||
if (_needweightalign)
|
||
{
|
||
_needweightalign = false;
|
||
scale.tare();
|
||
}
|
||
|
||
pullweight = get_pullweight();
|
||
|
||
/*从飞控拿数据*/
|
||
fc.comm_receive(mavlink_receiveCallback);
|
||
// vTaskDelay(1);
|
||
if (fc.checkWiFiStatus())
|
||
fc.mqttLoop("cmd"); // 保持mqtt心跳,如果Mqtt没有连接会自动连接
|
||
vTaskDelay(10);
|
||
|
||
_looptm_core0 = millis() - _tm_core0;
|
||
/*如果循环时间超过100ms,则打印错误日志*/
|
||
if (_looptm_core0 > 200)
|
||
{
|
||
//addLogMessage("core0 timeout: " + String(_looptm_core0));
|
||
}
|
||
}
|
||
}
|
||
void sendinfo() // 每500ms发送状态信息
|
||
{
|
||
// sercomm.sendinfo(motocontrol.gethooktatus(), 5324, pullweight);
|
||
showinfo();
|
||
}
|
||
|
||
float Value;
|
||
#define FILTER_A 0.5
|
||
// 低通滤波和限幅后的拉力数值,单位:克
|
||
int get_pullweight()
|
||
{
|
||
float NewValue;
|
||
if (scale.wait_ready_timeout(100)) // 等待数据ok,100ms超时
|
||
NewValue = scale.get_units();
|
||
else
|
||
NewValue = 0;
|
||
Value = NewValue * FILTER_A + (1.0 - FILTER_A) * Value; // 低通滤波
|
||
Value = constrain(Value, -10000, 10000); // 限制到0-10公斤
|
||
return round(Value);
|
||
}
|
||
// 提示灯光控制
|
||
void led_show(uint8_t cr, uint8_t cg, uint8_t cb)
|
||
{
|
||
leds[0] = CRGB(cg, cr, cb); // 颜色交叉了
|
||
FastLED.show();
|
||
}
|
||
// 显示颜色
|
||
void showledidel()
|
||
{
|
||
// 超重--红色
|
||
if (motocontrol.getcontrolstatus().is_overweight)
|
||
{
|
||
led_show(255, 0, 255); // 紫色
|
||
return;
|
||
}
|
||
switch (motocontrol.gethooktatus())
|
||
{
|
||
case HookStatus::HS_UnInit:
|
||
{
|
||
led_show(255, 255, 0); // 黄色
|
||
break;
|
||
}
|
||
case HookStatus::HS_Down:
|
||
case HookStatus::HS_DownSlow:
|
||
case HookStatus::HS_WaitUnhook:
|
||
{
|
||
led_show(0, 0, 255); // 蓝色
|
||
break;
|
||
}
|
||
case HookStatus::HS_Up:
|
||
{
|
||
led_show(200, 0, 50); // 粉红
|
||
break;
|
||
}
|
||
case HookStatus::HS_InStore:
|
||
{
|
||
led_show(255, 0, 0); // 红
|
||
break;
|
||
}
|
||
|
||
case HookStatus::HS_Locked:
|
||
{
|
||
led_show(0, 255, 0); // 绿色
|
||
break;
|
||
}
|
||
case HookStatus::HS_Stop:
|
||
{
|
||
led_show(255, 255, 255); // 白色
|
||
break;
|
||
}
|
||
}
|
||
/*
|
||
|
||
switch (motocontrol.getcontrolstatus().motostatus)
|
||
{
|
||
case MotoStatus::MS_Down:
|
||
{
|
||
led_show(0, 0, 255); // 蓝色
|
||
break;
|
||
}
|
||
case MotoStatus::MS_Up:
|
||
{
|
||
|
||
if (motocontrol.getcontrolstatus().is_instore)
|
||
led_show(255, 0, 0); // 红
|
||
else
|
||
led_show(200, 0, 50); // 粉红
|
||
break;
|
||
}
|
||
|
||
case MotoStatus::MS_Stop:
|
||
{
|
||
if (motocontrol.getcontrolstatus().is_toplocked)
|
||
{
|
||
if (initstatus == IS_OK)
|
||
led_show(0, 255, 0); // 绿色
|
||
else
|
||
{
|
||
// Serial.println("not IS_OK");
|
||
led_show(255, 255, 0); // 黄色
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if (motocontrol.getcontrolstatus().is_setzero)
|
||
led_show(255, 255, 255); // 白色
|
||
else
|
||
{
|
||
// Serial.println("not is_setzero");
|
||
led_show(255, 255, 0); // 黄色
|
||
}
|
||
}
|
||
break;
|
||
}
|
||
}
|
||
*/
|
||
}
|
||
|
||
/////////////////////////////////////按钮处理
|
||
// 顶部按钮，检测是否到顶部
|
||
void ctbtn_pressstart()
|
||
{
|
||
// ESP_LOGD(MOUDLENAME, "Top_pressstart");
|
||
addLogMessage("Top_pressstart");
|
||
// 只在上升时停止
|
||
if ((motocontrol.gethooktatus() == HS_UnInit) || (motocontrol.gethooktatus() == HS_Up) || (motocontrol.gethooktatus() == HS_InStore))
|
||
{
|
||
_tm_bengstop = millis();
|
||
_bengstop = true;
|
||
}
|
||
}
|
||
// 顶部按钮，抬起
|
||
void ctbtn_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);
|
||
addLogMessage("Down_action sp:" + String(motospeed, 2) + " len:" + String(length, 2) + " cm");
|
||
if (motocontrol.getcontrolstatus().motostatus != MotoStatus::MS_Stop)
|
||
{
|
||
motocontrol.stoprun();
|
||
}
|
||
else
|
||
{
|
||
// 如果在自动放线状态，只是恢复自动放线
|
||
if (motocontrol.getcontrolstatus().is_autogoodsdown)
|
||
{
|
||
motocontrol.moto_goodsdownresume();
|
||
}
|
||
else
|
||
{
|
||
motocontrol.setspeed(motospeed);
|
||
motocontrol.moto_run(MotoStatus::MS_Down, length);
|
||
fc.playText("放线");
|
||
}
|
||
}
|
||
}
|
||
|
||
void up_action(float motospeed)
|
||
{
|
||
// ESP_LOGD(MOUDLENAME, "Up_action sp:%.2f", motospeed);
|
||
addLogMessage("Up_action sp:" + String(motospeed, 2));
|
||
|
||
if (motocontrol.getcontrolstatus().motostatus == MotoStatus::MS_Stop)
|
||
{
|
||
motocontrol.stopautodown(); // 终止自动放线
|
||
motocontrol.setspeed(motospeed);
|
||
motocontrol.moto_run(MotoStatus::MS_Up);
|
||
fc.playText("收线");
|
||
}
|
||
else
|
||
motocontrol.stoprun();
|
||
}
|
||
// 放线按钮--单击
|
||
void downbtn_click()
|
||
{
|
||
// ESP_LOGD(MOUDLENAME, "Down_click");
|
||
addLogMessage("Down_click");
|
||
_checkweightcal = true; // 检测是否需要校准称重传感器--在下放停止时检测
|
||
_checkweighttimes = 0;
|
||
down_action(SPEED_BTN_SLOW, 10);
|
||
}
|
||
// 放线按钮--双击
|
||
void downbtn_dbclick()
|
||
{
|
||
// ESP_LOGD(MOUDLENAME, "Down_dbclick");
|
||
addLogMessage("Down_dbclick");
|
||
down_action(SPEED_BTN_MID);
|
||
}
|
||
// 放线按钮--长按
|
||
void downbtn_pressstart()
|
||
{
|
||
// ESP_LOGD(MOUDLENAME, "Down_pressstart");
|
||
addLogMessage("Down_pressstart");
|
||
// 两个同时按用于对频
|
||
btn_pressatonce++;
|
||
if (btn_pressatonce > 2)
|
||
btn_pressatonce = 2;
|
||
if (btn_pressatonce == 2)
|
||
{
|
||
btn_presssatonce();
|
||
return;
|
||
}
|
||
down_action(SPEED_BTN_FAST);
|
||
}
|
||
// 放线按钮--长按抬起
|
||
void downbtn_pressend()
|
||
{
|
||
// ESP_LOGD(MOUDLENAME, "Down_pressend");
|
||
addLogMessage("Down_pressend");
|
||
btn_pressatonce--;
|
||
if (btn_pressatonce < 0)
|
||
btn_pressatonce = 0;
|
||
// 不是恢复自动放线抬起按键就停止
|
||
if (!motocontrol.getcontrolstatus().is_autogoodsdown)
|
||
motocontrol.stoprun();
|
||
}
|
||
|
||
// 收线按钮-单击
|
||
void upbtn_click()
|
||
{
|
||
// fc.mav_send_text(MAV_SEVERITY_INFO,"upbtn_click");
|
||
// ESP_LOGD(MOUDLENAME, "UP_click");
|
||
addLogMessage("UP_click");
|
||
up_action(SPEED_BTN_SLOW);
|
||
}
|
||
// 收线按钮-双击
|
||
void upbtn_dbclick()
|
||
{
|
||
// ESP_LOGD(MOUDLENAME, "UP_dbclick");
|
||
addLogMessage("UP_dbclick");
|
||
up_action(SPEED_BTN_MID);
|
||
}
|
||
// 两个按钮同时按下
|
||
void btn_presssatonce()
|
||
{
|
||
// ESP_LOGD(MOUDLENAME, "UP_presssatonce");
|
||
addLogMessage("UP_presssatonce");
|
||
led_show(255, 255, 255); // 高亮一下
|
||
fc.playText("发送对频信息");
|
||
fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd());
|
||
}
|
||
// 收线按钮-长按
|
||
void upbtn_pressstart()
|
||
{
|
||
// ESP_LOGD(MOUDLENAME, "UP_pressstart");
|
||
addLogMessage("UP_pressstart");
|
||
// 两个同时按用于对频
|
||
btn_pressatonce++;
|
||
if (btn_pressatonce > 2)
|
||
btn_pressatonce = 2;
|
||
if (btn_pressatonce == 2)
|
||
{
|
||
btn_presssatonce();
|
||
return;
|
||
}
|
||
up_action(SPEED_BTN_FAST);
|
||
}
|
||
// 收线按钮-长按抬起
|
||
void upbtn_pressend()
|
||
{
|
||
// ESP_LOGD(MOUDLENAME, "UP_pressend");
|
||
addLogMessage("UP_pressend");
|
||
btn_pressatonce--;
|
||
if (btn_pressatonce < 0)
|
||
btn_pressatonce = 0;
|
||
|
||
motocontrol.stoprun();
|
||
}
|
||
|
||
// 自动下放
|
||
void Hook_autodown(float length_cm)
|
||
{
|
||
if (motocontrol.gethooktatus() == HS_Locked)
|
||
{
|
||
// 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
|
||
}
|
||
else
|
||
addLogMessage("autodown fault, not HS_Locked");
|
||
// ESP_LOGE(MOUDLENAME, "autodown fault, not HS_Locked ");
|
||
}
|
||
void 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");
|
||
addLogMessage("Hook_resume");
|
||
motocontrol.moto_goodsdownresume();
|
||
}
|
||
else
|
||
// ESP_LOGE(MOUDLENAME, "resume fault, not HS_Stop ");
|
||
addLogMessage("resume fault, not HS_Stop ");
|
||
}
|
||
void Hook_recovery()
|
||
{
|
||
// ESP_LOGD(MOUDLENAME, "Hook_recovery");
|
||
addLogMessage("Hook_recovery");
|
||
if (motocontrol.gethooktatus() != HS_Locked)
|
||
{
|
||
motocontrol.stoprun();
|
||
up_action(SPEED_HOOK_UP);
|
||
}
|
||
}
|
||
|
||
// 测试按钮
|
||
void testbtn_click()
|
||
{
|
||
// ESP_LOGD(MOUDLENAME, "testbtn_click");
|
||
addLogMessage("testbtn_click");
|
||
switch (motocontrol.gethooktatus())
|
||
{
|
||
case HS_Locked: // 在顶部--会自动检测是否有物体，有才下放
|
||
{
|
||
// ESP_LOGD(MOUDLENAME, "moto_goodsdown");
|
||
addLogMessage("moto_goodsdown");
|
||
motocontrol.moto_goodsdown(22); // 二楼340 //桌子40
|
||
break;
|
||
}
|
||
case HS_Stop:
|
||
{
|
||
// ESP_LOGD(MOUDLENAME, "moto_resume");
|
||
addLogMessage("moto_resume");
|
||
|
||
motocontrol.moto_goodsdownresume();
|
||
break;
|
||
}
|
||
default:
|
||
{
|
||
// ESP_LOGD(MOUDLENAME, "stop");
|
||
addLogMessage("stop");
|
||
motocontrol.stoprun();
|
||
}
|
||
}
|
||
}
|