修改自动放物体的bug，整理代码

src/config.h

@@ -10,22 +10,25 @@
 #define BTN_DOWN 22 // 放线开关
 #define BTN_CT 21   // 到顶检测开关
 #define BTN_TEST 18   // 测试开关
-// LED
-#define LED_TIP 19 // 指示灯     接线：LED_TIP---3.3V
 // 称重传感器- HX711
-#define LOADCELL_DOUT_PIN 2
+#define LOADCELL_DOUT_PIN  16
-#define LOADCELL_SCK_PIN 4
+#define LOADCELL_SCK_PIN  17
 ///////////////////////////////////////////
 #define SPEED_UP_SLOW 4   //rmp 或者  弧度/秒
 #define SERVO_PIN   14      //锁定舵机PWM控制脚
-
+////LED
+#define LED_DATA_PIN 25
+/////
+#define WEIGHT_SCALE 276   //这是缩放值，根据砝码实测516.f
+#define TM_INSTORE_DELAY 200 // 入仓动力延时关闭时间  ms
 enum HookStatus
 {
   HS_UnInit, //还未初始化
   HS_Down,   // 货物下放中
-  HS_DownSlow,  //货物慢速下放中
+  HS_DownSlow,  //货物慢速下放中  --接近地面慢速，不受设置速度影响
+  HS_WaitUnhook,  //等待脱钩
   HS_Up,     // 回收中
-  HS_InStore,     // 入仓中
+  HS_InStore,     // 入仓中     --慢速，不受设置速度影响
   HS_Locked,  //已到顶部锁定
   HS_Stop   //已停止
 };

src/main.cpp

@@ -9,7 +9,7 @@
 #include "moto.h"
 #include <FastLED.h>
 #include <ESP32Servo.h>
-#define TM_INSTORE_DELAY 100 // 入仓延时关闭时间  ms
+
 
 // 角度传感器
 // 收放线电机控制
@@ -26,7 +26,7 @@ HX711 scale;
 Serialcommand sercomm;
 Servo myservo;
 #define NUM_LEDS 1
-#define DATA_PIN 25
+
 CRGB leds[NUM_LEDS];
 Motocontrol motocontrol;
 
@@ -58,20 +58,22 @@ void showledidel();
 int pullweight = 0;
 unsigned long _tm_bengstop;
 bool _bengstop = false;
-
+bool _needweightalign = false;
 // 需要做的初始化工作
 enum Initstatus
 {
+  IS_WaitStart, // 0. 刚上电,等待初始化--顶部按钮按下有延时，需要等一下看看这个按钮是否按下以确定是否需要收线
   IS_Start,     // 0. 刚上电
-  IS_Wait_LengthZero,
+  IS_Wait_Locked,
   IS_LengthZero, // 1.已确定零点
   IS_Wait_WeightZero,
   IS_WeightZero,      // 2.已确定称重传感器0点
   IS_InStoreLocked,   // 3. 挂钩入仓顶锁定
+  IS_CheckWeightZero, // 检查称重传感器是否归0
   IS_OK,              // 4.所有初始化已经OK，可以正常操作
   IS_Error            // 5.初始化遇到错误
 } initstatus;
-
+unsigned long _tm_waitinit;
 void setup()
 {
   WRITE_PERI_REG(RTC_CNTL_BROWN_OUT_REG, 0); // 关闭低电压检测,避免无限重启
@@ -107,13 +109,15 @@ void setup()
   myservo.attach(SERVO_PIN, 1000, 2000); // attaches the servo on pin 18 to the servo object
 
   // 初始化RGB  LED   显示黄色灯表示需要注意 LED
-  FastLED.addLeds<WS2812, DATA_PIN, RGB>(leds, NUM_LEDS); // GRB ordering is typical
+  FastLED.addLeds<WS2812, LED_DATA_PIN, RGB>(leds, NUM_LEDS); // GRB ordering is typical
   if (!motocontrol.init(&myservo))                        // 初始化电机控制
     printf("motocontrol init fault\n");
 
   // 发送状态任务开启--500ms一次
   tksendinfo.start();
-  initstatus = IS_Start;
+  initstatus = IS_WaitStart;
+  _tm_waitinit = millis();
+  _needweightalign = true;
   // if (motocontrol.getstatus()==MS_Stop)
   // {
   //     //slowup();
@@ -131,8 +135,15 @@ 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)
+  // if (pullweight > 10)
   // printf("PullWeight:%d\n", pullweight);
+
+ // HookStatus hs=motocontrol.gethooktatus() ;
+  //control_status_t cs=motocontrol.getcontrolstatus() ;
+
+  // printf("hooktatus:%d  motostatus:%d \n ", hs,cs.motostatus);
+
+
 }
 // 初始化过程--
 // 先收线到顶，确定线的0点位置，再下降2cm开始称重归零（到顶部后有压力，重量不准，需要降一点）,再上升到顶，初始化完成
@@ -141,25 +152,64 @@ 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)
     {
       // 开始自动慢速上升，直到顶部按钮按下
       printf("IS_Start: startup wait locked\n");
       motocontrol.setspeed(SPEED_BTN_SLOW);
       motocontrol.moto_run(MotoStatus::MS_Up);
-    initstatus = IS_Wait_LengthZero;
+      initstatus = IS_Wait_Locked;
+    }
+    else
+    {
+      _needweightalign = true;
+      // scale.tare(10);
+      // motocontrol.weightalign(true);
+      initstatus = IS_CheckWeightZero;
+    }
     break;
   }
-  case Initstatus::IS_Wait_LengthZero:
+  case Initstatus::IS_Wait_Locked:
   {
     if (motocontrol.gethooktatus() == HS_Locked)
     {
       printf("IS_Wait_LengthZero: is locked\n");
-      initstatus = IS_LengthZero;
+      _needweightalign = true;
+      // scale.tare(10);
+      // motocontrol.weightalign(true);
+      initstatus = IS_CheckWeightZero;
+      // initstatus = IS_LengthZero;
     }
     break;
   }
 
+  case Initstatus::IS_CheckWeightZero:
+  {
+    // 重量校准成功
+    if (pullweight < 10)
+    {
+      motocontrol.weightalign(true);
+      initstatus = IS_OK;
+    }
+    else
+    {
+      // 没成功继续校准
+      printf("pullweight fault: %d \n", pullweight);
+      _needweightalign = true;
+      // scale.tare(10);
+    }
+    break;
+  }
+    /*
       case Initstatus::IS_LengthZero:
       {
         // 开始自动慢速下降2cm
@@ -198,6 +248,7 @@ void checkinited()
         initstatus = IS_OK;
         break;
       }
+      */
   }
 }
 // 在核心1上执行，重要的延迟低的
@@ -228,12 +279,17 @@ void Task1(void *pvParameters)
 {
   // 初始化称重传感器
   scale.begin(LOADCELL_DOUT_PIN, LOADCELL_SCK_PIN);
-  scale.set_scale(963.f); // 这是缩放值，根据砝码实测516.f
+  scale.set_scale(WEIGHT_SCALE); // 这是缩放值，根据砝码实测516.f
   scale.tare();                  // 重置为0
 
   // 在这里可以添加一些代码，这样的话这个任务执行时会先执行一次这里的内容（当然后面进入while循环之后不会再执行这部分了）
   while (1)
   {
+    if (_needweightalign)
+    {
+      _needweightalign = false;
+      scale.tare();
+    }
     pullweight = get_pullweight();
     vTaskDelay(10);
   }
@@ -266,21 +322,26 @@ void led_show(uint8_t cr, uint8_t cg, uint8_t cb)
 // 显示颜色
 void showledidel()
 {
+  // 超重--红色
+  if (motocontrol.getcontrolstatus().is_overweight)
+  {
+    led_show(255, 0, 0); // 红色
+    return;
+  }
   switch (motocontrol.getcontrolstatus().motostatus)
   {
   case MotoStatus::MS_Down:
   {
-    led_show(0, 0, 255); // blue
+    led_show(0, 0, 255); // 蓝色
-
     break;
   }
   case MotoStatus::MS_Up:
   {
 
     if (motocontrol.getcontrolstatus().is_instore)
-      led_show(255, 0, 0); // red
+      led_show(255, 0, 0); // 红
     else
-      led_show(200, 0, 50); // red
+      led_show(200, 0, 50); // 粉红
     break;
   }
 
@@ -288,22 +349,27 @@ void showledidel()
   {
     if (motocontrol.getcontrolstatus().is_toplocked)
     {
-      led_show(0, 255, 0); // green
+      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); // 紫色
+        led_show(255, 255, 255); // 白色
       else
-        led_show(255, 255, 0); // yellow
+      {
+        Serial.println("not is_setzero");
+        led_show(255, 255, 0); // 黄色
+      }
     }
     break;
   }
   }
-
-  //超重--红色
-  if (motocontrol.getcontrolstatus().is_overweight)
-    led_show(255, 0, 0); // red
 }
 
 /////////////////////////////////////按钮处理
@@ -427,5 +493,5 @@ void upbtn_pressend()
 void testbtn_click()
 {
   Serial.println("testbtn_click");
-  motocontrol.moto_goodsdown();
+  motocontrol.moto_goodsdown(40);
 }

src/motocontrol.cpp

@@ -21,7 +21,8 @@ Motocontrol::Motocontrol()
     _hooksstatus = HS_UnInit;
     _weightalign = false;
     _overweightcount = 0;
-    _autogoodsdown=false;
+    _notweightcount = 0;
+    _autogoodsdown = false;
 }
 Motocontrol::~Motocontrol()
 {
@@ -40,19 +41,21 @@ void Motocontrol::setspeed(float motospeed) // 设置速度
 void Motocontrol::setweight(int pullweight) // 设置重量
 {
     _pullweight = pullweight;
-    checkoverweight();
+    checkgoods();
 }
 
-void Motocontrol::checkoverweight() // 检测是否超重
+void Motocontrol::checkgoods() // 检测是否超重
 {
     // 到顶部锁定状态，有向上的压力，重量不准，不能检测
     if (_controlstatus.is_toplocked)
         return;
+
+    // 检查是否超重
     if (_pullweight > HOOK_WEIHT_MAX)
     {
         // 防止毛刺
         _overweightcount++;
-        if (_overweightcount > 5)
+        if (_overweightcount > 40)
             _controlstatus.is_overweight = true;
     }
     else
@@ -60,6 +63,23 @@ void Motocontrol::checkoverweight() // 检测是否超重
         _controlstatus.is_overweight = false;
         _overweightcount = 0;
     }
+
+    // 检查是否有货物
+    if (_pullweight < HOOK_WEIHT_MIN)
+    {
+        // 防止毛刺
+        _notweightcount++;
+        if (_notweightcount > 40)
+        {
+            // printf("goods weight<min 40 times :%d \n", _pullweight);
+            _controlstatus.is_havegoods = false;
+        }
+    }
+    else
+    {
+        _controlstatus.is_havegoods = true;
+        _notweightcount = 0;
+    }
 }
 void Motocontrol::stop() // 停止
 {
@@ -78,9 +98,10 @@ void Motocontrol::setlocked(bool locked)
     {
         stop();
         setzero();
+        _hooksstatus = HS_Locked;
+        _autogoodsdown = false;
     }
     _controlstatus.is_toplocked = locked;
-    _hooksstatus = HS_Locked;
 }
 void Motocontrol::setzero() // 设置0长度位置
 {
@@ -92,19 +113,12 @@ int16_t Motocontrol::getlength() // 得到长度
 {
     return 0;
 }
-// 检测是否挂有货物
-bool Motocontrol::havegoods()
-{
-    return _pullweight > HOOK_WEIHT_MIN;
-}
 
 // 检测是否挂有货物
 void Motocontrol::goodsstartup()
 {
-    moto_run(MS_Stop);
+    _hooksstatus = HS_WaitUnhook;
-    setspeed(SPEED_HOOK_UP);
+    _tm_waitunhook = millis();
-    moto_run(MS_Up);
-    _hooksstatus = HS_Up;
 }
 // 重量传感器已经校准
 void Motocontrol::weightalign(bool weightalign)
@@ -113,8 +127,9 @@ void Motocontrol::weightalign(bool weightalign)
 }
 void Motocontrol::checkhookstatus() // 检查挂钩状态
 {
-    //需要在自动放货物中
+    // 需要在自动放货物中
-    if (!_autogoodsdown) return;
+    if (!_autogoodsdown)
+        return;
 
     _hook_currlen = _curr_length;
     switch (_hooksstatus)
@@ -123,17 +138,25 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
     case HookStatus::HS_Down:
     {
         // 如果没有货物--开始回收
-        if (!havegoods())
+        if (!_controlstatus.is_havegoods)
         {
+            printf("HS_Down not havegoods wait unhook \n");
             goodsstartup();
             break;
         }
+
+        break;
+    }
+    case HookStatus::HS_Stop:
+    {
         // 如果有长度目标
-        if (_hook_targetlen > 0)
+        if ((_hook_targetlen > 0) && (_hook_targetlen <= HOOK_SLOWDOWN_LENGTH))
         {
+            printf("_hook_targetlen>0 \n");
             // 如果停了就开始慢速
-            if (_controlstatus.motostatus == MS_Stop)
+
             {
+                printf("begin hook slow down \n");
                 // 慢速一直下放，直到货物卸下--没有重量
                 setspeed(SPEED_HOOK_SLOW);
                 moto_run(MS_Down);
@@ -143,33 +166,45 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
         else
         {
             // 没有长度目标,如果停了并且货物还在，一般是线到最大了，所有开始回收
-            if (_controlstatus.motostatus == MS_Stop)
+
+            {
+                printf("not settargetlen arrived wait unhook \n");
                 goodsstartup();
             }
+        }
 
         break;
     }
+
     case HookStatus::HS_DownSlow:
     {
-        if (!havegoods())
+        if (!_controlstatus.is_havegoods)
         {
+            printf("HS_DownSlow not havegoods wait unhook \n");
             goodsstartup();
         }
         break;
     }
+    case HookStatus::HS_WaitUnhook:
+    {
+        if ((millis() - _tm_waitunhook) > HOOK_UNHOOK_TIME)
+        {
+            printf("HS_WaitUnhook ok startup \n");
+            moto_run(MS_Stop);
+            setspeed(SPEED_HOOK_UP);
+            moto_run(MS_Up);
+        }
+        break;
+    }
+
     case HookStatus::HS_Up:
     {
-        if (_controlstatus.is_instore)
+        // 会自动设置HS_InStore状态
-            _hooksstatus = HS_InStore;
         break;
     }
     case HookStatus::HS_InStore:
     {
-        if (_controlstatus.is_toplocked)
+        // 如果到顶部，会触发setlocked,不用在这里设置
-        {
-            _autogoodsdown=false;
-            _hooksstatus = HS_Locked;
-        }
         break;
     }
     }
@@ -193,7 +228,12 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
         if ((_curr_length < INSTORE_LENGTH) && (_controlstatus.motostatus == MotoStatus::MS_Up))
         {
             _moto_dji.settarget(-SPEED_INSTORE);
+            if (!_controlstatus.is_instore)
+            {
                 _controlstatus.is_instore = true;
+                _hooksstatus = HS_InStore;
+                printf("begin instore \n");
+            }
         }
         else
             _controlstatus.is_instore = false;
@@ -241,38 +281,52 @@ control_status_t Motocontrol::getcontrolstatus() // 得到控制信息
 
 void Motocontrol::moto_goodsdown(float length)
 {
+    if (length < 0.0)
+    {
+        printf("length<0 fault\n");
+        return;
+    }
     // 没设置零点
     if (!_controlstatus.is_setzero)
     {
         printf("not lengthzero fault\n");
         return;
     }
-    if (_weightalign)
+    if (!_weightalign)
     {
         printf("weight not align fault\n");
         return;
     }
-    // 没挂东西也没设置下放长度
+    // 没挂东西
-    if (!havegoods() && (length == 0.0))
+    if (!_controlstatus.is_havegoods )
     {
-        printf("goods min fault:%d ,len:%.2f\n", _pullweight, length);
+        printf("goods min fault:%d \n", _pullweight);
         return;
     }
 
-    //开始自动放货物状态
+    // 开始自动放货物状态
-    _autogoodsdown=true;
+    _autogoodsdown = true;
 
     _hook_targetlen = length;
+    // 长度=0，直接中慢速下降，
     if (length == 0.0)
+    {
         setspeed(SPEED_HOOK_CHECK);
-    else
-        setspeed(SPEED_HOOK_FAST);
-    if (length > 0)
-        moto_run(MS_Down, length - HOOK_SLOWDOWN_LENGTH);
-    else
         moto_run(MS_Down);
-    _hooksstatus = HS_Down;
+    }
+    else if (length > HOOK_SLOWDOWN_LENGTH) // 长度大于慢速长度
+    {
+        setspeed(SPEED_HOOK_FAST);
+        moto_run(MS_Down, length - HOOK_SLOWDOWN_LENGTH);
+        _hook_targetlen = HOOK_SLOWDOWN_LENGTH;
+    }
+    else
+    {
+        setspeed(SPEED_HOOK_SLOW); // 长度小于慢速长度
+        moto_run(MS_Down, length);
+    }
 }
+
 // 按指定速度收放线 放线为+，收线为- 单位cm
 void Motocontrol::moto_run(MotoStatus updown, float length)
 {
@@ -297,12 +351,20 @@ void Motocontrol::moto_run(MotoStatus updown, float length)
     }
     if (updown == MS_Up)
     {
+        if (_controlstatus.is_toplocked)
+        {
+            printf("is_toplocked return\n");
+            return;
+        }
         if (_controlstatus.is_overweight)
         {
             printf("overweight fault :%d \n", _pullweight);
             return;
         }
+        _hooksstatus = HS_Up;
     }
+    else
+        _hooksstatus = HS_Down;
 
     _lockservo->write(SERVO_UNLOCKPOS);
     _need_closemoto = false;

src/motocontrol.h

@@ -17,21 +17,22 @@
 #define SPEED_BTN_MID 100   //按键中等收放线速度
 #define SPEED_BTN_FAST 200   //按键最快收放线速度
 
-#define SPEED_HOOK_FAST 200         //货物下放速度--有目标长度得情况下
+#define SPEED_HOOK_FAST 100         //货物下放速度--有目标长度得情况下
 #define SPEED_HOOK_SLOW 20   //货物快到地面速度--有目标长度得情况下
 #define SPEED_HOOK_CHECK 50   //货物下放速度--没有目标长度得情况下能检测到脱钩的合适速度
 #define SPEED_HOOK_UP 50     //空钩上升速度  
 
 
 
-#define SERVO_LOCKPOS  1900    //舵机锁定位置
+#define SERVO_LOCKPOS  1820    //舵机锁定位置
-#define SERVO_UNLOCKPOS  1500   //舵机解锁位置
+#define SERVO_UNLOCKPOS  1600   //舵机解锁位置
 
-#define HOOK_WEIHT_MIN  10   //最小货物重量 小于这个认为没挂东西  (克)
+#define HOOK_WEIHT_MIN  100   //最小货物重量 小于这个认为没挂东西  (克)
 
-#define HOOK_WEIHT_MAX  5000   //最大货物重量 大于这个认为超重不工作  (克)
+#define HOOK_WEIHT_MAX  6000   //最大货物重量 大于这个认为超重不工作  (克)
 
-#define HOOK_SLOWDOWN_LENGTH  50   //下放物体时快到目标长度时慢速长度  (cm)
+#define HOOK_SLOWDOWN_LENGTH  10   //下放物体时快到目标长度时慢速长度  (cm)
+#define HOOK_UNHOOK_TIME  1000       //等待脱钩时间ms
 
 // 挂钩状态
 enum MotoStatus
@@ -47,6 +48,7 @@ typedef struct
   bool is_setzero;        //已设置0长度位置
   bool is_toplocked;      //已到顶锁住
   bool is_overweight;     //是否超重
+  bool is_havegoods;      //是否有货物
   float zero_cnt;         //0长度位置
   float speed_targe;      //当前目标速度
   MotoStatus motostatus;  //电机运行状态
@@ -75,12 +77,16 @@ private:
   float _curr_length;
   bool _weightalign;
   bool _autogoodsdown;
-  u_int8_t _overweightcount;
+  uint8_t _overweightcount;
+
+  uint8_t _notweightcount;
+
+  unsigned long  _tm_waitunhook;
+
   void checkmotostatus();
   void checkhookstatus();
-  bool havegoods();
   void goodsstartup() ;
-  void checkoverweight();
+  void checkgoods();
 public:
     Motocontrol();                  // 构造函数
     ~Motocontrol();                 // 析构函数