PullupDev/src/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(sendinfo, 1000); // 发送状态
 
// 收
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";
// 称重校准状态
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);
 



  // 初始化按钮
  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");
  
  tksendinfo.start();
  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(topicPub, topicPubCount);           //连接mqtt
  fc.mqttClient->setCallback(mqtt_receiveCallback);  //设置订阅成功 回调
  fc.mav_request_data();                             //指定飞控串口返回的数据类别(飞控启动之后发送才有意义)

  /*异步线程*/
  pubTicker.start();          //定时 发布主题
  mavTicker.start();         //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
  flashTicker.start();  //监听 按flash键时 主动发布对频主题
  /////////////////////////////////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");
   _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);
      return true;
     }else _needweightalign = true;
  }
  else
  {
    // 没成功继续校准
    ESP_LOGD(MOUDLENAME,"weight not zero: %d", pullweight);
    _needweightalign = true;
    pullweightoktimes=0;
  }
  return false;
}
// 初始化过程--
// 先收线到顶，确定线的0点位置，再下降2cm开始称重归零（到顶部后有压力，重量不准，需要降一点）,再上升到顶，初始化完成
//
void checkinited()
{
  switch (initstatus)
  {
  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");
      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");
      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);
      scale.set_offset(wei_offset);
      motocontrol.weightalign(true);
      _needweightalign = false;
     // fc.playText("挂钩已锁定");
      initstatus = IS_OK;
    }else
    {
      ESP_LOGD(MOUDLENAME,"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");
      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()
{
  tksendinfo.update();    // 定时发送信息任务
  //pubTicker.update();          //定时 发布主题
  //mavTicker.update();         //定时 指定飞控mavlink 串口返回的数据类别 防止飞控启动滞后
  
 
 // 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();
  /////////////////////////////////MQTT_语音_MAVLINK 部分
  /*从飞控拿数据*/
  fc.comm_receive(mavlink_receiveCallback);
    /*保持mqtt心跳*/
  fc.mqttLoop(topicPub, topicPubCount);
  /////////////////////////////////MQTT_语音_MAVLINK 部分结束
  delay(1);

}
// 在核心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)
  {
    if (_needweightalign)
    {
      _needweightalign = false;
      scale.tare();
    }
    pullweight = get_pullweight();
    //显示重量
    //printf("pullweight: %d \n", pullweight);
    
    /*保持mqtt心跳*/
   // fc.mqttLoop(topicSub, topicSubCount);
   
   ///px1
   // if (fc.checkWiFiStatus())
      /*保持mqtt心跳,如果Mqtt没有连接会自动连接*/
   //   fc.mqttLoop(topicSub, topicSubCount);
    vTaskDelay(10);
  }
}
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");
  //只在上升时停止
  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");
  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);

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

  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");
  _checkweightcal=true;//检测是否需要校准称重传感器--在下放停止时检测
  _checkweighttimes=0;
  down_action(SPEED_BTN_SLOW,10);
}
// 放线按钮--双击
void downbtn_dbclick()
{
  ESP_LOGD(MOUDLENAME,"Down_dbclick");
  down_action(SPEED_BTN_MID);
}
// 放线按钮--长按
void downbtn_pressstart()
{
  ESP_LOGD(MOUDLENAME,"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");
  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");
  up_action(SPEED_BTN_SLOW); 
}
// 收线按钮-双击
void upbtn_dbclick()
{
  ESP_LOGD(MOUDLENAME,"UP_dbclick");
  up_action(SPEED_BTN_MID);
}
// 两个按钮同时按下
void btn_presssatonce()
{
  ESP_LOGD(MOUDLENAME,"UP_presssatonce");
  led_show(255,255, 255); // 高亮一下
  fc.playText("发送对频信息");
  ///px1
  //fc.pubMQTTmsg(topicHandle[0], fc.getMacAdd());
}
// 收线按钮-长按
void upbtn_pressstart()
{
  ESP_LOGD(MOUDLENAME,"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");
  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);
    fc.Camera_action_down();
    motocontrol.moto_goodsdown(length_cm); // 二楼340  //桌子40
  }else
    ESP_LOGE(MOUDLENAME,"autodown fault, not HS_Locked ");
}
void Hook_stop()
{
    ESP_LOGD(MOUDLENAME,"Hook_stop");
    motocontrol.stoprun();
}
void Hook_resume()
{
  if (motocontrol.gethooktatus()==HS_Stop)
  {
    ESP_LOGD(MOUDLENAME,"Hook_resume");
    motocontrol.moto_goodsdownresume();
  }else
    ESP_LOGE(MOUDLENAME,"resume fault, not HS_Stop ");
}
void Hook_recovery()
{
    ESP_LOGD(MOUDLENAME,"Hook_recovery");
    if (motocontrol.gethooktatus()!=HS_Locked)
    {
      motocontrol.stoprun();
      up_action(SPEED_HOOK_UP);
    }
}

// 测试按钮
void testbtn_click()
{
  ESP_LOGD(MOUDLENAME,"testbtn_click");
  switch (motocontrol.gethooktatus())
  {
  case HS_Locked: // 在顶部--会自动检测是否有物体，有才下放
  {
    ESP_LOGD(MOUDLENAME,"moto_goodsdown");
    motocontrol.moto_goodsdown(22); // 二楼340  //桌子40
    break;
  }
  case HS_Stop:
  {
    ESP_LOGD(MOUDLENAME,"moto_resume");

    motocontrol.moto_goodsdownresume();
    break;
  }
  default:
  {
    ESP_LOGD(MOUDLENAME,"stop");
    motocontrol.stoprun();
  }
  }
}

//////////////////////////////MQTT_语音_MAVLINK 部分