Arduino控制小米微电机（基于MCP2515）运动

一、前言

最近接触到一个项目需要用到小米电机，本来是想用STM32进行控制，但是必须得使用Arduino进行开发，于是尝试了一下。小米电机是基于CAN通讯的，但恰好Arduino不带CAN通讯协议，于是难度上了一个档次。在查阅了相关资料后，偶然间看到B站某位大佬的视频，得到了启发，在此感谢这位大佬，并成功实现了目标功能，链接如下：

https://www.bilibili.com/video/BV1P64y1p7hx/?spm_id_from=333.999.0.0

二、准备

硬件：

1、MCP2515 SPI2CAN转换器 *1

2、小米微电机 *1

3、电机转接头 *1

4、Arduino Nano *1

5、转接线 *若干

6、usb to can转换头

知识储备：

1、掌握Arduino开发，能读懂程序

2、了解CAN协议

3、了解小米微电机相关参数，电机运转方式

三、接线及库的准备

参考国外一篇文章的CAN协议进行接线，链接如下：

https://gitcode.com/autowp/arduino-mcp2515/overview?utm_source=csdn_github_accelerator&isLogin=1

注意，小米电机供电为24V，电流在额定电流左右即可。具体相关参数在小米电机手册中查看。CAN通讯线最好双绞。

并下载相关库函数，导入我们的library文件夹中

CAN库需要我们另外下载，下载渠道很多，这里贴一个下载链接：

https://github.com/sandeepmistry/arduino-CAN

四、电机ID

使用usb2can转换头连接电脑，并进行如下设置：

设置好后打开小米提供的上位机更改ID为1

五、运行代码

xm_motor.cpp

#include "xm_motor.h"
#include <Arduino.h>
MCP2515 mcp2515(10);
struct can_frame canMsg;
uint32_t ExtId;             //定义can扩展id
uint8_t rx_data[8];         //接收数据
uint32_t Motor_Can_ID;      //接收数据电机ID
static uint8_t byte_ls[4];  //转换临时数据
uint8_t tx_data[8];         //can写入的数据

int filter(int queue[], char n)  //数值过滤
{
  int sum = 0;
  byte i;
  int maxsz = queue[0];  //寻找最大值
  int minsz = queue[0];  //寻找最小值
  for (i = 0; i < n; i++) {
    if (maxsz < queue[i]) { maxsz = queue[i]; }  //寻找最大值和最小值
    if (minsz > queue[i]) { minsz = queue[i]; }
  }

  for (i = 0; i < n; i++) {
    sum += queue[i];
  }
  sum = sum - maxsz - minsz;  //去除最大值和最小值
  return (sum / (n - 2));
}  //数值过滤
int check(byte ao_port, byte n)  //采样
{

  int check_date[n];  //定义采样数组
  for (byte i = 0; i < n; ++i) {
    check_date[i] = analogRead(ao_port);  //获得指定传感器数据
  }

  int vvvv = filter(check_date, n);
  return vvvv;

}  //采样
void xm_can_start() {

  mcp2515.reset();
  mcp2515.setBitrate(CAN_1000KBPS, MCP_8MHZ);
  mcp2515.setNormalMode();
  delay(4);
}
static uint8_t* Float_to_Byte(float f)  //float分解成四个byte数据
{
  unsigned long longdata = 0;
  longdata = *(unsigned long*)&f;
  byte_ls[0] = (longdata & 0xFF000000) >> 24;
  byte_ls[1] = (longdata & 0x00FF0000) >> 16;
  byte_ls[2] = (longdata & 0x0000FF00) >> 8;
  byte_ls[3] = (longdata & 0x000000FF);
  return byte_ls;
}
static float uint16_to_float(uint16_t x, float x_min, float x_max, int bits)  //把uint 16位数据变成浮点数 用在接受数据的处理上
{
  uint32_t span = (1 << bits) - 1;
  float offset = x_max - x_min;
  return offset * x / span + x_min;
}
static int float_to_uint(float x, float x_min, float x_max, int bits)  //把浮点数转换成uint_16 用在位置 扭矩 上面
{
  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);
}
void exid_count(uint8_t Communication_Type, uint16_t msid, uint8_t can_id)  //计算扩展ExtId,Communication_Type通信类型，msid主canid，
{
  uint8_t msid_l = msid;
  uint8_t msid_h = msid >> 8;
  uint32_t di_data = ((0xFFFFFFFF & Communication_Type) << 24) | 0x00FFFFFF;  //求出高32位
  uint32_t di_datab = ((0xFFFFFFFF & msid_h) << 16) | 0xFF00FFFF;             //求出高32位
  uint32_t di_datac = ((0xFFFFFFFF & msid_l) << 8) | 0xFFFF00FF;              //求出高32位
  uint32_t di_datad = (0xFFFFFFFF & can_id) | 0xFFFFFF00;                     //求出高32位
  ExtId = (di_data & di_datab & di_datac & di_datad);
}  //计算扩展ExtId,Communication_Type通信类型，msid主canid，

void data_count_dcs(uint16_t Index, float Value, char Value_type) {
  //计算can在 单参数写入，通信类型 12下发送的8位数据，Index 是命令类型0: 运控模式1: 位置模式2: 速度模式3: 电流模式Value是0 值，Value_type是数据类型，浮点数用f非浮点用s
  //速度数值要 注明浮点数 f ,
  //写入扭矩 n


  canMsg.data[0] = Index;
  canMsg.data[1] = Index >> 8;
  canMsg.data[2] = 0x00;
  canMsg.data[3] = 0x00;
  if (Value_type == 'f') {
    Float_to_Byte(Value);
    canMsg.data[4] = byte_ls[3];
    canMsg.data[5] = byte_ls[2];
    canMsg.data[6] = byte_ls[1];
    canMsg.data[7] = byte_ls[0];
  } else if (Value_type == 's') {
    canMsg.data[4] = (uint8_t)Value;
    canMsg.data[5] = 0x00;
    canMsg.data[6] = 0x00;
    canMsg.data[7] = 0x00;
  }
}  //计算can在 单参数写入，通信类型 12下发送的8位数据，Index 是命令类型Value是值，Value_type是数据类型，浮点数用f非浮点用s
void data_count_zero()  //can数据置零
{
  canMsg.data[0] = 0x00;
  canMsg.data[1] = 0x00;
  canMsg.data[2] = 0x00;
  canMsg.data[3] = 0x00;
  canMsg.data[4] = 0x00;
  canMsg.data[5] = 0x00;
  canMsg.data[6] = 0x00;
  canMsg.data[7] = 0x00;
}  //can数据置零

void motor_enable(uint8_t id = 1)  //电机使能 电机canid
{
  exid_count(3, Master_CAN_ID, id);
  canMsg.can_id = ExtId | CAN_EFF_FLAG;
  canMsg.can_dlc = 8;
  data_count_zero();
  mcp2515.sendMessage(MCP2515::TXB1, &canMsg);
  delay(4);
}  //电机使能 电机canid
void motor_mode(uint8_t id, char type)  //电机运行模式 电机canid  模式值1位置模式2速度模式 3 电流模式0运控模式
{
  exid_count(0x12, Master_CAN_ID, CanID);
  canMsg.can_id = ExtId | CAN_EFF_FLAG;  //ExtId  0x12000001
  canMsg.can_dlc = 8;
  data_count_dcs(0x7005, type, 's');
  mcp2515.sendMessage(MCP2515::TXB1, &canMsg);
  delay(4);
}

void motor_speed_value(uint8_t id, float speed_ref) {  //设置速度模式下的参数转速
  exid_count(0x12, Master_CAN_ID, CanID);
  canMsg.can_id = ExtId | CAN_EFF_FLAG;  //ExtId  0x12000001
  canMsg.can_dlc = 8;
  data_count_dcs(0x700A, speed_ref, 'f');
  mcp2515.sendMessage(MCP2515::TXB1, &canMsg);
  delay(4);
}

void motor_pos_zero(uint8_t id = 1)  //位置置0
{
  exid_count(6, Master_CAN_ID, id);
  canMsg.can_id = ExtId | CAN_EFF_FLAG;
  canMsg.can_dlc = 8;
  data_count_zero();
  canMsg.data[0] = 1;
  mcp2515.sendMessage(MCP2515::TXB1, &canMsg);
  delay(4);

}  //位置置0
void motor_pos_value(uint8_t id, float speed_ref) {  //设置位置模式下的位置
  exid_count(0x12, Master_CAN_ID, CanID);
  canMsg.can_id = ExtId | CAN_EFF_FLAG;  //ExtId  0x12000001
  canMsg.can_dlc = 8;
  data_count_dcs(0x7016, speed_ref, 'f');
  mcp2515.sendMessage(MCP2515::TXB1, &canMsg);
  delay(4);
}

void motor_pow_value(uint8_t id, float torque, float limit_cur, float kp = 1, float ki = 0.0158) {  //设置速度 电流限制，kp,kd
  exid_count(0x12, Master_CAN_ID, CanID);
  canMsg.can_id = ExtId | CAN_EFF_FLAG;  //ExtId  0x12000001
  canMsg.can_dlc = 8;
  data_count_dcs(0x7017, torque, 'f');
  mcp2515.sendMessage(MCP2515::TXB1, &canMsg);
  delay(4);
  data_count_dcs(0x7018, limit_cur, 'f');
  mcp2515.sendMessage(MCP2515::TXB1, &canMsg);
  delay(4);
  data_count_dcs(0x7010, kp, 'f');
  mcp2515.sendMessage(MCP2515::TXB1, &canMsg);
  delay(4);
  data_count_dcs(0x7011, ki, 'f');
  mcp2515.sendMessage(MCP2515::TXB1, &canMsg);
  delay(4);
}

xm_motor.h

#include "Arduino.h"
#include <SPI.h>
#include "mcp2515.h"

#define pi 3.14159265359f
#define Communication_Type_MotorEnable 0x03
#define Master_CAN_ID 0x00 
#define CanID 0x01
#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 MAX_P 720
#define MIN_P -720

#define Communication_Type_GetID 0x00           //获取设备的ID和64位MCU唯一标识符
#define Communication_Type_MotionControl 0x01 	//用来向主机发送控制指令
#define Communication_Type_MotorRequest 0x02	//用来向主机反馈电机运行状态
#define Communication_Type_MotorEnable 0x03	    //电机使能运行
#define Communication_Type_MotorStop 0x04	    //电机停止运行
#define Communication_Type_SetPosZero 0x06	    //设置电机机械零位
#define Communication_Type_CanID 0x07	        //更改当前电机CAN_ID
#define Communication_Type_Control_Mode 0x12
#define Communication_Type_GetSingleParameter 0x11	//读取单个参数
#define Communication_Type_SetSingleParameter 0x12	//设定单个参数
#define Communication_Type_ErrorFeedback 0x15	    //故障反馈帧
//参数读取宏定义
#define Run_mode 0x7005	
#define Iq_Ref   0x7006
#define Spd_Ref  0x700A
#define Limit_Torque 0x700B
#define Cur_Kp 0x7010
#define Cur_Ki 0x7011
#define Cur_Filt_Gain 0x7014
#define Loc_Ref 0x7016
#define Limit_Spd 0x7017
#define Limit_Cur 0x7018
#define Gain_Angle 720/32767.0
#define Bias_Angle 0x8000
#define Gain_Speed 30/32767.0
#define Bias_Speed 0x8000
#define Gain_Torque 12/32767.0
#define Bias_Torque 0x8000
#define Temp_Gain   0.1

#define Motor_Error 0x00
#define Motor_OK 0X01
 enum CONTROL_MODE   //控制模式定义
{
    Motion_mode = 0,//运控模式  
    Position_mode,  //位置模式
    Speed_mode,     //速度模式  
    Current_mode    //电流模式
};
enum ERROR_TAG      //错误回传对照
{
    OK                 = 0,//无故障
    BAT_LOW_ERR        = 1,//欠压故障
    OVER_CURRENT_ERR   = 2,//过流
    OVER_TEMP_ERR      = 3,//过温
    MAGNETIC_ERR       = 4,//磁编码故障
    HALL_ERR_ERR       = 5,//HALL编码故障
    NO_CALIBRATION_ERR = 6//未标定
};
typedef struct{           //小米电机结构体
	uint8_t CAN_ID;       //CAN ID
    uint8_t MCU_ID;       //MCU唯一标识符【后8位，共64位】
	float Angle;          //回传角度
	float Speed;          //回传速度
	float Torque;         //回传力矩
	float Temp;			  //回传温度
	
	uint16_t set_current;
	uint16_t set_speed;
	uint16_t set_position;
	
	uint8_t error_code;
	
	float Angle_Bias;
	
}MI_Motor;
 extern MI_Motor mi_motor;//预先定义1个小米电机

void xm_can_start();
void motor_enable( uint8_t id=1 ) ;
void motor_mode( uint8_t id ,char type );
void motor_speed_value( uint8_t id ,float speed_ref );//-30rad-30rad
void motor_yk( uint8_t id ,float torque, float MechPosition, float speed, float kp, float kd );
void motor_pos_zero( uint8_t id=1 ); //位置置0
void motor_pos_value( uint8_t id ,float speed_ref );
void motor_pow_value( uint8_t id , float torque,float limit_cur ,float  kp,float kd );

int filter(int queue[], char n) ; //数值过滤
int check(byte ao_port, byte n); //获取ad口电压

 main.ino

#include "xm_motor.h"
String comdata = "";  //蓝牙字符
void setup() {
  while (!Serial)
    ;
  Serial.begin(115200);

  xm_can_start();                         //初始化can设置
  motor_enable(1);                        //使能id 1电机
  motor_pos_zero(1);                      //位置置0
  motor_mode(1, 1);                       //电机运行模式 电机canid 模式值 1位置模式2速度模式 3 电流模式0运控模式
  motor_pow_value(1, 30, 20, 0.2, 0.13);  //uint8_t id , float torque 位置模式速度限制 ,float limit_cur ,float  kp=0.8,float ki=0.13  id 速度 电流限制，kp,kd  速度 0~30rad/s 6.28rad 等于1圈  电流最大23A
  Serial.println("Example: Write to CAN");
}
void loop() {

  int speed_value = 100;
  int speed_valueb = map(speed_value, 0, 1023, 0, 30);  //前进模拟量
  float bb = (float)speed_value;
  bb = bb / 163.9423;
  if (bb > 6.2) { bb = 6.2; }
  motor_pos_value(1, bb);          //电机位置模式赋值，id，位置角度rad 2派=360度。

  delay(20);
}

在main.ino中更改loop里speed_value的值即可更改角度，在0-1023范围内进行更改。

六、运行结果

使用桌面电源供电时注意每次更改值的改动不要过大，以免转动猛烈造成桌面电源损坏。