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