加入锁定舵机，电机控制调整

platformio.ini

@@ -21,3 +21,4 @@ lib_deps =
 	robtillaart/CRC@^0.3.3
 	sandeepmistry/CAN@^0.3.1
 	fastled/FastLED@^3.5.0
+	madhephaestus/ESP32Servo@^0.13.0

src/config.h

@@ -16,7 +16,7 @@
 #define LOADCELL_SCK_PIN 4
 ///////////////////////////////////////////
 #define SPEED_UP_SLOW 4   //rmp 或者  弧度/秒
-
+#define SERVO_PIN   14      //锁定舵机PWM控制脚
 
 
 // 挂钩状态

src/main.cpp

@@ -8,6 +8,8 @@
 #include "motocontrol.h"
 #include "moto.h"
 #include <FastLED.h>
+#include <ESP32Servo.h>
+#define TM_INSTORE_DELAY 100   //入仓延时关闭时间  ms
 
 // 角度传感器
 // 收放线电机控制
@@ -19,6 +21,7 @@ OneButton button_down(BTN_DOWN, true);
 OneButton button_checktop(BTN_CT, true);
 HX711 scale;
 Serialcommand sercomm;
+Servo myservo; 
 #define NUM_LEDS 1
 #define DATA_PIN 25
 CRGB leds[NUM_LEDS];
@@ -50,6 +53,9 @@ void led_show(CRGB ledcolor);
 // void Task1code( void *pvParameters );
 void showledidel();
 int pullweight = 0;
+ unsigned long _tm_bengstop;
+ bool _bengstop=false;
+
 
 void setup()
 {
@@ -76,9 +82,17 @@ void setup()
   button_checktop.attachLongPressStart(ctbtn_pressstart); // 按下马上产生事件，
   button_checktop.attachLongPressStop(ctbtn_pressend);    // 抬起
 
+  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, DATA_PIN, RGB>(leds, NUM_LEDS); // GRB ordering is typical
-   if (!motocontrol.init()) // 初始化电机控制
+  if (!motocontrol.init(&myservo)) // 初始化电机控制
     printf("motocontrol init fault\n");
 
   // 发送状态任务开启--500ms一次
@@ -99,8 +113,8 @@ void slowup()
 }
 void showinfo()
 {
-  // moto_measure_t mf=motocontrol.getmotoinfo() ;
+  //  moto_measure_t mf=motocontrol.getmotoinfo() ;
-  // printf("total_cnt:%.3f;speed:%.2f \n ", mf.output_round_cnt, mf.output_speed_rpm);
+  //  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);
 }
@@ -116,6 +130,13 @@ void loop()
   motocontrol.update();   // 电机控制
   showledidel();          //显示LED灯光
   showinfo();             // 显示一些调试用信息
+  if ((_bengstop)&&((millis()-_tm_bengstop)>TM_INSTORE_DELAY))
+  {
+    _bengstop=false;
+    motocontrol.setlocked(true);
+    hookstatus = HS_Lock;
+  }
+
   delay(1);
 }
 // 在核心0上执行耗时长的低优先级的
@@ -171,7 +192,11 @@ void showledidel()
   }
   case MotoStatus::MS_Up:
   {
-    led_show(255,0,0); // red
+    
+    if (motocontrol.getcontrolstatus().is_instore)
+      led_show(255,0,0); // red
+    else
+      led_show(200,0,50); // red
     break;
   }
 
@@ -183,7 +208,7 @@ void showledidel()
     }else
     {
       if (motocontrol.getcontrolstatus().is_setzero)
-        led_show(255,0,255);           // 紫色
+        led_show(255,255,255);           // 紫色
       else
         led_show(255,255,0);   //yellow
     }
@@ -197,14 +222,17 @@ void showledidel()
 void ctbtn_pressstart()
 {
   Serial.println("ctbtn_pressstart");
-  motocontrol.setlocked(true);
+  
-  hookstatus = HS_Lock;
+  _tm_bengstop=millis();
+  _bengstop=true;
+
 }
 
 void ctbtn_pressend()
 {
   Serial.println("ctbtn_pressend");
   motocontrol.setlocked(false);
+  _bengstop=false;
 }
 void downbtn_click()
 {

src/moto.cpp

@@ -7,11 +7,14 @@ PID_TypeDef moto_pid;
 
 moto::moto()
 {
+  _runing = false;
+  brake = true;
+  _closed=false;
 }
 bool moto::init()
 {
   Serial.println("init moto");
-  pid_param_init(&moto_pid, 16000, 1000, 10, 8000, 0, 3.0f, 0.2f, 0.0f);
+  pid_init();
   CAN.setPins(26, 27);
   // start the CAN bus at 500 kbps
   if (!CAN.begin(1000E3))
@@ -25,20 +28,64 @@ bool moto::init()
 moto::~moto()
 {
 }
+
+// int16_t delta;
+// int16_t max_speed_change = 100; // 500 ok  100电源不报高压错误但有点震荡
+// int16_t set_speed_temp;
 void moto::update()
 {
-  pid_cal(&moto_pid, moto_chassis.speed_rpm);
+
-  set_moto_current(moto_pid.output);
+  //  //可以柔和改变速度，柔和程度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);
+    set_moto_current(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)
 {
-  moto_pid.target = new_target*MOTO_REDUCTION;
+  // 为了马上停止，跳过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;
 }
 
 void moto::set_moto_current(int16_t iq1)
 {
-  iq1=MOTO_REVERSED*iq1;
+  iq1 = MOTO_REVERSED * iq1;
   CAN.beginPacket(0x200, 8);
   CAN.write((uint8_t)((iq1 >> 8) & 0xFF));
   CAN.write((uint8_t)(iq1 & 0xFF));
@@ -54,13 +101,10 @@ void moto::get_moto_measure(moto_measure_t *ptr, uint8_t *message_ptr)
 {
   // 处理数据
   ptr->last_angle = ptr->angle;
-  ptr->angle = (uint16_t)MOTO_REVERSED*(message_ptr[0] << 8 | message_ptr[1]);       // 角度
+  ptr->angle = (uint16_t)MOTO_REVERSED * (message_ptr[0] << 8 | message_ptr[1]);       // 角度
-  ptr->speed_rpm = (int16_t)MOTO_REVERSED*(message_ptr[2] << 8 | message_ptr[3]);    // 速度
+  ptr->speed_rpm = (int16_t)MOTO_REVERSED * (message_ptr[2] << 8 | message_ptr[3]);    // 速度
-  ptr->real_current = (int16_t)MOTO_REVERSED*(message_ptr[4] << 8 | message_ptr[5]); // 实际转矩电流
+  ptr->real_current = (int16_t)MOTO_REVERSED * (message_ptr[4] << 8 | message_ptr[5]); // 实际转矩电流
-  ptr->temperature = message_ptr[6];                                   // 电机温度，2006没有，为0
+  ptr->temperature = message_ptr[6];                                                   // 电机温度，2006没有，为0
-
-
-
 
   if (ptr->angle - ptr->last_angle > MOTO_MAXANG_HALF)
     ptr->round_cnt--;
@@ -74,11 +118,17 @@ void moto::get_moto_measure(moto_measure_t *ptr, uint8_t *message_ptr)
 
 moto_measure_t moto::getmotoinfo()
 {
-  //在中断里面计算时间太长会导致重启,在取信息时计算浮点
+  // 在中断里面计算时间太长会导致重启,在取信息时计算浮点
   moto_chassis.output_speed_rpm = (float)moto_chassis.speed_rpm / MOTO_REDUCTION;
   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);
+   _closed=true;
+}
 void moto::onReceive(int packetSize)
 {
   if (!CAN.packetRtr())
@@ -91,6 +141,11 @@ void moto::onReceive(int packetSize)
   }
 }
 /////////////PID控制
+void pid_init()
+{
+  pid_param_init(&moto_pid, 10000, 10000, 10, 8000, 0, 10.0f, 1.0f, 0.5f); //
+  // pid_param_init(&moto_pid, 10000, 10000, 10, 8000, 0, 14.0f, 0.01f, 0.0f); ////
+}
 void pid_param_init(PID_TypeDef *pid,
                     uint16_t maxout,
                     uint16_t intergral_limit,
@@ -129,7 +184,7 @@ inline void pid_target(PID_TypeDef *pid, float new_target)
   pid->target = new_target;
 }
 
-float pid_cal(PID_TypeDef *pid, int16_t measure)
+float pid_cal(PID_TypeDef *pid, int16_t measure, float pidtarget)
 {
 
   pid->lasttime = pid->thistime;
@@ -137,7 +192,8 @@ float pid_cal(PID_TypeDef *pid, int16_t measure)
   pid->dtime = pid->thistime - pid->lasttime;             // 更新两次计算的时间
 
   pid->last_err = pid->err;
-  pid->err = pid->target - measure;
+  pid->err = pidtarget - measure;
+  // pid->err = pid->target - measure;
 
   if (abs(pid->err) > pid->DeadBand)
   {

src/moto.h

@@ -64,11 +64,14 @@ void pid_param_init(PID_TypeDef *pid,
                     float kd);
 inline void pid_reset(PID_TypeDef *pid, float kp, float ki, float kd);
 inline void pid_target(PID_TypeDef *pid, float new_target);
-float pid_cal(PID_TypeDef *pid, int16_t measure);
+float pid_cal(PID_TypeDef *pid, int16_t measure,float pidtarget);
+void pid_init();
 
 class moto
 {
 private:
+  bool _runing;
+  bool _closed;
   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);
@@ -76,8 +79,10 @@ private:
 public:
   moto();  // 构造函数
   ~moto(); // 析构函数
+  bool brake;  //刹车==默认true
   bool init();
   void update();
   void settarget(float new_target);
   moto_measure_t getmotoinfo();
+  void close();
 };

src/motocontrol.cpp

@@ -2,6 +2,7 @@
 #include "Arduino.h"
 #include "motocontrol.h"
 #include "moto.h"
+#include <ESP32Servo.h>
 
 #define DEBUG_OUT false
 Motocontrol::Motocontrol()
@@ -12,33 +13,30 @@ Motocontrol::Motocontrol()
     _controlstatus.motostatus = MotoStatus::MS_Stop;
     _controlstatus.speed_targe = SPEED_SLOW;
     _check_targetlength = false;
+    _need_closemoto=false;
 }
 Motocontrol::~Motocontrol()
 {
 }
 
-bool Motocontrol::init() // 初始化
+bool Motocontrol::init(Servo *lockservo) // 初始化
 {
+    _lockservo=lockservo;
     return _moto_dji.init();
 }
 void Motocontrol::setspeed(float motospeed) // 设置速度
 {
     _controlstatus.speed_targe = motospeed;
 }
-void Motocontrol::down(float length) // 放线
+
-{
-    printf("down:%.2f \n", length);
-    moto_run(MotoStatus::MS_Down, length);
-}
-void Motocontrol::up(float length) // 收线
-{
-    printf("up:%.2f \n", length);
-    moto_run(MotoStatus::MS_Up, length);
-}
 void Motocontrol::stop() // 停止
 {
     _moto_dji.settarget(0.0f);
     _controlstatus.motostatus = MS_Stop;
+    _lockservo->write(SERVO_LOCKPOS); 
+    _need_closemoto=true;
+    _tm_closemoto=millis();
+    printf("write servo :%d \n",SERVO_LOCKPOS);
     printf("stop \n");
 }
 void Motocontrol::setlocked(bool locked)
@@ -62,6 +60,11 @@ int16_t Motocontrol::getlength() // 得到长度
 }
 void Motocontrol::checkstatus() // 检查状态，比如什么时候停
 {
+    //1秒后断电
+    if ((_need_closemoto)&&((millis()-_tm_closemoto)>1000))
+        _moto_dji.close(); 
+      
+
     moto_measure_t mf = _moto_dji.getmotoinfo();
     // 已设置零点
     if (_controlstatus.is_setzero)
@@ -136,6 +139,10 @@ void Motocontrol::moto_run(MotoStatus updown, float length)
     if (_controlstatus.speed_targe == 0)
         return;
 
+    _lockservo->write(SERVO_UNLOCKPOS); 
+    _need_closemoto=false;
+
+     printf("write servo :%d \n",SERVO_UNLOCKPOS);
     _controlstatus.motostatus = updown;
     // 有长度限制
     if (length != 0)

src/motocontrol.h

@@ -2,18 +2,21 @@
 #include "Arduino.h"
 #include "config.h"
 #include "moto.h"
+#include <ESP32Servo.h>
 #define ROPE_MAXLENGTH 600   //最多能放600cm---实际绳子应该比这个长650之类的
 #define WHEEL_DIAMETER 4.0   //轮子直径cm
-#define INSTORE_LENGTH 15   //入仓长度,低于该长度要缓慢入仓 cm
+#define INSTORE_LENGTH 10   //入仓长度,低于该长度要缓慢入仓 cm
 
 #define SPEED_MAX 330   //最快收放线速度
-#define SPEED_MIN 3   //入仓速度
+#define SPEED_MIN 5   //入仓速度
 #define SPEED_SLOW 20   //慢收放线速度
 #define SPEED_FAST 100   //快收放线速度
 #define SPEED_BTN_FAST 200   //最快收放线速度
 
 #define WHEEL_PERIMETER  (WHEEL_DIAMETER * 3.1416)  //轮子周长
 #define ROPE_MAXCOUNT (ROPE_MAXLENGTH/WHEEL_PERIMETER)   //最大圈数
+#define SERVO_LOCKPOS  1900    //舵机锁定位置
+#define SERVO_UNLOCKPOS  1500   //舵机解锁位置
 
 
 
@@ -22,6 +25,7 @@ enum MotoStatus
 {
   MS_Down,   // 自动下放中
   MS_Up,     // 回收中
+
   MS_Stop   //停止中
 };
 
@@ -43,28 +47,30 @@ typedef struct
 class Motocontrol
 {
 private:
+  Servo* _lockservo;
   moto _moto_dji;
   float _start_cnt;
   float _target_cnt;
   control_status_t _controlstatus;
   bool _check_targetlength;
+  bool _need_closemoto;
+  unsigned long _tm_closemoto;
   
   void checkstatus();
 public:
     Motocontrol();                  // 构造函数
     ~Motocontrol();                 // 析构函数
     void setspeed(float motospeed); // 设置速度
-    void down(float length);        // 收线
-    void up(float length);          // 放线
     void stop();                    // 停止
     void setzero();                 // 设置0长度位置
     int16_t getlength();            // 得到长度
     void update();                  // 更新
-    bool init(); // 初始化
+    bool init(Servo* lockservo); // 初始化
     void direct_speed(float motospeed);  //直接设置电机速度--立马生效,仅用于测试
     moto_measure_t getmotoinfo();        //得到电机信息   
     control_status_t getcontrolstatus();        //得到控制信息  
     void setlocked(bool locked);             //是否到顶锁定
     void moto_run(MotoStatus updown,float length= 0.0f) ;// 放线
+    
 
 };