可以控制电机，彩色LED，和按钮的逻辑关系

问题：采用速度环控制，电机力量不够，需要改为位置环

.vscode/settings.json

@@ -2,6 +2,14 @@
     "files.associations": {
         "cstddef": "cpp",
         "algorithm": "cpp",
-        "cmath": "cpp"
+        "cmath": "cpp",
+        "array": "cpp",
+        "string": "cpp",
+        "string_view": "cpp",
+        "chrono": "cpp",
+        "functional": "cpp",
+        "ios": "cpp",
+        "xlocale": "cpp",
+        "xstring": "cpp"
     }
 }

platformio.ini

@@ -18,3 +18,6 @@ lib_deps =
 	bogde/HX711@^0.7.5
 	mathertel/OneButton@^2.0.3
 	sstaub/Ticker@^4.4.0
+	robtillaart/CRC@^0.3.3
+	sandeepmistry/CAN@^0.3.1
+	fastled/FastLED@^3.5.0

src/Serialcommand.cpp

@@ -1,6 +1,9 @@
 #include "Serialcommand.h"
 #include "Arduino.h"
-
+#include "CRC.h"
+#define HEAD_HI 0xA5
+#define HEAD_LI 0x5A
+#define DEBUG_OUT false
 Serialcommand::Serialcommand()
 {
   // 连接控制器
@@ -12,8 +15,8 @@ Serialcommand::Serialcommand()
   CmdState = 0;
   bufferIndex = 0;
   recv_flag = false;
-  outBuffer[0] = 0xff;
+  outBuffer[0] = HEAD_HI;
-  outBuffer[1] = 0xff;
+  outBuffer[1] = HEAD_LI;
 }
 Serialcommand::~Serialcommand()
 {
@@ -24,27 +27,27 @@ void Serialcommand::Uart_Command_Rev(void)
   while (Serial2.available() > 0)
   {
     ch = Serial2.read();
-    // printf("rev:%d\n", ch);
+    if (DEBUG_OUT) printf("rev:%d\n", ch);
     switch (CmdState)
     {
       // 开始标志1
     case 0:
     {
-      if (0xFF == ch)
+      if (HEAD_HI == ch)
       {
         CmdState = 1;
-        // printf("st:%d\n", CmdState);
+        if (DEBUG_OUT) printf("st:%d\n", CmdState);
       }
     }
     break;
     // 开始标志2
     case 1:
     {
-      if (0xFF == ch)
+      if (HEAD_LI == ch)
       {
         bufferIndex = 0;
         CmdState = 2;
-        // printf("st:%d\n", CmdState);
+        if (DEBUG_OUT)  printf("st:%d\n", CmdState);
       }
       else
         CmdState = 0;
@@ -54,13 +57,13 @@ void Serialcommand::Uart_Command_Rev(void)
     case 2:
     {
       datalen = ch;
-      // printf("datalen:%d\n", datalen);
+      if (DEBUG_OUT) printf("datalen:%d\n", datalen);
       if (datalen <= inbuffersize)
       {
         CmdState = 3;
         bufferIndex = 0;
         inBuffer[bufferIndex] = ch;
-        // printf("buf:%d:%d\n", bufferIndex, inBuffer[bufferIndex]);
+        if (DEBUG_OUT) printf("buf:%d:%d\n", bufferIndex, inBuffer[bufferIndex]);
       }
       else // 超长
       {
@@ -71,13 +74,13 @@ void Serialcommand::Uart_Command_Rev(void)
     // 收数据直到结束
     case 3:
     {
-      // printf("st3:%d:%d\n", bufferIndex, datalen);
+      if (DEBUG_OUT) printf("st3:%d:%d\n", bufferIndex, datalen);
 
       if (bufferIndex <= datalen)
       {
         bufferIndex++;
         inBuffer[bufferIndex] = ch;
-        // printf("buf:%d:%d\n", bufferIndex, inBuffer[bufferIndex]);
+        if (DEBUG_OUT) printf("buf:%d:%d\n", bufferIndex, inBuffer[bufferIndex]);
         if (bufferIndex == datalen) // 收完了
         {
           CmdState = 0;
@@ -85,22 +88,25 @@ void Serialcommand::Uart_Command_Rev(void)
           bufferIndex = 0;
           return; // 不收数据了，先出去解析
         }
+      } else  //异常
+      {
+        if (DEBUG_OUT) printf("buf error:%d:%d\n", bufferIndex, datalen);
+        CmdState = 0;
+        bufferIndex = 0;
+        recv_flag=false;
+        return; 
       }
     }
     break;
     }
   }
 }
+//CRC8  
+//多项式：CRC-8：x^8+x^2+x^1+1 0x07 （0x107）
+//https://zhuanlan.zhihu.com/p/114049042
 uint8_t Serialcommand::getsum(const uint8_t *buffer, size_t size)
 {
-  uint16_t usum = 0;
+   return crc8(buffer, size, 0x07, 0x00, 0x00, false, false);
-  uint8_t chksum = 0;
-  for (uint8_t i = 0; i < size; i++)
-  {
-    usum += buffer[i];
-  }
-  chksum = (uint8_t)usum & 0xff; // 取低8位
-  return chksum;
 }
 bool Serialcommand::checksum()
 {
@@ -148,13 +154,13 @@ void Serialcommand::getcommand()
   // 收到有效数据
   if (recv_flag)
   {
-    // printf("rev end len:%d\n", datalen);
+    if (DEBUG_OUT) printf("rev end len:%d\n", datalen);
     recv_flag = false;
     // 处理数据
     if (checksum())
     {
       uint8_t commandid = inBuffer[1];
-      printf("rev ok cid:0x%02X\n", commandid);
+      printf("rev command id:0x%02X\n", commandid);
       switch (commandid)
       {
       // 放钩

src/config.h

@@ -15,6 +15,9 @@
 #define LOADCELL_DOUT_PIN 2
 #define LOADCELL_SCK_PIN 4
 ///////////////////////////////////////////
+#define SPEED_UP_SLOW 4   //rmp 或者  弧度/秒
+
+
 
 // 挂钩状态
 enum HookStatus

src/moto.cpp

@@ -0,0 +1,167 @@
+#include "moto.h"
+#include "Arduino.h"
+
+uint8_t CaninBuffer[8]; // 接收指令缓冲区
+moto_measure_t moto_chassis;
+PID_TypeDef moto_pid;
+
+moto::moto()
+{
+}
+bool moto::init()
+{
+  Serial.println("init moto");
+  pid_param_init(&moto_pid, 16000, 1000, 10, 8000, 0, 3.0f, 0.2f, 0.0f);
+  CAN.setPins(26, 27);
+  // start the CAN bus at 500 kbps
+  if (!CAN.begin(1000E3))
+  {
+    Serial.println("Starting CAN failed!");
+    return false;
+  }
+  CAN.onReceive(onReceive);
+  return true;
+}
+moto::~moto()
+{
+}
+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;
+}
+
+void moto::set_moto_current(int16_t iq1)
+{
+  iq1=MOTO_REVERSED*iq1;
+  CAN.beginPacket(0x200, 8);
+  CAN.write((uint8_t)((iq1 >> 8) & 0xFF));
+  CAN.write((uint8_t)(iq1 & 0xFF));
+  CAN.write(0x00);
+  CAN.write(0x00);
+  CAN.write(0x00);
+  CAN.write(0x00);
+  CAN.write(0x00);
+  CAN.write(0x00);
+  CAN.endPacket();
+}
+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->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->temperature = message_ptr[6];                                   // 电机温度，2006没有，为0
+
+
+
+
+  if (ptr->angle - ptr->last_angle > MOTO_MAXANG_HALF)
+    ptr->round_cnt--;
+  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()
+{
+  //在中断里面计算时间太长会导致重启,在取信息时计算浮点
+  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::onReceive(int packetSize)
+{
+  if (!CAN.packetRtr())
+  {
+    for (int i = 0; i < packetSize; i++)
+    {
+      CaninBuffer[i] = CAN.read();
+    }
+    get_moto_measure(&moto_chassis, CaninBuffer);
+  }
+}
+/////////////PID控制
+void pid_param_init(PID_TypeDef *pid,
+                    uint16_t maxout,
+                    uint16_t intergral_limit,
+                    float deadband,
+                    int16_t max_err,
+                    int16_t target,
+                    float kp,
+                    float ki,
+                    float kd)
+{
+
+  memset(pid, 0, sizeof(PID_TypeDef));
+
+  pid->DeadBand = deadband;
+  pid->IntegralLimit = intergral_limit;
+  pid->MaxOutput = maxout;
+  pid->Max_Err = max_err;
+  pid->target = target;
+
+  pid->kp = kp;
+  pid->ki = ki;
+  pid->kd = kd;
+
+  pid->output = 0;
+}
+
+inline void pid_reset(PID_TypeDef *pid, float kp, float ki, float kd)
+{
+  pid->kp = kp;
+  pid->ki = ki;
+  pid->kd = kd;
+}
+
+inline void pid_target(PID_TypeDef *pid, float new_target)
+{
+  pid->target = new_target;
+}
+
+float pid_cal(PID_TypeDef *pid, int16_t measure)
+{
+
+  pid->lasttime = pid->thistime;
+  pid->thistime = xTaskGetTickCount() * portTICK_RATE_MS; // 换算为绝对时间
+  pid->dtime = pid->thistime - pid->lasttime;             // 更新两次计算的时间
+
+  pid->last_err = pid->err;
+  pid->err = pid->target - measure;
+
+  if (abs(pid->err) > pid->DeadBand)
+  {
+    pid->pout = pid->kp * pid->err;
+    pid->iout += (pid->ki * pid->err);
+    pid->dout = pid->kd * (pid->err - pid->last_err); // 除以dtime？？
+
+    // 积分限幅
+    if (pid->iout > pid->IntegralLimit)
+      pid->iout = pid->IntegralLimit;
+    if (pid->iout < -pid->IntegralLimit)
+      pid->iout = -pid->IntegralLimit;
+
+    pid->output = pid->pout + pid->iout + pid->dout;
+
+    if (pid->output > pid->MaxOutput)
+    {
+      pid->output = pid->MaxOutput;
+    }
+    if (pid->output < -(pid->MaxOutput))
+    {
+      pid->output = -(pid->MaxOutput);
+    }
+  }
+
+  return pid->output; // 进入死区维持原值
+}

src/moto.h

@@ -0,0 +1,83 @@
+#pragma once
+#include <Arduino.h>
+#include <CAN.h>
+#include <ESP32SJA1000.h>
+#define MOTO_REVERSED  1     //正反转1为正转，-1为反转
+#define MOTO_REDUCTION 36
+#define MOTO_MAXANG 8192   //0-8191
+#define MOTO_MAXANG_HALF MOTO_MAXANG / 2
+
+
+typedef struct _PID_TypeDef
+{
+  float target; // 目标值====
+  float lastNoneZeroTarget;
+  float kp;
+  float ki;
+  float kd;
+  float measure;  // 测量值
+  float err;      // 误差
+  float last_err; // 上次误差
+  float pout;
+  float iout;
+  float dout;
+  float output; // 本次输出====
+  // float last_output;			//上次输出
+  float MaxOutput;     // 输出限幅=====
+  float IntegralLimit; // 积分限幅=====
+  float DeadBand;      // 死区（绝对值）=====
+  // float ControlPeriod;		//控制周期====
+  float Max_Err; // 最大误差====
+
+  uint32_t thistime;
+  uint32_t lasttime;
+  uint8_t dtime;
+
+} PID_TypeDef;
+
+typedef struct
+{
+  int16_t speed_rpm;
+  int16_t real_current;
+  uint8_t temperature;
+  uint16_t angle;      // abs angle range:[0,8191]
+  uint16_t last_angle; // abs angle range:[0,8191]
+  uint16_t offset_angle;
+  int32_t round_cnt;
+  int32_t total_angle;
+  uint32_t msg_cnt;
+
+  float output_speed_rpm;     //输出轴速度 ---rpm
+  float output_round_cnt;     //输出轴 总圈数
+
+} moto_measure_t;
+
+/// PID控制器
+void pid_param_init(PID_TypeDef *pid,
+                    uint16_t maxout,
+                    uint16_t intergral_limit,
+                    float deadband,
+                    int16_t max_err,
+                    int16_t target,
+                    float kp,
+                    float ki,
+                    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);
+
+class moto
+{
+private:
+  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 init();
+  void update();
+  void settarget(float new_target);
+  moto_measure_t getmotoinfo();
+};

src/motocontrol.cpp

@@ -0,0 +1,168 @@
+
+#include "Arduino.h"
+#include "motocontrol.h"
+#include "moto.h"
+
+#define DEBUG_OUT false
+Motocontrol::Motocontrol()
+{
+    _controlstatus.is_instore = false;
+    _controlstatus.is_setzero = false;
+    _controlstatus.zero_cnt = 0;
+    _controlstatus.motostatus = MotoStatus::MS_Stop;
+    _controlstatus.speed_targe = SPEED_SLOW;
+    _check_targetlength = false;
+}
+Motocontrol::~Motocontrol()
+{
+}
+
+bool Motocontrol::init() // 初始化
+{
+    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;
+    printf("stop \n");
+}
+void Motocontrol::setlocked(bool locked)
+{
+  if (locked)
+  {
+    stop();
+    setzero();
+  }
+  _controlstatus.is_toplocked=locked;
+}
+void Motocontrol::setzero() // 设置0长度位置
+{
+    // 记录0长度位置--以后需要保存到falsh里面
+    _controlstatus.zero_cnt = getmotoinfo().output_round_cnt;
+    _controlstatus.is_setzero = true;
+}
+int16_t Motocontrol::getlength() // 得到长度
+{
+    return 0;
+}
+void Motocontrol::checkstatus() // 检查状态，比如什么时候停
+{
+    moto_measure_t mf = _moto_dji.getmotoinfo();
+    // 已设置零点
+    if (_controlstatus.is_setzero)
+    {
+        // 总放线长度
+        float curr_length = abs(mf.output_round_cnt - _controlstatus.zero_cnt) * WHEEL_PERIMETER;
+        // 不能超ROPE_MAXLENGTH
+        if ((curr_length > ROPE_MAXLENGTH)&&(_controlstatus.motostatus == MS_Down))
+            stop();
+        // 开始入仓
+        if ((curr_length < INSTORE_LENGTH) && (_controlstatus.motostatus == MotoStatus::MS_Up))
+        {
+            _moto_dji.settarget(-SPEED_MIN);
+            _controlstatus.is_instore = true;
+        }
+        else
+            _controlstatus.is_instore = false;
+    }
+    if (_check_targetlength) // 有目标长度
+    {
+        if (_controlstatus.motostatus == MS_Down)
+        {
+            if (mf.output_round_cnt > _target_cnt)
+            {
+                printf("cnt:%.2f ,tar:%.2f\n", mf.output_round_cnt, _target_cnt);
+                stop();
+            }
+        }
+        else if (_controlstatus.motostatus == MS_Up)
+        {
+            if (mf.output_round_cnt < _target_cnt)
+            {
+                printf("cnt:%.2f ,tar:%.2f\n", mf.output_round_cnt, _target_cnt);
+                stop();
+            }
+        }
+    }
+}
+
+void Motocontrol::update() // 更新
+{
+    _moto_dji.update();
+    checkstatus();
+}
+void Motocontrol::direct_speed(float motospeed) // 直接控制电机--测试用
+{
+    _moto_dji.settarget(motospeed);
+}
+
+moto_measure_t Motocontrol::getmotoinfo()
+{
+    return _moto_dji.getmotoinfo();
+}
+control_status_t Motocontrol::getcontrolstatus() // 得到控制信息
+{
+    return _controlstatus;
+}
+// 按指定速度收放线 放线为+，收线为- 单位cm
+void Motocontrol::moto_run(MotoStatus updown, float length)
+{
+    // 传入要stop，直接停
+    if (updown == MS_Stop)
+    {
+        stop();
+        return;
+    }
+
+    // 运动中暂时不管
+    if ((_controlstatus.motostatus == MS_Down) || (_controlstatus.motostatus == MS_Up))
+        return;
+    // 没设置速度不转
+    if (_controlstatus.speed_targe == 0)
+        return;
+
+    _controlstatus.motostatus = updown;
+    // 有长度限制
+    if (length != 0)
+    {
+        // 收需要负
+        if (updown == MotoStatus::MS_Up)
+            length = -length;
+        // 记录开始位置
+        _start_cnt = _moto_dji.getmotoinfo().output_round_cnt;
+        _target_cnt = _start_cnt + length / WHEEL_PERIMETER;
+        _check_targetlength = true;
+    }
+    else
+        _check_targetlength = false;
+
+    // 记录开始位置
+    float runspeed;
+    if (updown == MotoStatus::MS_Down)
+        runspeed = _controlstatus.speed_targe;
+    else
+    {
+        // 如果没有设置零点，不能快速收线
+        if (!_controlstatus.is_setzero)
+            _controlstatus.speed_targe = SPEED_SLOW;
+        runspeed = -_controlstatus.speed_targe;
+    }
+    // 开始转
+    printf("run speed:%.0f,tarcnt:%.2f\n", runspeed, _target_cnt);
+    _moto_dji.settarget(runspeed);
+}

src/motocontrol.h

@@ -0,0 +1,70 @@
+#pragma once
+#include "Arduino.h"
+#include "config.h"
+#include "moto.h"
+#define ROPE_MAXLENGTH 600   //最多能放600cm---实际绳子应该比这个长650之类的
+#define WHEEL_DIAMETER 4.0   //轮子直径cm
+#define INSTORE_LENGTH 15   //入仓长度,低于该长度要缓慢入仓 cm
+
+#define SPEED_MAX 330   //最快收放线速度
+#define SPEED_MIN 3   //入仓速度
+#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)   //最大圈数
+
+
+
+// 挂钩状态
+enum MotoStatus
+{
+  MS_Down,   // 自动下放中
+  MS_Up,     // 回收中
+  MS_Stop   //停止中
+};
+
+typedef struct
+{
+  bool is_instore;
+  bool is_setzero;
+  bool is_toplocked;
+  float zero_cnt;
+  float speed_targe;
+  MotoStatus motostatus;
+
+} control_status_t;
+
+
+
+
+
+class Motocontrol
+{
+private:
+  moto _moto_dji;
+  float _start_cnt;
+  float _target_cnt;
+  control_status_t _controlstatus;
+  bool _check_targetlength;
+  
+  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(); // 初始化
+    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) ;// 放线
+
+};