简化Moto类

重构停止逻辑

src/main.cpp

@@ -493,5 +493,5 @@ void upbtn_pressend()
 void testbtn_click()
 {
   Serial.println("testbtn_click");
-  motocontrol.moto_goodsdown(40);
+  motocontrol.moto_goodsdown(40);  //二楼340  //桌子40
 }

src/moto.cpp

@@ -2,18 +2,13 @@
 #include "Arduino.h"
 #include "config.h"
 
-
 uint8_t CaninBuffer[8]; // 接收指令缓冲区
 moto_measure_t moto_chassis;
 PID_TypeDef moto_pid;
 
-
-
 moto::moto()
 {
-  _runing = false;
+  _closed = true;
-  brake = true;
-  _closed=false;
 }
 bool moto::init()
 {
@@ -33,62 +28,45 @@ moto::~moto()
 {
 }
 
-
 void moto::update()
 {
-  if ((_runing || brake)&&!_closed)
+  if (!_closed)
   {
-   
+    unsigned long dt = millis() - moto_chassis.starttime; // 时间差
-
+    float x = 0.0f;
-    unsigned long dt=millis() - moto_chassis.starttime;  //时间差
+    float mspeed = 0.0f;
-    float x=0.0f;
+    float _msoftspeed = moto_pid.target;
-    float mspeed=0.0f;
+    if (dt < _acctime)
-    float _msoftspeed=moto_pid.target;
-    if (dt<_acctime)
     {
-      x=float(dt)/_acctime;  //归一化时间
+      x = float(dt) / _acctime; // 归一化时间
-      //公式参考 https://mp.weixin.qq.com/s/bhdvA3Ex6lAWVmBril-Sag
+      // 公式参考 https://mp.weixin.qq.com/s/bhdvA3Ex6lAWVmBril-Sag
-      mspeed=(-2*x*x*x+3*x*x);//S曲线速度公式--计算归一化速度
+      mspeed = (-2 * x * x * x + 3 * x * x); // S曲线速度公式--计算归一化速度
-      _msoftspeed=_start_speed+mspeed*_ds;
+      _msoftspeed = _start_speed + mspeed * _ds;
     }
     pid_cal(&moto_pid, moto_chassis.speed_rpm, _msoftspeed);
     set_moto_current(moto_pid.output);
-     // printf("tar:%.2f,dt:%d,ds:%.2f,x:%.2f,mspeed:%.2f,motspeed:%.2f,out:%.2f\n",
+    // printf("tar:%.2f,dt:%d,ds:%.2f,x:%.2f,mspeed:%.2f,motspeed:%.2f,out:%.2f\n",
-     // moto_pid.target,dt,_ds,x,mspeed,_msoftspeed,moto_pid.output);
+    // moto_pid.target,dt,_ds,x,mspeed,_msoftspeed,moto_pid.output);
   }
 }
 void moto::settime_acc(float acctime)
 {
- if (acctime==-1)
+  if (acctime == -1)
-    _acctime=abs((_ds*SACCEL_TIME)/((float)OUTPUT_MAX*MOTO_REDUCTION));  //加减速时间
+    _acctime = abs((_ds * SACCEL_TIME) / ((float)OUTPUT_MAX * MOTO_REDUCTION)); // 加减速时间
-  else _acctime=acctime;
+  else
+    _acctime = acctime;
+  
+  printf("settime_acc:%.2f\n",_acctime);
 }
 
-void moto::settarget(float new_target)
+void moto::setspeedtarget(float new_target)
 {
-  // 为了马上停止，跳过PID控制器
-  if (new_target == 0.0f)
-  { 
-    // 没刹车直接断开电流控制
-    _runing = false;
-    if (!brake)
-    {
-      pid_init();
-      set_moto_current(0);
-    }
-  }
-  else
-  {  _runing = true;
-     _closed=false;
-  }
-
   moto_pid.target = new_target * MOTO_REDUCTION;
-   
+  moto_chassis.starttime = millis(); // 换算为绝对时间
-  moto_chassis.starttime=millis(); // 换算为绝对时间
+  _start_speed = moto_chassis.speed_rpm;
-  _start_speed=moto_chassis.speed_rpm; 
+  _ds = moto_pid.target - _start_speed; // 速度差
-  _ds=moto_pid.target-_start_speed;  //速度差
+  _closed = false;
-
+  printf("speedtarget:tm:%d,target:%.2f\n", moto_chassis.starttime, moto_pid.target);
-  printf("set_target:tm:%d,target:%.2f\n",moto_chassis.starttime,moto_pid.target);
 }
 
 void moto::set_moto_current(int16_t iq1)
@@ -119,9 +97,6 @@ void moto::get_moto_measure(moto_measure_t *ptr, uint8_t *message_ptr)
   else if (ptr->angle - ptr->last_angle < -MOTO_MAXANG_HALF)
     ptr->round_cnt++;
   ptr->total_angle = ptr->round_cnt * MOTO_MAXANG + ptr->angle - ptr->offset_angle;
-
-  // ptr->output_speed_rpm= (float)ptr->speed_rpm / MOTO_REDUCTION;
-  //  ptr->output_round_cnt=  ptr->round_cnt  + (float)(ptr->angle - ptr->offset_angle)/MOTO_MAXANG/ MOTO_REDUCTION;
 }
 
 moto_measure_t moto::getmotoinfo()
@@ -131,11 +106,12 @@ moto_measure_t moto::getmotoinfo()
   moto_chassis.output_round_cnt = (moto_chassis.round_cnt + (float)(moto_chassis.angle - moto_chassis.offset_angle) / MOTO_MAXANG) / MOTO_REDUCTION;
   return moto_chassis;
 }
-//关闭电机输出，防止堵转
+// 关闭电机输出，防止堵转
 void moto::close()
 {
-   set_moto_current(0);
+  pid_init();
-   _closed=true;
+  set_moto_current(0);
+  _closed = true;
 }
 void moto::onReceive(int packetSize)
 {

src/moto.h

@@ -75,23 +75,23 @@ void pid_init();
 class moto
 {
 private:
-  bool _runing;
   bool _closed;
   float  _ds;
   int16_t _start_speed;
   float _acctime;
-
   void set_moto_current(int16_t iq1);
   static void get_moto_measure(moto_measure_t *ptr, uint8_t *message_ptr);
   static void onReceive(int packetSize);
 public:
   moto();  // 构造函数
   ~moto(); // 析构函数
-  bool brake;  //刹车==默认true
   bool init();
   void update();
-  void settarget(float new_target);
+  //设置速度--如果需要立即停止需要settime_acc(0)，然后setspeedtarget(0)
-  void settime_acc(float acctime=-1);
+  //否则setspeedtarget(0)会按照settime_acc的时间停止
+  void setspeedtarget(float new_target);  
+  void settime_acc(float acctime=-1); //设置加速和减速时间
+  void close();                       //关闭电机供电
   moto_measure_t getmotoinfo();
-  void close();
+  
 };

src/motocontrol.cpp

@@ -98,15 +98,16 @@ void Motocontrol::unlockservo() // 解锁舵机
     printf("unlockservo\n");
     // 解锁操作
     _lockservo->write(SERVO_UNLOCKPOS);
-    _moto_dji.settarget(-1.0f);//解锁时逆向慢速转动
+    _moto_dji.setspeedtarget(-1.0f);//解锁时逆向慢速转动
     _servotatus = SS_WaitUnLock;
     _tm_servotatus = millis();
     _unblocktimes = 0;
 }
-void Motocontrol::stop() // 停止
+void Motocontrol::stop(float acctime) // 停止
 {
     printf("stop \n");
-    _moto_dji.settarget(0.0f);
+    _moto_dji.settime_acc(acctime);
+    _moto_dji.setspeedtarget(0.0f);
     _controlstatus.motostatus = MS_Stop;
     _hooksstatus = HS_Stop;
     lockservo();
@@ -244,7 +245,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
     {
         if ((millis() - _tm_servotatus) > TM_SERVOLOCK)
         {
-             _moto_dji.settarget(_runspeed);
+             _moto_dji.setspeedtarget(_runspeed);
              _servotatus=SS_ServoUnLocked;
         }
         break;
@@ -285,7 +286,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
                 // 写一个不会堵转的位置
                 _lockservo->write(SERVO_BLOCKUNLOCKPOS);
                 // 转一点电机角度
-                _moto_dji.settarget(SPEED_UNBLOCK);
+                _moto_dji.setspeedtarget(SPEED_UNBLOCK);
             }
         }
 
@@ -315,12 +316,12 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
         // 不能超ROPE_MAXLENGTH
         if ((_curr_length > ROPE_MAXLENGTH) && (_controlstatus.motostatus == MS_Down))
             stop();
-        // 开始入仓
+         // 开始入仓
         if ((_curr_length < (INSTORE_LENGTH_MIN+(mf.output_speed_rpm*mf.output_speed_rpm)*INSTORE_LENGTH_SPEED)) && (_controlstatus.motostatus == MotoStatus::MS_Up))
         {
-            _moto_dji.settarget(-SPEED_INSTORE);
             if (!_controlstatus.is_instore)
             {
+                _moto_dji.setspeedtarget(-SPEED_INSTORE);
                 _controlstatus.is_instore = true;
                 _hooksstatus = HS_InStore;
                 printf("begin instore \n");
@@ -335,8 +336,8 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
         {
             if (mf.output_round_cnt > _target_cnt)
             {
-                printf("cnt:%.2f ,tar:%.2f\n", mf.output_round_cnt, _target_cnt);
+                printf("stop--cnt:%.2f ,tar:%.2f\n", mf.output_round_cnt, _target_cnt);
-                stop();
+                stop(1000);//缓慢停止
             }
         }
         else if (_controlstatus.motostatus == MS_Up)
@@ -344,7 +345,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
             if (mf.output_round_cnt < _target_cnt)
             {
                 printf("cnt:%.2f ,tar:%.2f\n", mf.output_round_cnt, _target_cnt);
-                stop();
+                stop(1000);//缓慢停止
             }
         }
     }
@@ -358,7 +359,7 @@ void Motocontrol::update() // 更新
 }
 void Motocontrol::direct_speed(float motospeed) // 直接控制电机--测试用
 {
-    _moto_dji.settarget(motospeed);
+    _moto_dji.setspeedtarget(motospeed);
 }
 
 moto_measure_t Motocontrol::getmotoinfo()
@@ -488,5 +489,5 @@ void Motocontrol::moto_run(MotoStatus updown, float length)
     // 开始转
     printf("run speed:%.0f,tarcnt:%.2f\n", runspeed, _target_cnt);
     _runspeed=runspeed;
-   // _moto_dji.settarget(runspeed);
+   // _moto_dji.setspeedtarget(runspeed);
 }

src/motocontrol.h

@@ -4,7 +4,7 @@
 #include "moto.h"
 #include <ESP32Servo.h>
 
-#define ROPE_MAXLENGTH 600   //最多能放600cm---实际绳子应该比这个长650之类的
+#define ROPE_MAXLENGTH 700   //最多能放700cm---实际绳子应该比这个长750之类的
 #define WHEEL_DIAMETER 4.0   //轮子直径cm
 #define WHEEL_PERIMETER  (WHEEL_DIAMETER * 3.1416)  //轮子周长
 #define ROPE_MAXCOUNT (ROPE_MAXLENGTH/WHEEL_PERIMETER)   //最大圈数
@@ -14,15 +14,15 @@
 
 
 
-#define SPEED_MAX 200   //最快收放线速度--任何时候不应该超过这个速度
+#define SPEED_MAX 330   //最快收放线速度--任何时候不应该超过这个速度
 #define SPEED_INSTORE 5   //入仓速度,自动重量归零也是这个速度下降2cm
 #define SPEED_BTN_SLOW 20   //按键慢收放线速度(没有设置顶点时只能按这个速度收线)
 #define SPEED_BTN_MID 100   //按键中等收放线速度
-#define SPEED_BTN_FAST 200   //按键最快收放线速度
+#define SPEED_BTN_FAST SPEED_MAX   //按键最快收放线速度
-#define SPEED_HOOK_FAST 100         //货物下放速度--有目标长度得情况下
+#define SPEED_HOOK_FAST SPEED_MAX         //货物下放速度--有目标长度得情况下
 #define SPEED_HOOK_SLOW 20   //货物快到地面速度--有目标长度得情况下
 #define SPEED_HOOK_CHECK 50   //货物下放速度--没有目标长度得情况下能检测到脱钩的合适速度
-#define SPEED_HOOK_UP 100     //空钩上升速度  
+#define SPEED_HOOK_UP SPEED_MAX     //空钩上升速度  
 #define SPEED_UNBLOCK  0.1   //舵机堵转需要缓慢转的速度
 
 
@@ -119,7 +119,7 @@ public:
     Motocontrol();                  // 构造函数
     ~Motocontrol();                 // 析构函数
     void setspeed(float motospeed,float acctime=-1); // 设置速度
-    void stop();                    // 停止
+    void stop(float acctime=0);                    // 停止
     void setzero();                 // 设置0长度位置
     int16_t getlength();            // 得到长度
     void update();                  // 更新