增加电机缓启动和停止

增加舵机锁定和解锁时和电机的配合，防止电机还在转就锁定和防止正在解锁电机开始高速旋转导致磨损
入仓长度随速度平方比例变化，防止速度太快刹不住车--
放货物变慢距离是否也需要这样还不确定，测试确定
增加速度，按实际330的速度收放货物，按钮双击和长按速度也加快

src/config.h

@@ -14,10 +14,12 @@
 #define LOADCELL_DOUT_PIN  16
 #define LOADCELL_SCK_PIN  17
 ///////////////////////////////////////////
-#define SPEED_UP_SLOW 4   //rmp 或者  弧度/秒
 #define SERVO_PIN   14      //锁定舵机PWM控制脚
 ////LED
 #define LED_DATA_PIN 25
+//Moto-CAN
+#define MOTO_CAN_RX 26  
+#define MOTO_CAN_TX 27
 /////
 #define WEIGHT_SCALE 276   //这是缩放值，根据砝码实测516.f
 #define TM_INSTORE_DELAY 200 // 入仓动力延时关闭时间  ms

src/moto.cpp

@@ -1,10 +1,14 @@
 #include "moto.h"
 #include "Arduino.h"
+#include "config.h"
+
 
 uint8_t CaninBuffer[8]; // 接收指令缓冲区
 moto_measure_t moto_chassis;
 PID_TypeDef moto_pid;
 
+
+
 moto::moto()
 {
   _runing = false;
@@ -15,7 +19,7 @@ bool moto::init()
 {
   Serial.println("init moto");
   pid_init();
-  CAN.setPins(26, 27);
+  CAN.setPins(MOTO_CAN_RX, MOTO_CAN_TX);
   // start the CAN bus at 500 kbps
   if (!CAN.begin(1000E3))
   {
@@ -29,39 +33,31 @@ moto::~moto()
 {
 }
 
-// int16_t delta;
+
-// int16_t max_speed_change = 100; // 500 ok  100电源不报高压错误但有点震荡
-// int16_t set_speed_temp;
 void moto::update()
 {
-
-  //  //可以柔和改变速度，柔和程度max_speed_change越小越柔和,但太小需要调PID
-  // for (uint8_t i = 0; i < 2; i++)
-  // {
-  //   // 加减速
-  //   delta = (int16_t)moto_pid.target - moto_chassis.speed_rpm;
-  //   if (delta > max_speed_change)
-  //     set_speed_temp = (float)(moto_chassis.speed_rpm + max_speed_change);
-  //   else if (delta < -max_speed_change)
-  //     set_speed_temp = (float)(moto_chassis.speed_rpm - max_speed_change);
-  //   else
-  //     set_speed_temp = moto_pid.target;
-  //   pid_cal(&moto_pid, moto_chassis.speed_rpm,set_speed_temp);
-  // }
-
   if ((_runing || brake)&&!_closed)
   {
-    pid_cal(&moto_pid, moto_chassis.speed_rpm, moto_pid.target);
+   
+
+    unsigned long dt=millis() - moto_chassis.starttime;  //时间差
+    float x=0.0f;
+    float mspeed=0.0f;
+    float _msoftspeed=moto_pid.target;
+    if (dt<_acctime)
+    {
+      x=float(dt)/_acctime;  //归一化时间
+      //公式参考 https://mp.weixin.qq.com/s/bhdvA3Ex6lAWVmBril-Sag
+      mspeed=(-2*x*x*x+3*x*x);//S曲线速度公式--计算归一化速度
+      _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",
+     // moto_pid.target,dt,_ds,x,mspeed,_msoftspeed,moto_pid.output);
   }
 }
 
-// void moto::update()
-// {
-//   pid_cal(&moto_pid, moto_chassis.speed_rpm);
-//   set_moto_current(moto_pid.output);
-// }
-
 void moto::settarget(float new_target)
 {
   // 为了马上停止，跳过PID控制器
@@ -81,6 +77,13 @@ void moto::settarget(float new_target)
   }
 
   moto_pid.target = new_target * MOTO_REDUCTION;
+   
+  moto_chassis.starttime=millis(); // 换算为绝对时间
+  _start_speed=moto_chassis.speed_rpm; 
+  _ds=moto_pid.target-_start_speed;  //速度差
+  _acctime=abs((_ds*SACCEL_TIME)/((float)OUTPUT_MAX*MOTO_REDUCTION));  //加减速时间
+
+  printf("set_target:tm:%d,target:%.2f\n",moto_chassis.starttime,moto_pid.target);
 }
 
 void moto::set_moto_current(int16_t iq1)

src/moto.h

@@ -7,6 +7,9 @@
 #define MOTO_MAXANG 8192   //0-8191
 #define MOTO_MAXANG_HALF MOTO_MAXANG / 2
 
+#define OUTPUT_MAX   400     //最大输出转速rpm
+#define SACCEL_TIME 100    //使用S速度曲线加速到OUTPUT_MAX转需要的时间---如果目标速度相差200转就  (200*SACCEL_TIME)/OUTPUT_MAX  秒
+
 
 typedef struct _PID_TypeDef
 {
@@ -47,11 +50,13 @@ typedef struct
   int32_t total_angle;
   uint32_t msg_cnt;
 
+  unsigned long starttime;
+
   float output_speed_rpm;     //输出轴速度 ---rpm
   float output_round_cnt;     //输出轴 总圈数
-
 } moto_measure_t;
 
+
 /// PID控制器
 void pid_param_init(PID_TypeDef *pid,
                     uint16_t maxout,
@@ -72,10 +77,13 @@ 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(); // 析构函数

src/motocontrol.cpp

@@ -15,7 +15,6 @@ Motocontrol::Motocontrol()
     _controlstatus.motostatus = MotoStatus::MS_Stop;
     _controlstatus.speed_targe = SPEED_BTN_SLOW;
     _check_targetlength = false;
-    _need_closemoto = false;
     _pullweight = 0;
     _curr_length = 0.0;
     _hooksstatus = HS_UnInit;
@@ -23,6 +22,8 @@ Motocontrol::Motocontrol()
     _overweightcount = 0;
     _notweightcount = 0;
     _autogoodsdown = false;
+    _servotatus = SS_ServoUnLocked; // 停止状态，为了防止舵机堵转，和延时关闭电机需要一个停止状态机
+    _unblocktimes = 0;
 }
 Motocontrol::~Motocontrol()
 {
@@ -31,6 +32,10 @@ Motocontrol::~Motocontrol()
 bool Motocontrol::init(Servo *lockservo) // 初始化
 {
     _lockservo = lockservo;
+    // 不管堵转问题，马上会执行其他初始化让舵机介绍
+    _lockservo->write(SERVO_LOCKPOS);
+    _servotatus = SS_ServoLocked;
+
     return _moto_dji.init();
 }
 void Motocontrol::setspeed(float motospeed) // 设置速度
@@ -81,16 +86,29 @@ void Motocontrol::checkgoods() // 检测是否超重
         _notweightcount = 0;
     }
 }
+void Motocontrol::lockservo() // 锁定舵机
+{
+    printf("start_lockservo\n");
+    _servotatus = SS_WaitMotoStop;
+    _tm_servotatus = millis();
+}
+void Motocontrol::unlockservo() // 解锁舵机
+{
+    printf("unlockservo\n");
+    // 解锁操作
+    _lockservo->write(SERVO_UNLOCKPOS);
+    _moto_dji.settarget(-1.0f);//解锁时逆向慢速转动
+    _servotatus = SS_WaitUnLock;
+    _tm_servotatus = millis();
+    _unblocktimes = 0;
+}
 void Motocontrol::stop() // 停止
 {
+    printf("stop \n");
     _moto_dji.settarget(0.0f);
     _controlstatus.motostatus = MS_Stop;
     _hooksstatus = HS_Stop;
-    _lockservo->write(SERVO_LOCKPOS);
+    lockservo();
-    _need_closemoto = true;
-    _tm_closemoto = millis();
-    printf("write servo :%d \n", SERVO_LOCKPOS);
-    printf("stop \n");
 }
 void Motocontrol::setlocked(bool locked)
 {
@@ -209,13 +227,85 @@ void Motocontrol::checkhookstatus() // 检查挂钩状态
     }
     }
 }
+// 检测舵机堵转---电流大或者角度不够，需要传感器
+bool Motocontrol::checkservoblock()
+{
+    return false;
+}
 void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候停
 {
-    // 1秒后断电，防止电调堵转保护
-    if ((_need_closemoto) && ((millis() - _tm_closemoto) > 1000))
-        _moto_dji.close();
-
     moto_measure_t mf = _moto_dji.getmotoinfo();
+
+    // 停止状态---1防止舵机堵转，2延时关闭电机供电
+    switch (_servotatus)
+    {
+  case SS_WaitUnLock: //舵机解锁
+    {
+        if ((millis() - _tm_servotatus) > TM_SERVOLOCK)
+        {
+             _moto_dji.settarget(_runspeed);
+             _servotatus=SS_ServoUnLocked;
+        }
+        break;
+    }
+  
+
+
+        //等待电机停止
+    case SS_WaitMotoStop:
+    {
+        if (abs(mf.speed_rpm)<3)
+         {
+            _lockservo->write(SERVO_LOCKPOS);
+            _servotatus=SS_WaitServo;
+            _tm_servotatus = millis();
+         }
+        break;
+    }
+    // 等待舵机执行到位---------防止堵转，功能没有使用，采用硬件防堵转舵机
+    case SS_WaitServo:
+    {
+        // 舵机执行时间
+        if ((millis() - _tm_servotatus) > TM_SERVOLOCK)
+        {
+           // 检测堵转,没堵转直接关闭电机,否则打开舵机，让电机旋转一点再锁定--最多重复3次
+            if (!checkservoblock() || (_unblocktimes > 2))
+            {
+                printf("SS not servoblock close moto \n");
+                _moto_dji.close();
+                _servotatus = SS_ServoLocked;
+            }
+            else
+            // 堵转
+            {
+                printf("SS  servoblock unlock servo and turn moto \n");
+                _tm_servotatus = millis();
+                _servotatus = SS_WaitUnBlock;
+                // 写一个不会堵转的位置
+                _lockservo->write(SERVO_BLOCKUNLOCKPOS);
+                // 转一点电机角度
+                _moto_dji.settarget(SPEED_UNBLOCK);
+            }
+        }
+
+        break;
+    }
+    case SS_WaitUnBlock:
+    {
+        if ((millis() - _tm_servotatus) > TM_UNBLOCK)
+        {
+            printf("SS lock servo  \n");
+            // 继续锁定等待舵机检测
+            _lockservo->write(SERVO_LOCKPOS);
+            _servotatus = SS_WaitServo;
+            _tm_servotatus = millis();
+            _unblocktimes++;
+        }
+        break;
+    }
+    }
+
+    
     // 已设置零点
     if (_controlstatus.is_setzero)
     {
@@ -225,7 +315,7 @@ void Motocontrol::checkmotostatus() // 检查电机状态，比如什么时候
         if ((_curr_length > ROPE_MAXLENGTH) && (_controlstatus.motostatus == MS_Down))
             stop();
         // 开始入仓
-        if ((_curr_length < INSTORE_LENGTH) && (_controlstatus.motostatus == MotoStatus::MS_Up))
+        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)
@@ -298,7 +388,7 @@ void Motocontrol::moto_goodsdown(float length)
         return;
     }
     // 没挂东西
-    if (!_controlstatus.is_havegoods )
+    if (!_controlstatus.is_havegoods)
     {
         printf("goods min fault:%d \n", _pullweight);
         return;
@@ -366,10 +456,8 @@ void Motocontrol::moto_run(MotoStatus updown, float length)
     else
         _hooksstatus = HS_Down;
 
-    _lockservo->write(SERVO_UNLOCKPOS);
+    unlockservo();
-    _need_closemoto = false;
 
-    printf("write servo :%d \n", SERVO_UNLOCKPOS);
     _controlstatus.motostatus = updown;
     // 有长度限制
     if (length != 0)
@@ -398,5 +486,6 @@ void Motocontrol::moto_run(MotoStatus updown, float length)
     }
     // 开始转
     printf("run speed:%.0f,tarcnt:%.2f\n", runspeed, _target_cnt);
-    _moto_dji.settarget(runspeed);
+    _runspeed=runspeed;
+   // _moto_dji.settarget(runspeed);
 }

src/motocontrol.h

@@ -9,23 +9,33 @@
 #define WHEEL_PERIMETER  (WHEEL_DIAMETER * 3.1416)  //轮子周长
 #define ROPE_MAXCOUNT (ROPE_MAXLENGTH/WHEEL_PERIMETER)   //最大圈数
 
-#define INSTORE_LENGTH 10   //入仓长度,低于该长度要缓慢入仓 cm
+#define INSTORE_LENGTH_MIN 8.0f   //最小入仓长度,在最慢的速度下低于该长度也需要以SPEED_INSTORE缓慢入仓 cm  -------加了软减速后必须根据速度改变！！！！
+#define INSTORE_LENGTH_SPEED  0.0002f   //最小入仓长度速度平方的(输出轴速度)比例,越快越长，加了软减速后必须根据速度改变！！！！
+
+
 
 #define SPEED_MAX 330   //最快收放线速度--任何时候不应该超过这个速度
 #define SPEED_INSTORE 5   //入仓速度,自动重量归零也是这个速度下降2cm
 #define SPEED_BTN_SLOW 20   //按键慢收放线速度(没有设置顶点时只能按这个速度收线)
-#define SPEED_BTN_MID 100   //按键中等收放线速度
+#define SPEED_BTN_MID 200   //按键中等收放线速度
-#define SPEED_BTN_FAST 200   //按键最快收放线速度
+#define SPEED_BTN_FAST 330   //按键最快收放线速度
-
+#define SPEED_HOOK_FAST 330         //货物下放速度--有目标长度得情况下
-#define SPEED_HOOK_FAST 100         //货物下放速度--有目标长度得情况下
 #define SPEED_HOOK_SLOW 20   //货物快到地面速度--有目标长度得情况下
 #define SPEED_HOOK_CHECK 50   //货物下放速度--没有目标长度得情况下能检测到脱钩的合适速度
-#define SPEED_HOOK_UP 50     //空钩上升速度  
+#define SPEED_HOOK_UP 330     //空钩上升速度  
+#define SPEED_UNBLOCK  0.1   //舵机堵转需要缓慢转的速度
 
 
 
-#define SERVO_LOCKPOS  1820    //舵机锁定位置
+#define TM_UNBLOCK 100   //舵机堵转需要缓慢转等待的时间ms
-#define SERVO_UNLOCKPOS  1600   //舵机解锁位置
+#define TM_SERVOLOCK 200   //舵机转到Lock和Unlock等待的时间ms
+
+
+
+
+#define SERVO_LOCKPOS  1890    //舵机锁定位置
+#define SERVO_UNLOCKPOS  1780   //舵机解锁位置
+#define SERVO_BLOCKUNLOCKPOS  1700   //舵机堵转需要转回的位置--可以和SERVO_UNLOCKPOS一样，为了速度快也可以更小
 
 #define HOOK_WEIHT_MIN  100   //最小货物重量 小于这个认为没挂东西  (克)
 
@@ -54,6 +64,20 @@ typedef struct
   MotoStatus motostatus;  //电机运行状态
 
 } control_status_t;
+// 锁定舵机状态
+enum ServoStatus
+{
+  SS_WaitMotoStop,  //等待电机停止
+  
+  SS_ServoUnLocked,   // 
+
+  SS_WaitUnLock,   // 
+  SS_WaitServo,     //等待舵机不堵转
+  SS_WaitUnBlock,      //等待舵机不堵转的位置--同时电机会缓慢旋转
+  SS_ServoLocked   //成功停止
+};
+
+
 
 
 
@@ -68,8 +92,8 @@ private:
   float _target_cnt;
   control_status_t _controlstatus;
   bool _check_targetlength;
-  bool _need_closemoto;
+  ServoStatus  _servotatus;
-  unsigned long _tm_closemoto;
+  unsigned long _tm_servotatus;
   int _pullweight;
   HookStatus _hooksstatus;
   int _hook_targetlen;
@@ -80,13 +104,17 @@ private:
   uint8_t _overweightcount;
 
   uint8_t _notweightcount;
-
+  uint8_t _unblocktimes;
   unsigned long  _tm_waitunhook;
+  float _runspeed;
 
   void checkmotostatus();
   void checkhookstatus();
   void goodsstartup() ;
   void checkgoods();
+  bool checkservoblock();
+  void lockservo() ;
+  void unlockservo() ; 
 public:
     Motocontrol();                  // 构造函数
     ~Motocontrol();                 // 析构函数