加入自动放线可暂停和恢复功能

微调参数

src/main.cpp

@@ -493,5 +493,25 @@ void upbtn_pressend()
 void testbtn_click()
 {
   Serial.println("testbtn_click");
-  motocontrol.moto_goodsdown(22);  //二楼340  //桌子40
+  switch (motocontrol.gethooktatus())
+  {
+  case HS_Locked: // 在顶部--会自动检测是否有物体，有才下放
+  {
+    Serial.println("moto_goodsdown");
+    motocontrol.moto_goodsdown(22); // 二楼340  //桌子40
+    break;
+  }
+  case HS_Stop:
+  {
+    Serial.println("moto_resume");
+
+    motocontrol.moto_goodsdownresume();
+    break;
+  }
+  default:
+  {
+    Serial.println("stop");
+    motocontrol.stop();
+  }
+  }
 }

src/motocontrol.cpp

@@ -109,7 +109,11 @@ void Motocontrol::stop(float acctime) // 停止
     _moto_dji.settime_acc(acctime);
     _moto_dji.setspeedtarget(0.0f);
     _controlstatus.motostatus = MS_Stop;
-    //_hooksstatus = HS_Stop;
+    if ((_hooksstatus != HS_UnInit) || (_hooksstatus != HS_Stop))
+    {
+        _hooksstatus_prv = _hooksstatus;
+        _hooksstatus = HS_Stop;
+    }
     lockservo();
 }
 void Motocontrol::setlocked(bool locked)
@@ -168,8 +172,7 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
         {
             _hooksstatus = HS_DownSlow;
             _moto_dji.settime_acc(500);
-            _moto_dji.setspeedtarget(SPEED_HOOK_SLOW);
+            set_hook_speed(SPEED_HOOK_SLOW);
-           
         }
         break;
     }
@@ -189,13 +192,10 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
         if ((millis() - _tm_waitunhook) > HOOK_UNHOOK_TIME)
         {
             printf("HS_WaitUnhook ok startup \n");
-             _moto_dji.settime_acc(500);
+            _moto_dji.settime_acc(1000);
-            _moto_dji.setspeedtarget(-SPEED_HOOK_UP);
+            set_hook_speed(-SPEED_HOOK_UP);
             _hooksstatus = HS_Up;
             _controlstatus.motostatus = MS_Up;
-          //  moto_run(MS_Stop);
-           // setspeed(SPEED_HOOK_UP);
-          //  moto_run(MS_Up);
         }
         break;
     }
@@ -311,10 +311,10 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
             if (!_controlstatus.is_instore)
             {
                 _moto_dji.settime_acc(500);
-                _moto_dji.setspeedtarget(-SPEED_INSTORE);
+                set_hook_speed(-SPEED_INSTORE);
                 _controlstatus.is_instore = true;
                 _hooksstatus = HS_InStore;
-                printf("begin instore currlen:%.2f,instorelen:%.2f,speed:%.2f \n",_curr_length,instlen,mf.output_speed_rpm);
+                printf("begin instore currlen:%.2f,instorelen:%.2f,speed:%.2f \n", _curr_length, instlen, mf.output_speed_rpm);
             }
         }
         else
@@ -341,6 +341,12 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
     }
 }
 
+void Motocontrol::set_hook_speed(float hooksspeed)
+{
+    _goods_speed = hooksspeed;
+    _runspeed = hooksspeed;
+    _moto_dji.setspeedtarget(hooksspeed);
+}
 void Motocontrol::update() // 更新
 {
     _moto_dji.update();
@@ -361,6 +367,24 @@ control_status_t Motocontrol::getcontrolstatus() // 得到控制信息
     return _controlstatus;
 }
 
+void Motocontrol::moto_goodsdownresume()
+{
+    if (_hooksstatus == HS_Stop)
+    {
+        _runspeed = _goods_speed;
+        if (_hooksstatus_prv == HS_Up)
+            _controlstatus.motostatus = MS_Up;
+        if (_hooksstatus_prv == HS_InStore)
+        {
+            _controlstatus.motostatus = MS_Up;
+            _runspeed = -SPEED_INSTORE;
+        }
+        _hooksstatus = _hooksstatus_prv;
+        _moto_dji.settime_acc(1000);
+        unlockservo(); // 打开舵机
+        // _moto_dji.setspeedtarget(_goods_speed);
+    }
+}
 void Motocontrol::moto_goodsdown(float length)
 {
     if (length < 0.0)
@@ -393,12 +417,14 @@ void Motocontrol::moto_goodsdown(float length)
     
     {   _goods_down_target_cnt =0;
         setspeed(SPEED_HOOK_CHECK);
+        _goods_speed = SPEED_HOOK_CHECK;
     }
     else
     { 
         _goods_down_start_cnt = _moto_dji.getmotoinfo().output_round_cnt;
         _goods_down_target_cnt = _goods_down_start_cnt + length / WHEEL_PERIMETER;
        setspeed(SPEED_HOOK_FAST, TM_ACC_HS);
+        _goods_speed = SPEED_HOOK_FAST;
     }
     moto_run(MS_Down);
 }

src/motocontrol.h

@@ -12,7 +12,7 @@
 #define INSTORE_LENGTH_MIN_NONE 10.0f   //最小入仓长度,没有挂东西的情况，在最慢的速度下低于该长度也需要以SPEED_INSTORE缓慢入仓 cm  -------加了软减速后必须根据速度改变！！！！
 #define INSTORE_LENGTH_MIN_GOODS 8.0f   //最小入仓长度,有挂东西的情况 在最慢的速度下低于该长度也需要以SPEED_INSTORE缓慢入仓 cm  -------加了软减速后必须根据速度改变！！！！
 
-#define INSTORE_LENGTH_SPEED  0.045f   //0.0001f最小入仓长度速度平方的(输出轴速度)比例,越快越长，加了软减速后必须根据速度改变！！！！
+#define INSTORE_LENGTH_SPEED 0.05f // 0.0001f最小入仓长度速度平方的(输出轴速度)比例,越快越长，加了软减速后必须根据速度改变！！！！
 
 
 
@@ -44,7 +44,7 @@
 #define HOOK_WEIHT_MAX  6000   //最大货物重量 大于这个认为超重不工作  (克)
 
 #define HOOK_SLOWDOWN_LENGTH  10   //下放物体时快到目标长度时慢速长度  (cm)
-#define HOOK_UNHOOK_TIME  1000       //等待脱钩时间ms
+#define HOOK_UNHOOK_TIME  1500       //等待脱钩时间ms
 
 // 挂钩状态
 enum MotoStatus
@@ -95,6 +95,7 @@ private:
 
   float _goods_down_start_cnt;
   float _goods_down_target_cnt;
+  float _goods_speed;
 
   control_status_t _controlstatus;
   bool _check_targetlength;
@@ -102,7 +103,7 @@ private:
   unsigned long _tm_servotatus;
   int _pullweight;
   HookStatus _hooksstatus;
-
+  HookStatus _hooksstatus_prv;
   int _hook_currlen;
   float _curr_length;
   bool _weightalign;
@@ -121,6 +122,7 @@ private:
   bool checkservoblock();
   void lockservo() ;
   void unlockservo() ; 
+  void set_hook_speed(float hooksspeed);
 public:
     Motocontrol();                  // 构造函数
     ~Motocontrol();                 // 析构函数
@@ -137,7 +139,7 @@ public:
     void moto_run(MotoStatus updown,float length= 0.0f) ;// 收放线
     void setweight(int pullweight);//设置货物实时重量
     void moto_goodsdown(float length= 0.0f);// 放下货物,自动回收
-    HookStatus gethooktatus(){return _hooksstatus;}  //得到挂钩状态
+  void moto_goodsdownresume();                           // 恢复自动放线
-    void weightalign(bool weightalign);         //重量传感器是否校准--必须校准才能执行moto_hookdown
+  HookStatus gethooktatus() { return _hooksstatus; }     // 得到挂钩状态
-
+  void weightalign(bool weightalign);                    // 重量传感器是否校准--必须校准才能执行moto_hookdown
 };