ESP32实现TWAI CAN通信驱动小米电机（CyberGear微电机）Arduino库详解

文章目录

前言

鉴于项目要使用小米电机，并且要使用esp32上自带的CAN库来控制，但是没有找到合适的、能用的库，只能从现有STM32小米电机控制库的基础上进行修改。为了方便快速实现，采用Arduino的平台对esp32进行编程，对应需要修改成Arduino的库

一、前置任务

1. CAN通信基础
2. ESP32自带CAN控制器—TWAI：官方文档，文章介绍
3. 相关教程：CAN通信，Arduino外接CAN控制，
   STM32F103用CAN通讯驱动小米电机讲解（视频）

上面这个视频是本库的关键参考，UP讲的非常细致，基本操作都是一样的，就不再赘述了（重要！！！）

二、硬件配置

1. esp32×1
2. TTL转CAN模块（是一种电平转换模块）×1
3. 小米微电机×1 杜邦线×4，CAN线×2
4. 24V电池×1
5. USB转CAN模块（连接上位机查看数据，控制时不需要）

三、线路连接

四、引入底层库函数

由于esp32的Arduino库不自带CAN的库（也就是TWAI的库），需要自己去搜索下载

ESP32-TWAI-CAN

1.Arduino原生ide

2.VScode+PlatformIO

五、小米电机控制库（C++文件，H文件）

1.H文件

#ifndef _TWAI_CAN_MI_MOTOR_H__
#define _TWAI_CAN_MI_MOTOR_H__
/*
	驱动器通信协议及使用说明 
	
  电机通信为 CAN 2.0 通信接口，波特率 1Mbps，采用扩展帧格式，如下所示：
  数据域          29 位 ID        8Byte 数据区
  大小 Bit28~bit24 bit23~8 bit7~0 Byte0~Byte7
  描述 通信类型 数据区 2 目标地址 数据区 1

  电机支持的控制模式包括：
  运控模式：给定电机运控 5 个参数；
  电流模式：给定电机指定的 Iq 电流；
  速度模式：给定电机指定的运行速度；
  位置模式：给定电机指定的位置，电机将运行到该指定的位置；

  本台测试用的小米电机CAN ID=1，主机ID=0

  注意配置顺序：
      1. 电机CAN初始化->电机ID初始化->设定电机机械零点->设置电机控制模式->设置对应模式的控制参数->使能(启动)电机
      2. 必须给电机发数据才会有数据返回

  测试用例：
  MI_Motor_ M1_con;

  void setup() {
    Motor_CAN_Init();
    M1_con.Motor_Con_Init(MOTER_1_ID);
    M1_con.Motor_Set_Zero();
    M1_con.Change_Mode(SPEED_MODE);
    M1_con.Motor_Enable();
  }

  void loop() {
      M1_con.Motor_Data_Updata(20);
      Serial.printf("M1数据: %d,%d,%d,%d,%d,%d,%d,%d,%d,angle:%.2f,speed:%.2f,torque:%.2f,temp:%.2f,\r\n",
                                    M1_con.motor_rx_data.master_id,M1_con.motor_rx_data.motor_id,
                                    M1_con.motor_rx_data.err_sta,M1_con.motor_rx_data.HALL_err,
                                    M1_con.motor_rx_data.magnet_err,M1_con.motor_rx_data.temp_err,
                                    M1_con.motor_rx_data.current_err,M1_con.motor_rx_data.voltage_err,
                                    M1_con.motor_rx_data.mode_sta,
                                    M1_con.motor_rx_data.cur_angle,M1_con.motor_rx_data.cur_speed,
                                    M1_con.motor_rx_data.cur_torque,M1_con.motor_rx_data.cur_temp);
      M1_con.Set_SpeedMode(-1);
      vTaskDelay(20);
  }
*/

#include"Arduino.h"
#include <ESP32-TWAI-CAN.hpp>

/*CAN设置*/
#define CAN_TX          5
#define CAN_RX          4

#define MASTER_ID       0
#define MOTER_1_ID      1
#define MOTER_2_ID      2

/*基础配置*/
#define P_MIN           -12.5f 
#define P_MAX           12.5f 
#define V_MIN           -30.0f 
#define V_MAX           30.0f 
#define KP_MIN          0.0f 
#define KP_MAX          500.0f 
#define KD_MIN          0.0f 
#define KD_MAX          5.0f 
#define T_MIN           -12.0f 
#define T_MAX           12.0f

/*数据配置*/
#define RUN_MODE        0x7005    // 运行模式, uint8, 1字节, 0运控模式,1位置模式，2速度模式，3电流模式
#define CTRL_MODE       0
#define POS_MODE        1
#define SPEED_MODE      2
#define CUR_MODE        3
#define IQ_REF          0x7006    // 电流模式 Iq 指令, float, 4字节, -23~23A
#define SPD_REF         0x700A    // 转速模式转速指令, float, 4字节, -30~30rad/s
#define LIMIT_TORQUE    0x700B    // 转矩限制, float, 4字节, 0~12Nm
#define CUR_KP          0x7010    // 电流的 Kp, float, 4字节, 默认值 0.125
#define CUR_KI          0x7011    // 电流的 Ki, float, 4字节, 默认值 0.0158
#define CUR_FILT_GAIN   0x7014    // 电流滤波系数 filt_gain, float, 4字节, 0~1.0，默认值 0.1
#define LOC_REF         0x7016    // 位置模式角度指令, float, 4字节, rad
#define LIMIT_SPD       0x7017    // 位置模式速度限制, float, 4字节, 0~30rad/s
#define LIMIT_CUR       0x7018    // 速度位置模式电流限制, float, 4字节, 0~23A
#define MECH_POS        0x7019    // 负载端计圈机械角度, float, 4字节, rad, 只读
#define IQF             0x701A    // iq 滤波值, float, 4字节, -23~23A, 只读
#define MECH_VEL        0x701B    // 负载端转速, float, 4字节, -30~30rad/s, 只读
#define VBUS            0x701C    // 母线电压, float, 4字节, V, 只读
#define ROTATION        0x701D    // 圈数, int16, 2字节, 圈数, 可读写
#define LOC_KP          0x701E    // 位置的 kp, float, 4字节, 默认值 30, 可读写
#define SPD_KP          0x701F    // 速度的 kp, float, 4字节, 默认值 1, 可读写
#define SPD_KI          0x7020    // 速度的 ki, float, 4字节, 默认值 0.002, 可读写

//拆分29位ID
#define RX_29ID_DISASSEMBLE_MASTER_ID(id)       (uint8_t)((id)&0xFF)
#define RX_29ID_DISASSEMBLE_MOTOR_ID(id)        (uint8_t)(((id)>>8)&0xFF)
//右移16位获取所有故障信息(bit21-16，共6位，111111->0x3F)，三元运算符判断是否有故障。0无1有
#define RX_29ID_DISASSEMBLE_ERR_STA(id)         (uint8_t)(((((id)>>16)&0x3F) > 0) ? 1 : 0)
#define RX_29ID_DISASSEMBLE_HALL_ERR(id)        (uint8_t)(((id)>>20)&0X01)
#define RX_29ID_DISASSEMBLE_MAGNET_ERR(id)      (uint8_t)(((id)>>19)&0x01)
#define RX_29ID_DISASSEMBLE_TEMP_ERR(id)        (uint8_t)(((id)>>18)&0x01)
#define RX_29ID_DISASSEMBLE_CURRENT_ERR(id)     (uint8_t)(((id)>>17)&0x01)
#define RX_29ID_DISASSEMBLE_VOLTAGE_ERR(id)     (uint8_t)(((id)>>16)&0x01)
//模式状态: 0:Reset模式[复位]; 1:Cali 模式[标定]; 2:Motor模式[运行]
#define RX_29ID_DISASSEMBLE_MODE_STA(id)        (uint8_t)(((id)>>22)&0x03)

//拆分数据
#define RX_DATA_DISASSEMBLE_CUR_ANGLE(data)     (uint16_t)((data[0]<<8)|data[1])
#define RX_DATA_DISASSEMBLE_CUR_SPEED(data)     (uint16_t)((data[2]<<8)|data[3])
#define RX_DATA_DISASSEMBLE_CUR_TORQUE(data)    (uint16_t)((data[4]<<8)|data[5])
#define RX_DATA_DISASSEMBLE_CUR_TEMP(data)      (uint16_t)((data[6]<<8)|data[7])

//转换系数
//转换过程：angle=data×8PI/65535-4PI,   范围：+-4PI
#define INT2ANGLE     0.000383501049
//转换过程：speed=data×60/65535-30,     范围：+-30rad/s
#define INT2SPEED     0.000915541314
//转换过程：torque=data×24/65535-12,    范围：+-12N·m
#define INT2TORQUE    0.000366216526

//发送数据包
typedef struct {
  uint32_t id:8;      //8位CAN ID
  uint32_t data:16;   //16位数据
  uint32_t mode:5;    //5位模式
  uint32_t res:3;     //3位保留
  uint8_t tx_data[8];
}can_frame_t;

//解析返回数据包
typedef struct {
  //29位ID解码状态
  uint8_t master_id;
  uint8_t motor_id;
  uint8_t err_sta;
  uint8_t HALL_err;
  uint8_t magnet_err;
  uint8_t temp_err;
  uint8_t current_err;
  uint8_t voltage_err;
  uint8_t mode_sta;

  //具体数据
  float cur_angle;      //(-4π~4π)
  float cur_speed;      //(-30rad/s~30rad/s)
  float cur_torque;     //（-12Nm~12Nm）
  float cur_temp;       //Temp(摄氏度）*10
}can_rx_frame_t;

class MI_Motor_
{
    private:

        uint8_t id;
        CanFrame rxFrame;               //接收原始数据
        
    public:

        can_rx_frame_t motor_rx_data;   //返回解析数据
            
        void Motor_Enable(void);

        void Motor_Reset(void);

        void Motor_Set_Zero(void);

        void Motor_ControlMode(float torque, float MechPosition, float speed, float kp, float kd);

        void Set_SingleParameter(uint16_t parameter,float index);

        void Decoding_Motor_data(CanFrame* can_Frame_point, can_rx_frame_t* motor_frame_point);

        void Set_SpeedMode(float speed);

        void Set_PosMode(float position,float max_speed);

        void Set_CurMode(float current);

        void Change_Mode(uint8_t mode);

        void Motor_Con_Init(uint8_t motor_id);

        uint8_t Motor_Data_Updata(uint32_t timeout);
};

extern void Motor_CAN_Init(void);

#endif

2.CPP文件

#include "Arduino.h"
#include "TWAI_CAN_MI_Motor.h"

//把浮点数转换成uint_16 用在位置 扭矩 上面
static int float_to_uint(float x, float x_min, float x_max, int bits)
{
  float span = x_max - x_min;
  float offset = x_min;
  if (x > x_max) x = x_max;
  else if (x < x_min) x = x_min;
  return (int)((x - offset) * ((float)((1 << bits) - 1)) / span);
}

//底层的CAN发送指令，小米电机采用扩展帧，数据帧的格式
static void CAN_Send_Frame(can_frame_t* frame)
{
    CanFrame obdFrame = { 0 };
    uint32_t id_val,data_val,mode_val;
    uint32_t combined_val;

    obdFrame.extd = 1;              //0-标准帧; 1-扩展帧
    obdFrame.rtr = 0;               //0-数据帧; 1-远程帧
    obdFrame.ss = 0;                //0-错误重发; 1-单次发送(仲裁或丢失时消息不会被重发)，对接收消息无效
    obdFrame.self = 0;              //0-不接收自己发送的消息，1-接收自己发送的消息，对接收消息无效
    obdFrame.dlc_non_comp = 0;      //0-数据长度不大于8(ISO 11898-1); 1-数据长度大于8(非标);
    //拼接ID
    id_val = frame->id;
    data_val = frame->data;
    mode_val = frame->mode;
    combined_val |= (mode_val << 24);
    combined_val |= (data_val << 8);
    combined_val |= id_val;

    obdFrame.identifier = combined_val; //普通帧直接写id，扩展帧需要计算。11/29位ID
    obdFrame.data_length_code = 8;      //要发送的字节数

    for (int i = 0; i < 8; i++)
    {
    obdFrame.data[i] = frame->tx_data[i];
    }
    ESP32Can.writeFrame(obdFrame);
}

/**
      * @brief          电机使能
      */
void MI_Motor_::Motor_Enable(void)
{
    can_frame_t motor_con_frame;
    motor_con_frame.mode = 3;           //模式3，使能
    motor_con_frame.id = this->id;      //目标电机ID
    motor_con_frame.data = MASTER_ID;   //本机ID
    for (int i = 0; i < 8; i++)
    {
        motor_con_frame.tx_data[i] = 0;
    }
    CAN_Send_Frame(&motor_con_frame);
}

/**
      * @brief          电机停止
      */
void MI_Motor_::Motor_Reset(void)
{
    can_frame_t motor_con_frame;
    motor_con_frame.mode = 4;           //模式4，停止
    motor_con_frame.id = this->id;      //目标电机ID
    motor_con_frame.data = MASTER_ID;   //本机ID
    for (int i = 0; i < 8; i++)
    {
        motor_con_frame.tx_data[i] = 0;
    }
    CAN_Send_Frame(&motor_con_frame);
}

/**
      * @brief          设置电机机械零点
      */
void MI_Motor_::Motor_Set_Zero(void)
{
    can_frame_t motor_con_frame;
    motor_con_frame.mode = 6;           //模式6，设置零点
    motor_con_frame.id = this->id;      //目标电机ID
    motor_con_frame.data = MASTER_ID;   //本机ID
    for (int i = 0; i < 8; i++)
    {
        motor_con_frame.tx_data[i] = 0;
    }
    CAN_Send_Frame(&motor_con_frame);
}

/**
      * @brief          运动控制模式
      * @param[in]      torque 扭矩，N·m，-12到+12
      * @param[in]      MechPosition 角度，rad，-12.5到+12.5
      * @param[in]      speed 角速度，rad/s，-30到+30
      */
void MI_Motor_::Motor_ControlMode(float torque, float MechPosition, float speed, float kp, float kd)
{
    can_frame_t motor_con_frame;
    motor_con_frame.mode = 1;           //模式1,运控模式
    motor_con_frame.id = this->id;      //目标电机ID
    motor_con_frame.data = float_to_uint(torque,T_MIN,T_MAX,16);

    motor_con_frame.tx_data[0]=float_to_uint(MechPosition,P_MIN,P_MAX,16)>>8;
    motor_con_frame.tx_data[1]=float_to_uint(MechPosition,P_MIN,P_MAX,16);
    motor_con_frame.tx_data[2]=float_to_uint(speed,V_MIN,V_MAX,16)>>8;
    motor_con_frame.tx_data[3]=float_to_uint(speed,V_MIN,V_MAX,16);
    motor_con_frame.tx_data[4]=float_to_uint(kp,KP_MIN,KP_MAX,16)>>8;
    motor_con_frame.tx_data[5]=float_to_uint(kp,KP_MIN,KP_MAX,16);
    motor_con_frame.tx_data[6]=float_to_uint(kd,KD_MIN,KD_MAX,16)>>8;
    motor_con_frame.tx_data[7]=float_to_uint(kd,KD_MIN,KD_MAX,16);
    CAN_Send_Frame(&motor_con_frame);
}

/**
      * @brief          设置单参数
      * @param[in]      parameter 参数数据
      * @param[in]      index 参数列表，详见电机说明书的4.1.11
      */
void MI_Motor_::Set_SingleParameter(uint16_t parameter,float index)
{
    can_frame_t motor_con_frame;
    motor_con_frame.mode = 18;          //模式18,单参数修改模式
    motor_con_frame.id = this->id;      //目标电机ID
    motor_con_frame.data = MASTER_ID;   //来标识主CAN_ID

    motor_con_frame.tx_data[2]=0;
    motor_con_frame.tx_data[3]=0;     //Byte2~3: 00  

    memcpy(&motor_con_frame.tx_data[0],&parameter,2);   //Byte0~1: index，参数列表详见4.1.11
    memcpy(&motor_con_frame.tx_data[4],&index,4);       //Byte4~7: 参数数据，1字节数据在Byte4 
    CAN_Send_Frame(&motor_con_frame);
}

/**
      * @brief          解析电机发送数据包
      * @param[in]      can_Frame_point CAN原始数据包的指针
      * @param[in]      motor_frame_point 电机解析数据包的指针
      */
void MI_Motor_::Decoding_Motor_data(CanFrame* can_Frame_point, can_rx_frame_t* motor_frame_point)
{
    motor_frame_point->master_id = RX_29ID_DISASSEMBLE_MASTER_ID(can_Frame_point->identifier);
    motor_frame_point->motor_id = RX_29ID_DISASSEMBLE_MOTOR_ID(can_Frame_point->identifier);
    motor_frame_point->err_sta = RX_29ID_DISASSEMBLE_ERR_STA(can_Frame_point->identifier);
    motor_frame_point->HALL_err = RX_29ID_DISASSEMBLE_HALL_ERR(can_Frame_point->identifier);
    motor_frame_point->magnet_err = RX_29ID_DISASSEMBLE_MAGNET_ERR(can_Frame_point->identifier);
    motor_frame_point->temp_err = RX_29ID_DISASSEMBLE_TEMP_ERR(can_Frame_point->identifier);
    motor_frame_point->current_err = RX_29ID_DISASSEMBLE_CURRENT_ERR(can_Frame_point->identifier);
    motor_frame_point->voltage_err = RX_29ID_DISASSEMBLE_VOLTAGE_ERR(can_Frame_point->identifier);
    motor_frame_point->mode_sta = RX_29ID_DISASSEMBLE_MODE_STA(can_Frame_point->identifier);

    //弧度单位
    motor_frame_point->cur_angle = RX_DATA_DISASSEMBLE_CUR_ANGLE(can_Frame_point->data)*INT2ANGLE - 4*PI;
    motor_frame_point->cur_speed = RX_DATA_DISASSEMBLE_CUR_SPEED(can_Frame_point->data)*INT2SPEED -30;
    motor_frame_point->cur_torque = RX_DATA_DISASSEMBLE_CUR_TORQUE(can_Frame_point->data)*INT2TORQUE - 12;
    motor_frame_point->cur_temp = RX_DATA_DISASSEMBLE_CUR_TEMP(can_Frame_point->data)/10.f;
}

/**
      * @brief          设置速度模式的参数
      * @param[in]      speed 角速度，rad/s，-30到+30
      */
void MI_Motor_::Set_SpeedMode(float speed)
{
    Set_SingleParameter(SPD_REF, speed);
}

/**
      * @brief          设置位置模式的参数
      * @param[in]      position 角度，rad，-12.5到+12.5
      * @param[in]      max_speed 角速度，rad/s，-30到+30
      */
void MI_Motor_::Set_PosMode(float position,float max_speed)
{
    Set_SingleParameter(LIMIT_SPD, max_speed);
    Set_SingleParameter(LOC_REF, position);
}

/**
      * @brief          设置电流模式的参数
      * @param[in]      current 电流，A，-23到+23
      */
void MI_Motor_::Set_CurMode(float current)
{
    Set_SingleParameter(IQ_REF, current);
}

/**
      * @brief          设置运动模式
      * @param[in]      mode 运控模式：CTRL_MODE 电流模式：CUR_MODE 速度模式：SPEED_MODE 位置模式：POS_MODE
      */
void MI_Motor_::Change_Mode(uint8_t mode)
{
    uint16_t parameter=RUN_MODE;
    can_frame_t motor_con_frame;
    motor_con_frame.mode=18;  //模式18，单参数写入
    motor_con_frame.data=MASTER_ID;
    motor_con_frame.id=this->id;    //电机ID
    for(int j=0;j<=7;j++){
    motor_con_frame.tx_data[j]=0;
    }	
    motor_con_frame.tx_data[4]=mode;
    memcpy(&motor_con_frame.tx_data[0],&parameter,2);
    CAN_Send_Frame(&motor_con_frame);
}

/**
      * @brief          电机初始化，在上位机查看电机ID和主机ID，主机ID默认为0
      * @param[in]      motor_id 电机ID
      */
void MI_Motor_::Motor_Con_Init(uint8_t motor_id)
{
    this->id = motor_id;
}

/**
      * @brief          CAN初始化
      */
void Motor_CAN_Init(void)
{
    ESP32Can.setPins(CAN_TX, CAN_RX);                //设置pin脚
    ESP32Can.setRxQueueSize(50);                     //设置缓存区大小
    ESP32Can.setTxQueueSize(50);                     //设置缓存区大小
    ESP32Can.setSpeed(ESP32Can.convertSpeed(1000));  //设置速度，一定要1M，其它库都没有1M，就这个库有
    ESP32Can.begin();
    // if (ESP32Can.begin()) {
    //     Serial.println("CAN bus started!");
    // } else {
    //     Serial.println("CAN bus failed!");
    // }
}

/**
      * @brief          更新电机数据
      * @param[in]      timeout 超时时间
      * @retval         err_sta 0：正常读取；1：读取到数据，但不是自己的；2：没有读取到数据
      */
uint8_t MI_Motor_::Motor_Data_Updata(uint32_t timeout)
{
    uint8_t err_sta=0;
    if (ESP32Can.readFrame(rxFrame, timeout)){
        if (RX_29ID_DISASSEMBLE_MOTOR_ID(rxFrame.identifier) == this->id){
            Decoding_Motor_data(&rxFrame,&motor_rx_data);
        }else{
            err_sta = 1;
        }
    }else{
        err_sta = 2;
    }

    return err_sta;
}

3.使用例程

#include <Arduino.h>
#include "TWAI_CAN_MI_Motor.h"

MI_Motor_ M1_con;

void setup() {
  // put your setup code here, to run once:
  Serial.begin(115200);

  Motor_CAN_Init();
  M1_con.Motor_Con_Init(MOTER_1_ID);
  M1_con.Motor_Set_Zero();
  M1_con.Change_Mode(SPEED_MODE);
  M1_con.Motor_Enable();
}

void loop() {
    M1_con.Motor_Data_Updata(20);
    Serial.printf("M1数据: %d,%d,%d,%d,%d,%d,%d,%d,%d,angle:%.2f,speed:%.2f,torque:%.2f,temp:%.2f,\r\n",
                                  M1_con.motor_rx_data.master_id,M1_con.motor_rx_data.motor_id,
                                  M1_con.motor_rx_data.err_sta,M1_con.motor_rx_data.HALL_err,
                                  M1_con.motor_rx_data.magnet_err,M1_con.motor_rx_data.temp_err,
                                  M1_con.motor_rx_data.current_err,M1_con.motor_rx_data.voltage_err,
                                  M1_con.motor_rx_data.mode_sta,
                                  M1_con.motor_rx_data.cur_angle,M1_con.motor_rx_data.cur_speed,
                                  M1_con.motor_rx_data.cur_torque,M1_con.motor_rx_data.cur_temp);
    M1_con.Set_SpeedMode(-0.1);
    vTaskDelay(20);
}

总结

CAN的数据发送应该没什么问题（需要电机先启动，再发控制指令），数据接收暂时还是轮询模式，不过配置有队列应该问题不大，有时间看看能不能改成中断处理

作者：C語言的力量