一、电机相关

电机作为一种能将电能转化为机械能的装置，其在制造、医疗、运动控制等等许多地方都起着重要的作用。想学习了解机器人的小伙伴，从电机了解起走也是一条不错（坎坷）的道路。

0、废话

   其实电机对于我来说是接触的比较多了的，记得小时候玩四驱车，就特意将“马达”拆开来看过想搞懂原理，也单独将电机拿出来制作了一些小的diy，后来到了高中在学到了电磁学，算是了解了基础的原理了（在不停的刷题后），再后来到大学就是真正的使用了。第一次使用应该是在大一买的单片机，配了一个电机，有程序可以进行调速，但当时由于一些原因，没有再去使用。又到了大三，学习了电机拖动，对电机的认识又深了一点，也做了一些关于电机的实验。但是，令我难以忘记的是研究生开始调试电机的时候，真的是…一言难尽，之前也参考过许多大佬的博客，所以想把自己的这段难忘的经历做个总结，也给有需要的朋友一个参考。

1、电机种类

• 按电源种类分为：直流电机和交流电机。我们常见常用的电机大多是直流电机，相比前者，交流电机不需要换向器和电刷转换电流方向，与直流电机相比它的结构更简单，功率更大，在工业领域被广泛应用
  

• 根据有无电刷分为：有刷电机和无刷电机。有刷电机结构简单、开发久技术成熟、响应速度快，起动扭矩大、运行平稳，起制动效果好、控制精度高、使用成本低，维修方便；而无刷电机由于无电刷，具有低干扰、噪音低、运转顺畅、寿命长、低维护成本等优点。于是我接触的以无刷电机为主。
  

• 根据有无反馈分为：步进电机和伺服电机。前者没有反馈信号，位置精度不够高，且转速远远小于后者。在需要精确的控制，伺服电机更加常用。

2、电机控制方式

• 力矩控制：指定电机提供设置大小的力矩。（但是由于力矩传感器太贵了，这里的力矩的大小通常是通过电流换算的，其恒电流情况下，转矩=转矩常数*电流）
• 速度控制：指定电机达到设置的速度转动。
• 位置控制：指定电机转动到设置的位置。

由于位置是速度的积分，所以三种控制方式的控制框图是有要求的，下面是一种常见的控制结构图，当然，如果只针对某一两种控制模式，其控制方案将比这个更加简易。

二、电机控制器

    为了方便用户的使用，市面上许多电机都是针对上面的控制方式进行了封装的，也就是我们常听说的——控制器。控制器的控制方案有许多，针对不同的控制环也有不同的控制方案，例如：对于电流环，有FOC矢量控制，速度、位置环有PID。当然，也有其他的控制算法，但这里我们就使用常用的就行了。现在我们也可以开始谈谈标题了。

1、PID控制器

    PID 是一种传统且经典的控制算法，在工程中应用非常广泛，相比其他高大上甚至只存在于 paper 上的算法， PID 是非常接地气的。
    PID ，即：Proportional（比例）、Integral（积分）、Differential（微分）的缩写。顾名思义，PID 控制算法是结合比例、积分和微分的融合怪，其规律可以描述为：
u ( t ) = K p ( e ( t ) + 1 T t ∫ 0 t e ( t ) d t + T d d e ( t ) d t u(t)=K_p(e(t)+\frac{1}{T_t}\int_0^t e(t)dt+T_d\frac{de(t)}{dt} u(t)=Kp​(e(t)+Tt​1​∫0t​e(t)dt+Td​dtde(t)​
    其中 K p K_p Kp​ 是比例增益， T t T_t Tt​ 是积分时间常数， T d T_d Td​ 是微分时间常数， u ( t ) u(t) u(t) 是输入信号， e ( t ) e(t) e(t) 是误差。

    三个环节在控制中也分别起着不同的控制作用。

2、PID 各环节作用

• 比例环节 P：比例环节与稳态误差相关，比例环节越大，上升速度越快，且稳态误差越小，但无论怎样多大都会存在误差，不能消除误差，而且过大还会导致震荡，反而不稳定
  

• 积分环节 I：积分环节则可以消除误差，合适的积分环节可以很快的消除误差，但是设置较大会产生超调，并且过大也会导致震荡，从而不稳定
  

• 微分环节 D：微分环节具有预测作用，可以预测信号的变化方向，从而可以减小超调，提高响应速度，但过大会导致系统不稳定
  

matlab PID 的参考代码如下（上面的图是在下面代码基础上修改了一点，但是核心没有变）：

%%  说明
% 被控系统： 1/(0.1s+1)
% 控制器：    PID
%%
clc,clear
ts=0.001;  %采样时间=0.001s
sys=tf(1,[0.1,1]);            %建立被控对象传递函数
dsys=c2d(sys,ts,'z')         % 离散化
[num,den]=tfdata(dsys,'v');   % 得到差分方程系数  y(k) = -den[2]*y(k-1) + num[2]*u(k-1)
e_last=0;      %前一时刻的偏差      
E_integ=0;     %累积偏差
u_last=0.0;    %前一时刻的控制量
y_last=0;      %前一时刻的输出
% PID参数
kp=1;    
ki=0;
kd=0;
u=zeros(1,10000);    %设置仿真长度
time=zeros(1,10000); %时刻点（设定10000个）
for k=1:1:10000
    time(k)=k*ts;    %时间
    r(k)=100;        %期望值
    y(k)=-1*den(2)*y_last + num(2)*u_last;    %系统响应输出序列
    e(k)=r(k)-y(k);                           %误差信号
    u(k)=kp*e(k)+ki*E_integ+kd*(e(k)-e_last); %系统PID控制器输出序列
    E_integ=E_integ+e(k);    %误差的累加和
    u_last=u(k);    	     %前一个的控制器输出值
    y_last=y(k);    	     %前一个的系统响应输出值
    e_last=e(k);		     %前一个误差信号的值
end
p1=plot(time,r,'-.');xlim([0,1]);hold on; %指令信号的曲线（即期望输入）
p2=plot(time,y,'--');xlim([0,1]);         %不含积分分离的PID曲线
hold on;

3、PID 种类

    上面的 PID公示 是针对连续情况下的，而在生活中，我们常常使用的是离散型的变量，比如时间，于是我们需要将 PID 的公式进行离散化，根据离散化的方法不同，PID 控制的公式就有两种，即位置式 PID 和增量式 PID,

• 位置式 PID
  u ( n ) = K p ∗ e ( n ) + K i ∗ ∑ e ( n ) + K d ∗ [ e ( n ) − e ( n − 1 ) ] / T u(n)=K_p*e(n)+K_i*\sum e(n)+K_d*[e(n)-e(n-1)]/T u(n)=Kp​∗e(n)+Ki​∗∑e(n)+Kd​∗[e(n)−e(n−1)]/T
  从公式结构上看，位置式存在积分环节，误差会进行累加，当积分项饱和时，误差仍然会进行累加，当误差反向变化时，系统还需要一定时间从饱和区退出，所以常常需要积分限幅和输出限幅，实际使用位置式 pid 时一般常常使用 PD 进行控制。
• 增量式 PID
  Δ u ( n ) = K p ∗ [ e ( n ) − e ( n − 1 ) ] + K i ∗ e ( n ) + K d ∗ [ e ( n ) − 2 ∗ e ( n − 1 ) + e ( n − 2 ) ] / T \Delta u(n)=K_p*[e(n)-e(n-1)]+K_i* e(n)+K_d*[e(n)-2*e(n-1)+e(n-2)]/T Δu(n)=Kp​∗[e(n)−e(n−1)]+Ki​∗e(n)+Kd​∗[e(n)−2∗e(n−1)+e(n−2)]/T
  增量式不包含积分环节，控制增量只与前后三次测量值有关，对外界的抗扰性比位置式更好。
• 两者关系
  可以看到前者计算得到的是输入量，而后者算得的是输入增量，许多同学可能已经猜到了后者就是前者差分得到的
  想更加具体的了解两者关系，可以看这位博主的： 传送门.

三、电机调参

    常见的调参方式是比较快乐的，直接在生产商写好的驱动下进行参数的设置以及测试，找到合适的参数，更有的还有调参软件，遍历参数寻找合适的参数，从而省去人工调试的复杂环节。
    但这里想要分享的调参方法要多一点步骤，但是大体方向是不变的，这里以 Tmotor 的 AK10-9 与 大疆的 M2006 两款无刷直流电机为例子进行介绍。

1、Tmotor 调参

    Tmotor 的电机本来是有调参软件的，但是最开始的时候由于资料不完善，加上他的控制环不符合我们的应用要求，所以我们需要进行简单的修改。

1.1 控制框图

Tmotor 的运动控制框图如下，可以看到他的电流环采用 FOC 矢量控制，我们不能修改，另外两个环，速度环和位置环，只有比例环节，不能达到无误差的目标，所以我们需要在他的电流环上进行编写封装。

我们需要的控制环应该如下：

下面我们先编写控制程序。

1.2 PID 程序

之前探讨过离散 PID 有位置式 PID 算法和增量式 PID 算法，下面是根据其公式编写的 PID 程序，在使用之前需要稍稍修改一下参数。
位置式 PID

********************位置式 PID**************************
***        输入参数：电机电流速度位置等反馈值fed        ***
********************************************************
typedef struct PID
{
	float target;    //目标参考值
	float  deadband; //定义电机死区
	float err_now;   //定义当前误差
	float err_last;  //定义上一时刻误差
	float kp;        //比例环节系数
	float ki;        //积分环节系数
	float kd;        //微分环节系数，这里已将时间常数包含进去
	float Pout;      //比例环节输出
	float Iout;      //积分环节输出
	float Dout;      //微分环节输出
	float IntegLimt; //设置积分限幅
	float output;    //输出量
	float OutputLimt;//输出限幅
}PID_PARM;

//初始化PID参数的函数
void PID_parm_Init(PID_PARM *PID_parm,float target,float kp,float ki,float kd,float IntegLimt,float OutputLimt)
{
	PID_parm->target = target; 
	PID_parm->err_now = 0;
	PID_parm->err_last = 0;
	PID_parm->kp = kp; 
	PID_parm->ki = ki; 
	PID_parm->kd = kd; 
	PID_parm->Pout = 0;
	PID_parm->Iout = 0;
    PID_parm->Dout = 0;
	PID_parm->IntegLimt = IntegLimt;
	PID_parm->output = 0; 
	PID_parm->OutputLimt = OutputLimt;
}

//计算PID的函数
float PID_cal(PID_PARM *pid_parm,float feedback)
{
	pid_parm->err_now = pid_parm->target - feedback;
	pid_parm->Pout = pid_parm->kp*pid_parm->err_now;
	pid_parm->Iout += pid_parm->ki*pid_parm->err_now;
	pid_parm->Dout = pid_parm->kd*(pid_parm->err_now-pid_parm->err_last);
	//积分限幅
	if(pid_parm->Iout > pid_parm->IntegLimt)  pid_parm->Iout = pid_parm->IntegLimt;
	else if(pid_parm->Iout < -pid_parm->IntegLimt)  pid_parm->Iout = -pid_parm->IntegLimt;
	//输出限幅
	pid_parm->output = pid_parm->Pout + pid_parm->Iout + pid_parm->Dout;
	if(pid_parm->output > pid_parm->OutputLimt)  pid_parm->output = pid_parm->OutputLimt;
	else if(pid_parm->output < -pid_parm->OutputLimt)  pid_parm->output = -pid_parm->OutputLimt;
	//数据更新
	pid_parm->err_last = pid_parm->err_now;
	return  pid_parm->output;
}

float u;                  
PID_PARM pid_parm;
PID_parm_Init(&pid_parm,10,1,0.1,,0.5,50,100);  //这里的参数随机给的，具体参数需要调节
u = PID_cal(&pid_parm,fed);                     //计算出来的下一次输入

增量式 PID

********************增量式 PID**************************
***        输入参数：电机电流速度位置等反馈值fed        ***
********************************************************
typedef struct PID
{
	float target;      //目标参考值
	float  deadband; //定义电机死区
	float err_now;     //定义当前误差
	float err_last;    //定义上一时刻误差
	float err_llast;    //定义上上时刻误差
	float kp;          //比例环节系数
	float ki;          //积分环节系数
	float kd;          //微分环节系数，这里已将时间常数包含进去
	float Pout;        //比例环节输出
	float Iout;        //积分环节输出
	float Dout;        //微分环节输出
	float output;      //输出增量
	float output_last; //上一次输出的增量
	float OutputLimt;  //输出限幅
}PID_PARM;

//初始化PID参数的函数
void PID_parm_Init(PID_PARM *PID_parm,float target,float kp,float ki,float kd,float OutputLimt)
{
	PID_parm->target = target; 
	PID_parm->err_now = 0;
	PID_parm->err_last = 0;
	PID_parm->err_llast = 0;
	PID_parm->kp = kp; 
	PID_parm->ki = ki; 
	PID_parm->kd = kd; 
	PID_parm->Pout = 0;
	fPID_parm->Iout = 0;
	PID_parm->Dout = 0;
	PID_parm->output = 0; 
	PID_parm->output_last = 0; 
	PID_parm->OutputLimt = OutputLimt;
}

//计算PID的函数
float PID_cal(PID_PARM *pid_parm,float feedback)
{
	pid_parm->err_now = pid_parm->target - feedback;
	pid_parm->Pout = pid_parm->kp*(pid_parm->err_now - pid_parm->err_last);
	pid_parm->Iout = pid_parm->ki*pid_parm->err_now;
	pid_parm->Dout = pid_parm->kd*(pid_parm->err_now - 2*pid_parm->err_last + pid_parm->err_llast);
	//输出限幅
	pid_parm->output += pid_parm->Pout + pid_parm->Iout + pid_parm->Dout;
	if(pid_parm->output > pid_parm->OutputLimt)  pid_parm->output = pid_parm->OutputLimt;
	else if(pid_parm->output < -pid_parm->OutputLimt)  pid_parm->output = -pid_parm->OutputLimt;
	//数据更新
	pid_parm->err_llast = pid_parm->err_last;
	pid_parm->err_last = pid_parm->err_now;
	pid_parm->output_last = pid_parm->output;
	return  pid_parm->output;
}

float u;                  
PID_PARM pid_parm;
PID_parm_Init(&pid_parm,10,1,0.1,,0.5,100);  //这里的参数随机给的，具体参数需要调节
u = PID_cal(&pid_parm,fed);                  //计算出来的下一次输入

1.3 Tmotor 控制的完整程序

这里采用的 stm32 进行控制的，工程文件全部在下面：

• 头文件们

key.h

#ifndef __KEY_H
#define __KEY_H	 
#include "sys.h"  	 

/*下面的方式是通过直接操作库函数方式读取IO*/
#define KEY0 		GPIO_ReadInputDataBit(GPIOE,GPIO_Pin_4) //PE4
#define KEY1 		GPIO_ReadInputDataBit(GPIOE,GPIO_Pin_3)	//PE3 
#define KEY2 		GPIO_ReadInputDataBit(GPIOE,GPIO_Pin_2) //PE2
#define WK_UP 	GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_0)	//PA0

#define KEY0_PRES 	1
#define KEY1_PRES	2
#define KEY2_PRES	3
#define WKUP_PRES   4

void KEY_Init(void);	//IO初始化
u8 KEY_Scan(u8);  		//按键扫描函数	

#endif

can.h

#ifndef __CAN_H
#define __CAN_H	 
#include "sys.h"	    	 					    
	
void CAN1_Init(void);//CAN1初始化
 
u8 CAN1_Send_Msg(u8* msg);						//发送数据 
u8 CAN1_Receive_Msg(u8 *buf);

#endif

pid.h

#ifndef __PID_H
#define __PID_H	 
#include "sys.h"	
#include "stdlib.h"

typedef struct PID
{
	float target;      //目标参考值
	float deadband;    //定义电机死区
	float err_now;     //定义当前误差
	float err_last;    //定义上一时刻误差
	float err_llast;   //定义上上时刻误差
	float kp;          //比例环节系数
	float ki;          //积分环节系数
	float kd;          //微分环节系数，这里已将时间常数包含进去
	float Pout;        //比例环节输出
	float Iout;        //积分环节输出
	float Dout;        //微分环节输出
	float IntegLimt;   //设置积分限幅
	float output;      //输出量
	float output_last; //上一次输出的增量
	float OutputLimt;  //输出限幅
}PID_PARM;

void  PID_parm_Init(PID_PARM *PID_parm,float target,float deadband,float kp,float ki,float kd,float IntegLimt,float OutputLimt);
float Pos_PID_cal(PID_PARM *pid_parm,float feedback);
float Inc_PID_cal(PID_PARM *pid_parm,float feedback);
void PID_init(PID_PARM *Spd_PID,PID_PARM *Pos_PID);

#endif

motor.h

#ifndef __MOTOR_H
#define __MOTOR_H	 
#include "sys.h"	    	 
#include "pid.h"

#define pi 3.1415926
#define P_MAX   12.5
#define P_MIN  -12.5
#define V_MAX  46.57
#define V_MIN  -46.57
#define KP_MAX  500
#define KP_MIN  0
#define KD_MAX  5
#define KD_MIN  0
#define T_MAX  54
#define T_MIN  -54

extern u8 Tmotor_Mod_Buf[3][8];

typedef enum
{
	Tmotor_Open = 0xfc,
	Tmotor_Close = 0xfd,
	Tmotor_SetZero = 0xfe,
}Tmotor_Mod;

typedef struct{
	u8 id;                  	// id
	int16_t	 	speed_rps;    	// rad/s
	int16_t  	real_torque; 	  // 反馈力矩

	uint16_t 	angle;			    // 绝对角度
}Tmotor_measure_t;

extern Tmotor_measure_t  TmotorData;    // 保存Tmotor电机的状态

int float_to_uint(float x, float x_min, float x_max, int bits);
float uint_to_float(int x_int, float x_min, float x_max, int bits);
float limitf(float val, float min_val, float max_val);

void Tmotor_mod(u8 mod);
void get_Tmotor_measure(Tmotor_measure_t *ptr, CanRxMsg *Rxmsg);
u8 set_Tmotor_torque(float torque);

void Tmotor_Speed_Control(PID_PARM *PID_spd,float target);
void Tmotor_Position_Control(PID_PARM *PID_spd,PID_PARM *PID_pos,float target);

#endif

• 源文件们

key.c

#include "key.h"
#include "delay.h" 

void KEY_Init(void)
{
	 GPIO_InitTypeDef  GPIO_InitStructure;
	 RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA|RCC_AHB1Periph_GPIOE, ENABLE);//使能GPIOE时钟
	
	 GPIO_InitStructure.GPIO_Pin = GPIO_Pin_2|GPIO_Pin_3|GPIO_Pin_4; //KEY0 KEY1 KEY2对应引脚
	 GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN;//普通输入模式
	 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;//100M
	 GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;//上拉
	 GPIO_Init(GPIOE, &GPIO_InitStructure);//初始化GPIOE2，3，4
	
	 GPIO_InitStructure.GPIO_Pin = GPIO_Pin_0;//WK_UP对应引脚PA0
	 GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_DOWN ;//下拉
	 GPIO_Init(GPIOA, &GPIO_InitStructure);//初始化GPIOA0
} 
//按键处理函数
//返回按键值
//mode:0,不支持连续按;1,支持连续按;
//0，没有任何按键按下
//1，KEY0按下
//2，KEY1按下
//3，KEY2按下 
//4，WKUP按下 WK_UP
//注意此函数有响应优先级,KEY0>KEY1>KEY2>WK_UP!!
u8 KEY_Scan(u8 mode)
{	 
	static u8 key_up=1;//按键按松开标志
	if(mode)key_up=1;  //支持连按		  
	if(key_up&&(KEY0==0||KEY1==0||KEY2==0||WK_UP==1))
	{
		delay_ms(10);//去抖动 
		key_up=0;
		if(KEY0==0)return 1;
		else if(KEY1==0)return 2;
		else if(KEY2==0)return 3;
		else if(WK_UP==1)return 4;
	}else if(KEY0==1&&KEY1==1&&KEY2==1&&WK_UP==0)key_up=1; 	    
 	return 0;// 无按键按下
}

can.c

#include "can.h"
#include "motor.h"

Tmotor_measure_t  TmotorData;

void CAN1_Init()
{
	u8 tsjw = CAN_SJW_1tq;
	u8 tbs2 = CAN_BS2_4tq;
	u8 tbs1 = CAN_BS1_2tq;
	u16 brp = 6;
	u8 mode = CAN_Mode_Normal;     // 提前配置好波特率  这里是1000k
	
  	GPIO_InitTypeDef GPIO_InitStructure; 
	CAN_InitTypeDef        CAN_InitStructure;
  	CAN_FilterInitTypeDef  CAN_FilterInitStructure;

   	NVIC_InitTypeDef  NVIC_InitStructure;

    //使能相关时钟
	RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOA, ENABLE);//使能PORTA时钟	                   											 

  	RCC_APB1PeriphClockCmd(RCC_APB1Periph_CAN1, ENABLE);//使能CAN1时钟	
	
    //初始化GPIO
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_11| GPIO_Pin_12;
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;//复用功能
    GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;//推挽输出
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;//100MHz
    GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;//上拉
    GPIO_Init(GPIOA, &GPIO_InitStructure);//初始化PA11,PA12
	
	  //引脚复用映射配置
	GPIO_PinAFConfig(GPIOA,GPIO_PinSource11,GPIO_AF_CAN1); //GPIOA11复用为CAN1
	GPIO_PinAFConfig(GPIOA,GPIO_PinSource12,GPIO_AF_CAN1); //GPIOA12复用为CAN1
	  
  	//CAN单元设置
   	CAN_InitStructure.CAN_TTCM=DISABLE;	//非时间触发通信模式   
  	CAN_InitStructure.CAN_ABOM=ENABLE;	//软件自动离线管理	  
  	CAN_InitStructure.CAN_AWUM=DISABLE;//睡眠模式通过软件唤醒(清除CAN->MCR的SLEEP位)
  	CAN_InitStructure.CAN_NART=ENABLE;	//禁止报文自动传送 
  	CAN_InitStructure.CAN_RFLM=DISABLE;	//报文不锁定,新的覆盖旧的  
  	CAN_InitStructure.CAN_TXFP=DISABLE;	//优先级由报文标识符决定 
  	CAN_InitStructure.CAN_Mode= mode;	 //模式设置 
  	CAN_InitStructure.CAN_SJW=tsjw;	//重新同步跳跃宽度(Tsjw)为tsjw+1个时间单位 CAN_SJW_1tq~CAN_SJW_4tq
  	CAN_InitStructure.CAN_BS1=tbs1; //Tbs1范围CAN_BS1_1tq ~CAN_BS1_16tq
  	CAN_InitStructure.CAN_BS2=tbs2;//Tbs2范围CAN_BS2_1tq ~	CAN_BS2_8tq
  	CAN_InitStructure.CAN_Prescaler=brp;  //分频系数(Fdiv)为brp+1	
  	CAN_Init(CAN1, &CAN_InitStructure);   // 初始化CAN1 
    
		//配置过滤器
 	CAN_FilterInitStructure.CAN_FilterNumber=0;	  //过滤器0
  	CAN_FilterInitStructure.CAN_FilterMode=CAN_FilterMode_IdMask; 
  	CAN_FilterInitStructure.CAN_FilterScale=CAN_FilterScale_32bit; //32位 
  	CAN_FilterInitStructure.CAN_FilterIdHigh=0x0000;32位ID
  	CAN_FilterInitStructure.CAN_FilterIdLow=0x0000;
  	CAN_FilterInitStructure.CAN_FilterMaskIdHigh=0x0000;//32位MASK
  	CAN_FilterInitStructure.CAN_FilterMaskIdLow=0x0000;
   	CAN_FilterInitStructure.CAN_FilterFIFOAssignment=CAN_Filter_FIFO0;//过滤器0关联到FIFO0
  	CAN_FilterInitStructure.CAN_FilterActivation=ENABLE; //激活过滤器0
  	CAN_FilterInit(&CAN_FilterInitStructure);//滤波器初始化
		
	  CAN_ITConfig(CAN1,CAN_IT_FMP0,ENABLE);//FIFO0消息挂号中断允许.		    
  
  	NVIC_InitStructure.NVIC_IRQChannel = CAN1_RX0_IRQn;
  	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 0;     // 主优先级为1
  	NVIC_InitStructure.NVIC_IRQChannelSubPriority = 2;            // 次优先级为2
  	NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
  	NVIC_Init(&NVIC_InitStructure);

}   

void CAN1_RX0_IRQHandler(void)
{
  	CanRxMsg RxMessage;
    CAN_Receive(CAN1, 0, &RxMessage);
	if(RxMessage.StdId == 0x00)
			get_Tmotor_measure(&TmotorData, &RxMessage);	
}

u8 CAN1_Send_Msg(u8* msg)
{	
		u8 mbox;
		u16 i=0;
		CanTxMsg TxMessage;
		TxMessage.StdId=0x01;	 
		TxMessage.ExtId=0x00;	 
		TxMessage.IDE=0;		  
		TxMessage.RTR=0;		  
		TxMessage.DLC=8;		
		for(i=0;i<8;i++)
			TxMessage.Data[i]=msg[i];				 	
		mbox= CAN_Transmit(CAN1, &TxMessage);  
		i=0;
		while((CAN_TransmitStatus(CAN1, mbox)==CAN_TxStatus_Failed)&&(i<0XFFF))i++;
		if(i>=0XFFF)return 1;
		return 0;		
}

u8 CAN1_Receive_Msg(u8 *buf)
{		   		   
		u8 i;
		CanRxMsg RxMessage;
		if( CAN_MessagePending(CAN1,CAN_FIFO0)==0)return 0;		//没有接收到数据,直接退出 
		CAN_Receive(CAN1, CAN_FIFO0, &RxMessage);//读取数据	
		for(i=0;i<RxMessage.DLC;i++)
			buf[i]=RxMessage.Data[i]; 
		return RxMessage.DLC;	
}

pid.c

#include "pid.h"

//初始化PID参数的函数
void PID_parm_Init(PID_PARM *PID_parm,float target,float deadband,float kp,float ki,float kd,float IntegLimt,float OutputLimt)
{
	PID_parm->target = target; 
	PID_parm->deadband = deadband;
	PID_parm->err_now = 0;
	PID_parm->err_last = 0;
	PID_parm->err_last = 0;
	PID_parm->kp = kp; 
	PID_parm->ki = ki; 
	PID_parm->kd = kd; 
	PID_parm->Pout = 0;
	PID_parm->Iout = 0;
	PID_parm->Dout = 0;
	PID_parm->IntegLimt = IntegLimt;
	PID_parm->output = 0; 
	PID_parm->output_last = 0;
	PID_parm->OutputLimt = OutputLimt;
}

//计算位置PID的函数
float Pos_PID_cal(PID_PARM *pid_parm,float feedback)
{
	pid_parm->err_now = pid_parm->target - feedback;
	pid_parm->Pout = pid_parm->kp*pid_parm->err_now;
	pid_parm->Iout += pid_parm->ki*pid_parm->err_now;
	pid_parm->Dout = pid_parm->kd*(pid_parm->err_now-pid_parm->err_last);
	//积分限幅
	if(pid_parm->Iout > pid_parm->IntegLimt)  pid_parm->Iout = pid_parm->IntegLimt;
	else if(pid_parm->Iout < -pid_parm->IntegLimt)  pid_parm->Iout = -pid_parm->IntegLimt;
	//输出限幅
	pid_parm->output = pid_parm->Pout + pid_parm->Iout + pid_parm->Dout;
	if(pid_parm->output > pid_parm->OutputLimt)  pid_parm->output = pid_parm->OutputLimt;
	else if(pid_parm->output < -pid_parm->OutputLimt)  pid_parm->output = -pid_parm->OutputLimt;
	//数据更新
	pid_parm->err_last = pid_parm->err_now;
	return  pid_parm->output;
}

//计算增量PID的函数
float Inc_PID_cal(PID_PARM *pid_parm,float feedback)
{
	pid_parm->err_now = pid_parm->target - feedback;
	pid_parm->Pout = pid_parm->kp*(pid_parm->err_now - pid_parm->err_last);
	pid_parm->Iout = pid_parm->ki*pid_parm->err_now;
	pid_parm->Dout = pid_parm->kd*(pid_parm->err_now - 2*pid_parm->err_last + pid_parm->err_llast);
	//输出限幅
	pid_parm->output += pid_parm->Pout + pid_parm->Iout + pid_parm->Dout;
	if(pid_parm->output > pid_parm->OutputLimt)  pid_parm->output = pid_parm->OutputLimt;
	else if(pid_parm->output < -pid_parm->OutputLimt)  pid_parm->output = -pid_parm->OutputLimt;
	//数据更新
	pid_parm->err_llast = pid_parm->err_last;
	pid_parm->err_last = pid_parm->err_now;
	pid_parm->output_last = pid_parm->output;
	return  pid_parm->output;
}

//初始化PID参数
void PID_init(PID_PARM *Spd_PID,PID_PARM *Pos_PID)
{
		PID_parm_Init(Pos_PID,0,0.1,0.6,0,2,180,200);      
		PID_parm_Init(Spd_PID,0,0.01,0.2,0.001,0,0.5,54);  
}

motor.c

#include "delay.h"
#include "motor.h"
#include "usart.h"
#include "can.h"
#include "pid.h"

u8 Tmotor_Mod_Buf[3][8] =
{  //电机模式指令
	{0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xfc},
	{0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xfd},
	{0xff,0xff,0xff,0xff,0xff,0xff,0xff,0xfe}
};

//数值转换函数/
/*                                            /
float_to_uint: 浮点转整型                     /
uint_to_float: 整形变浮点                     /
limitf:        限幅函数                       /
*/                                            /
///
int float_to_uint(float x, float x_min, float x_max, int bits)
{
    float span = x_max - x_min;
    float offset = x_min;
    return (int) ( (x - offset) * ( (float) ((1<<bits) - 1)) / span);
}


float uint_to_float(int x_int, float x_min, float x_max, int bits)
{
    float span = x_max - x_min;
    float offset = x_min;
    return ((float) x_int) * span / ((float) ((1<<bits) - 1)) + offset;
}


float limitf(float val, float min_val, float max_val)
{
		float val_real = 0.0f;
		if (val < min_val){
				val_real = min_val;
				return val_real;
		}
		else if (val > max_val){
				val_real = max_val;
			  return val_real;
		}
		else{
				val_real = val;
			  return val_real;
		}
}

/Tmotor电机指令
/*                                                        /
Tmotor_mod:           设置电机模式（开、关、设置零点）    /
get_Tmotor_measure:   获取电机的反馈数据(16进制的)        /
set_Tmotor_torque:    设置电机力矩                        /
*/                                                        /
///
void Tmotor_mod(u8 mod)
{
	switch(mod)
	{
		case Tmotor_Open:
			CAN1_Send_Msg(Tmotor_Mod_Buf[0]);
		break;
		
		case Tmotor_Close:
			set_Tmotor_torque(0.0f);
			delay_ms(10);
			CAN1_Send_Msg(Tmotor_Mod_Buf[1]);
		break;
		
		case Tmotor_SetZero:
			CAN1_Send_Msg(Tmotor_Mod_Buf[2]);
		break;
		default:
			break;
	}
}


void get_Tmotor_measure(Tmotor_measure_t *ptr, CanRxMsg *Rxmsg)
{ 
		ptr->id = Rxmsg->Data[0];                                              //电机id
		ptr->angle = (uint16_t)(Rxmsg->Data[1]<<8 | Rxmsg->Data[2]);           //电机位置
		ptr->speed_rps  = (uint16_t)(Rxmsg->Data[3]<<4 | Rxmsg->Data[4]>>4);   //电机速度
		ptr->real_torque = ((Rxmsg->Data[4]&0x0f)<<8 | Rxmsg->Data[5]);        //电机力矩
}


u8 set_Tmotor_torque(float torque)
{	
		int p_int,v_int,kp_int,kd_int,t_int;
		u8 mbox;
		u16 i=0;
		CanTxMsg TxMessage;	
		// 由于采用力矩模式，不使用位置和速度模式
		kp_int = float_to_uint(0,KP_MIN,KP_MAX,12);
		kd_int = float_to_uint(0,KD_MIN,KD_MAX,12);
		p_int = float_to_uint(0,P_MIN,P_MAX,16);
		v_int = float_to_uint(0,V_MIN,V_MAX,12);
		// 真正需要设置的是力矩
		t_int = float_to_uint(torque,T_MIN,T_MAX,12);
		
		TxMessage.StdId=0x01;	 // 标准标识符为1
		TxMessage.ExtId=0x12;	    // 设置扩展标示符（29位）这里不需要设置
		TxMessage.IDE=0;		  // 使用扩展标识符
		TxMessage.RTR=0;		  // 消息类型为数据帧，一帧8位
		TxMessage.DLC=0x08;							 // 发送两帧信息

		TxMessage.Data[0] = p_int>>8;       //位置高8位
		TxMessage.Data[1] = p_int&0xFF;     //位置低8位
		TxMessage.Data[2] = v_int>>4;       //速度高8位
		TxMessage.Data[3] = ((v_int&0xF)<<4)|(kp_int>>8);  //速度低4位  KP高4位
		TxMessage.Data[4] = kp_int&0xFF;    //KP低8位
		TxMessage.Data[5] = kd_int>>4;		//KD高8位
		TxMessage.Data[6] = ((kd_int&0xF)<<4)|(t_int>>8);  //KP低4位  扭矩高4位
		TxMessage.Data[7] = t_int&0xFF;		//扭矩低8位
		mbox= CAN_Transmit(CAN1, &TxMessage);   
		i=0;
		while((CAN_TransmitStatus(CAN1, mbox)==CAN_TxStatus_Failed)&&(i<0XFFF))i++;	//等待发送结束
		if(i>=0XFFF)return 1;
		return 0;		
}

/Tmotor电机控制模式
/*                                                        /
Tmotor_Speed_Control:   	速度控制                        /
Tmotor_Postion_Control:   位置控制                        /
*/                                                        /
///

void Tmotor_Speed_Control(PID_PARM *PID_spd,float target)
{
		// 需要先初始化pid参数
		// 速度环采用单环即可
		//  spd单位rpm  所以将反馈的数据 *60/(2*pi)
		float set_torque;
		PID_spd->target = target;
		set_torque = Inc_PID_cal(PID_spd,uint_to_float(TmotorData.speed_rps,V_MIN,V_MAX,12) * 60/(2*pi));
		printf("v:%f\n",uint_to_float(TmotorData.speed_rps,V_MIN,V_MAX,12)* 60/(2*pi));
		set_Tmotor_torque(set_torque);
		
}

void Tmotor_Position_Control(PID_PARM *PID_spd,PID_PARM *PID_pos,float target)
{
		//位置控制模式需要设置双环
		//采用速度内环和位置外环
		//电机反馈的角度单位是rad，需要转换为°,所以*180.f/pi
		float set_torque;
		float set_spd;
		PID_pos->target = target;
		set_spd = Pos_PID_cal(PID_pos,uint_to_float(TmotorData.angle,P_MIN,P_MAX,16) * 180.f/pi);
		printf("p:%f\n",uint_to_float(TmotorData.angle,P_MIN,P_MAX,16) * 180.f/pi);
		set_torque = Inc_PID_cal(PID_spd,-set_spd);
		set_Tmotor_torque(set_torque);
}

main.c

#include "sys.h"
#include "delay.h"
#include "usart.h"
#include "led.h"
#include "can.h"
#include "key.h"
#include "pid.h"
#include "motor.h"

int main(void)
{ 	
		u8 key;
		u8 key_flag = 0;
		u8 open_flag = 0;
		float spd_target = 60;
		float pos_target = 0;
		PID_PARM PID_pos;
		PID_PARM PID_spd;
		//初始化相关基础模块
		NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//设置系统中断优先级分组2
		CAN1_Init();  // 设置CAN波特率为1000k
		delay_init(168);  //初始化延时函数
		LED_Init();				//初始化LED端口
		KEY_Init();
		uart_init(115200);
		
		//初始化pid参数
		PID_init(&PID_spd,&PID_pos);
		//Tmotor_mod(Tmotor_Open);
		LED0 = 0;
		//按键控制电机模式
		while(1)
		{
			key = KEY_Scan(0);	
			//printf("%d",key);
			if(key)		
			{				
					switch(key)
					{				 
						case WKUP_PRES:	
							Tmotor_mod(Tmotor_Close);
							open_flag = 0;
							break;
						case KEY0_PRES:	
							key_flag = 1;
							pos_target += 10;
							spd_target += 10;
							break;
						case KEY1_PRES:	 
							Tmotor_mod(Tmotor_Open);
							open_flag = 1;
							break;
						case KEY2_PRES:	
							key_flag = 0;
							break;
					}
			}else delay_ms(10);
			//当需要执行时			
			if(open_flag & key_flag)
			{
					Tmotor_Speed_Control(&PID_spd,spd_target);
					//Tmotor_Position_Control(&PID_spd,&PID_pos,pos_target);
					LED0 = ~LED0;
					delay_ms(10);
			}
			//delay_ms(100);
		}
		return 0;
}   

1.4 PID 调试技巧

    PID 的调试是比较复杂的一个活，所以一般来说都会有调参软件的，当当当当~
    这里当然没有了。。。
    所以我们就只有采用手动试！
    还有一点，我们现在考虑的是位置速度双闭环的系统，针对这个系统我们来进行调试。
    在试之前，我们还需要了解的是，位置式 PID 是输出的位置量（不一定就是位置），常用在位置环（双环时），增量式 PID 是输出的增量，存在稳态误差，常用在内环。同时考虑到位置是速度积分量，所以需要将速度环放在内部，位置环放在外部，先调内环，再调外环，调试的时候，可以先固定微分和积分环节为零，不断的从小到大增大比例环节，在增大到电机抖动就说明不行了，得降低，下降到稳态误差比较小且电机不抖得情况下，尝试增加积分，一点点加，加到电机转动不抖动且稳态误差几乎消除时即可，一般情况下此时稳态误差还会存在，且会伴随着电机在目标值附近来回徘徊，或者出现电机出现超调，即输出值先一下子大于目标值后然后降低到目标值附近（徘徊），这时就需要添加微分环节了，可以消除这个超调哦。调试好内环后，按照同样得方式调试外环参数，最终获得一个比较满意得参数即可。

2、M2006（M3508）调参

    做过RoboMaster比赛的同学应该接触的比较多，M2006与M3508是大疆出产的两款直流无刷减速电机，由于体积重量和所能提供的力矩大小各有特色各有不同的作用，之前因为项目原因简单的玩了一下。两款电机配备有各自的电机调速器（控制器），我们需要在其基础上对其进行控制。

2.1 M2006（M3508）控制框图

    该电机只有底层的电流环，速度环和位置环均不存在，但其实和 Tmotor 电机的控制类似，我们同样可以在其上编写位置环和速度环控制。

    添加了速度环和位置环的控制框图如下，和上面的其实并没有区别。

2.2 M2006与M3508 完整的控制程序

    具体的控制程序并没有太大区别，不同的只有给定电流的指令。
    具体文件参考 Tmotor 文件进行修改。

2.3 调试技巧

    大疆的电机和 Tmotor 控制指令的执行是有区别的，Tmotor 的电机给了电流指令后，它会保存这个指令一直执行，所以只需要给定一次就好了；而 DJI 的它是以矩形波的形式让给定电流维持一小段时间，所以需要不断的给定电流指令。具体的调试技巧和上面大同小异，除了参数的问题，还需要注意指令发送的频率，一般来说100HZ左右都是可以达到比较满意的工况的。

四、存在的问题

    在上面的电机控制中，运行程序的朋友们会发现电机依然存在问题，其中最明显的就是启动时的抖动问题，这个问题也是可以解决的，比如变 PID 控制或者对启动电流进行限制，实现软启动，也可以对电流上升的速度或加速度进行一个限制，如果有机会的话将会在后面将解决后的方案与大家分享。