【关节电机专栏】ESP32 TWAI CAN Arduino库驱动小米电机（CyberGear微电机）
转载声明
本文转载自 https://blog.csdn.net/doudou2weiwei/article/details/142696510
作者：C語言的力量
前言
鉴于项目要使用小米电机，并且要使用esp32上自带的CAN库来控制，但是没有找到合适的、能用的库，只能从现有STM32小米电机控制库的基础上进行修改。为了方便快速实现，采用Arduino的平台对esp32进行编程，对应需要修改成Arduino的库
一、前置任务
- CAN通信基础
- ESP32自带CAN控制器—TWAI：官方文档，文章介绍
- 相关教程：CAN通信，Arduino外接CAN控制，
STM32F103用CAN通讯驱动小米电机讲解（视频）
上面这个视频是本库的关键参考，UP讲的非常细致，基本操作都是一样的，就不再赘述了（重要！！！）
二、硬件配置
- esp32×1
- TTL转CAN模块（是一种电平转换模块）×1
- 小米微电机×1 杜邦线×4，CAN线×2
- 24V电池×1
- USB转CAN模块（连接上位机查看数据，控制时不需要）
三、线路连接
- esp32的4号引脚连接转换模块的RX，esp32的5号引脚连接转换模块的TX（esp32默认的CAN引脚）；
- 转换模块的CANL连接电机的CANL，转换模块的CANH连接电机的CANH；
- VCC接VCC，GND接GND；
- 连接24V电源；
四、引入底层库函数
由于esp32的Arduino库不自带CAN的库（也就是TWAI的库），需要自己去搜索下载
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);
#endif2.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],¶meter,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],¶meter,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的数据发送应该没什么问题（需要电机先启动，再发控制指令），数据接收暂时还是轮询模式，不过配置有队列应该问题不大，有时间看看能不能改成中断处理
