电机的种类特别多,小型机器人(无人机、无人车)上普遍使用直流无刷电机,航模使用的一般是高速低扭矩,车模一般通过减速器低速实现高扭矩。
一般电机与电机驱动器配套使用,用的话只需要知道如何供电和如何给控制信号就行了。本文使用大疆的M3508电机,使用CAN总线进行控制,长相如图所示。
CAN总线是汽车行业的应用标准,稍微大型的电机很多是CAN总线控制的。CAN总线是一种现场总线,具有速度快、传输距离远、连接节点多的优点,具有错误检测、错误通知和错误恢复功能,使得其特别适合工业过程监控设备的互联。CAN总线的知识点很复杂,现在还有升级版CAN-Open
回到我们的电机,众所周知伺服电机有位置伺服、速度伺服、力伺服,大家一定也听过电机的三闭环控制,也就是位置环——速度环——电流环,如图所示,根据控制目标的不同,从外环到内环分别对应电机的位置伺服、速度伺服与力伺服,也可以说三种模式:位置模式、速度模式、转矩模式。
电机一般要配合驱动控制器使用,有的驱动器很牛皮,里面做好了三种模式,控制器给位置信号、速度信号就行了。有的次一点,只做了速度伺服,有的更次,啥也没做。我们今天用的大疆M33508电机就啥也没做,,,驱动器只是上报电机的速度、电流、转角、温度等信息,需要控制器去读这些信息,然后做闭环计算,控制器将计算结果传输给驱动器,再去控制电机电流。这里我们主要是做速度伺服。上图变成这样。
使用CAN总线实现大疆M3508电机的速度闭环控制
这里我们需要对电机做一个速度闭环控制,由于大疆电机提供了例程,所以我直接移植了它的代码,当然有些地方做了修改。
创建一个motor_pid.h和motor_pid.c文件(其实完全可以用上一章写的pid.c和pid.h文件,这里因为是官方提供的例程,所以直接用了)。
motor_pid.h文件:
#ifndef __MOTOR_PID_H
#define __MOTOR_PID_H
/* Includes ------------------------------------------------------------------*/
#include "stm32f7xx_hal.h"
enum
{
LLAST = 0,
LAST = 1,
NOW = 2,
POSITION_PID,
DELTA_PID,
};
typedef struct __pid_t
{
float p;
float i;
float d;
float set[3]; //目标值,包含NOW, LAST, LLAST上上次
float get[3]; //测量值
float err[3]; //误差
float pout; //p输出
float iout; //i输出
float dout; //d输出
float pos_out; //本次位置式输出
float last_pos_out; //上次输出
float delta_u; //本次增量值
float delta_out; //本次增量式输出 = last_delta_out + delta_u
float last_delta_out;
float max_err;
float deadband; //err < deadband return
uint32_t pid_mode;
uint32_t MaxOutput; //输出限幅
uint32_t IntegralLimit; //积分限幅
void (*f_param_init)(struct __pid_t *pid, //PID参数初始化
uint32_t pid_mode,
uint32_t maxOutput,
uint32_t integralLimit,
float p,
float i,
float d);
void (*f_pid_reset)(struct __pid_t *pid, float p, float i, float d); //pid三个参数修改
} pid_t;
void PID_struct_init(
pid_t *pid,
uint32_t mode,
uint32_t maxout,
uint32_t intergral_limit,
float kp,
float ki,
float kd);
float pid_calc(pid_t *pid, float fdb, float ref);
#endif
motor_pid.c文件:
/* Includes ------------------------------------------------------------------*/
#include "motor_pid.h"
#include "mytype.h"
#include <math.h>
//#include "cmsis_os.h"
#define ABS(x) ((x>0)? (x): (-x))
void abs_limit(float *a, float ABS_MAX){
if(*a > ABS_MAX)
*a = ABS_MAX;
if(*a < -ABS_MAX)
*a = -ABS_MAX;
}
/*参数初始化--------------------------------------------------------------*/
static void pid_param_init(
pid_t *pid,
uint32_t mode,
uint32_t maxout,
uint32_t intergral_limit,
float kp,
float ki,
float kd)
{
pid->IntegralLimit = intergral_limit;
pid->MaxOutput = maxout;
pid->pid_mode = mode;
pid->p = kp;
pid->i = ki;
pid->d = kd;
}
/*中途更改参数设定(调试)------------------------------------------------------------*/
static void pid_reset(pid_t *pid, float kp, float ki, float kd)
{
pid->p = kp;
pid->i = ki;
pid->d = kd;
}
/**
*@bref. calculate delta PID and position PID
*@param[in] set: target
*@param[in] real measure
*/
float pid_calc(pid_t* pid, float get, float set){
pid->get[NOW] = get;
pid->set[NOW] = set;
pid->err[NOW] = set - get; //set - measure
if (pid->max_err != 0 && ABS(pid->err[NOW]) > pid->max_err )
return 0;
if (pid->deadband != 0 && ABS(pid->err[NOW]) < pid->deadband)
return 0;
if(pid->pid_mode == POSITION_PID) //位置式p
{
pid->pout = pid->p * pid->err[NOW];
pid->iout += pid->i * pid->err[NOW];
pid->dout = pid->d * (pid->err[NOW] - pid->err[LAST] );
abs_limit(&(pid->iout), pid->IntegralLimit);
pid->pos_out = pid->pout + pid->iout + pid->dout;
abs_limit(&(pid->pos_out), pid->MaxOutput);
pid->last_pos_out = pid->pos_out; //update last time
}
else if(pid->pid_mode == DELTA_PID)//增量式P
{
pid->pout = pid->p * (pid->err[NOW] - pid->err[LAST]);
pid->iout = pid->i * pid->err[NOW];
pid->dout = pid->d * (pid->err[NOW] - 2*pid->err[LAST] + pid->err[LLAST]);
abs_limit(&(pid->iout), pid->IntegralLimit);
pid->delta_u = pid->pout + pid->iout + pid->dout;
pid->delta_out = pid->last_delta_out + pid->delta_u;
abs_limit(&(pid->delta_out), pid->MaxOutput);
pid->last_delta_out = pid->delta_out; //update last time
}
pid->err[LLAST] = pid->err[LAST];
pid->err[LAST] = pid->err[NOW];
pid->get[LLAST] = pid->get[LAST];
pid->get[LAST] = pid->get[NOW];
pid->set[LLAST] = pid->set[LAST];
pid->set[LAST] = pid->set[NOW];
return pid->pid_mode==POSITION_PID ? pid->pos_out : pid->delta_out;
//
}
/**
*@bref. special calculate position PID @attention @use @gyro data!!
*@param[in] set: target
*@param[in] real measure
*/
float pid_sp_calc(pid_t* pid, float get, float set, float gyro){
pid->get[NOW] = get;
pid->set[NOW] = set;
pid->err[NOW] = set - get; //set - measure
if(pid->pid_mode == POSITION_PID) //位置式p
{
pid->pout = pid->p * pid->err[NOW];
if(fabs(pid->i) >= 0.001f)
pid->iout += pid->i * pid->err[NOW];
else
pid->iout = 0;
pid->dout = -pid->d * gyro/100.0f;
abs_limit(&(pid->iout), pid->IntegralLimit);
pid->pos_out = pid->pout + pid->iout + pid->dout;
abs_limit(&(pid->pos_out), pid->MaxOutput);
pid->last_pos_out = pid->pos_out; //update last time
}
else if(pid->pid_mode == DELTA_PID)//增量式P
{
pid->pout = pid->p * (pid->err[NOW] - pid->err[LAST]);
pid->iout = pid->i * pid->err[NOW];
pid->dout = pid->d * (pid->err[NOW] - 2*pid->err[LAST] + pid->err[LLAST]);
abs_limit(&(pid->iout), pid->IntegralLimit);
pid->delta_u = pid->pout + pid->iout + pid->dout;
pid->delta_out = pid->last_delta_out + pid->delta_u;
abs_limit(&(pid->delta_out), pid->MaxOutput);
pid->last_delta_out = pid->delta_out; //update last time
}
pid->err[LLAST] = pid->err[LAST];
pid->err[LAST] = pid->err[NOW];
pid->get[LLAST] = pid->get[LAST];
pid->get[LAST] = pid->get[NOW];
pid->set[LLAST] = pid->set[LAST];
pid->set[LAST] = pid->set[NOW];
return pid->pid_mode==POSITION_PID ? pid->pos_out : pid->delta_out;
//
}
/*pid总体初始化-----------------------------------------------------------------*/
void PID_struct_init(
pid_t* pid,
uint32_t mode,
uint32_t maxout,
uint32_t intergral_limit,
float kp,
float ki,
float kd)
{
/*init function pointer*/
pid->f_param_init = pid_param_init;
pid->f_pid_reset = pid_reset;
// pid->f_cal_pid = pid_calc;
// pid->f_cal_sp_pid = pid_sp_calc; //addition
/*init pid param */
pid->f_param_init(pid, mode, maxout, intergral_limit, kp, ki, kd);
}
再创建一个motor.h和motor.c文件,用于写电机PID控制的应用代码。
motor.h内容为:
#ifndef __MOTOR
#define __MOTOR
#include "stm32f7xx_hal.h"
#include "mytype.h"
#include "can.h"
/*电机的参数结构体*/
typedef struct{
int16_t speed_rpm;
int16_t real_current;
int16_t given_current;
uint8_t hall;
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;
u8 buf_idx;
u16 angle_buf[FILTER_BUF_LEN];
u16 fited_angle;
u32 msg_cnt;
}moto_measure_t;
#define motor_num 6 //电机数量
/* Extern ------------------------------------------------------------------*/
extern moto_measure_t moto_chassis[];
u8 get_moto_measure(moto_measure_t *ptr, CAN_HandleTypeDef* hcan);
u8 set_moto_current(CAN_HandleTypeDef* hcan, s16 SID, s16 iq1, s16 iq2, s16 iq3, s16 iq4);
#endif
motor.c文件:
#include "can.h"
#include "motor.h"
//#include "cmsis_os.h"
moto_measure_t moto_chassis[motor_num] = {0}; //4 chassis moto
moto_measure_t moto_info;
void get_total_angle(moto_measure_t *p);
void get_moto_offset(moto_measure_t *ptr, CAN_HandleTypeDef* hcan);
/*******************************************************************************************
* @Func void get_moto_measure(moto_measure_t *ptr, CAN_HandleTypeDef* hcan)
* @Brief 接收通过CAN发过来的信息
* @Param
* @Retval None
* @Date 2015/11/24
*******************************************************************************************/
u8 get_moto_measure(moto_measure_t *ptr, CAN_HandleTypeDef* hcan)
{
if(HAL_CAN_Receive(hcan,CAN_FIFO0,0)!=HAL_OK) return 0;//接收数据
ptr->last_angle = ptr->angle;
ptr->angle = (uint16_t)(hcan->pRxMsg->Data[0]<<8 | hcan->pRxMsg->Data[1]) ;
ptr->real_current = (int16_t)(hcan->pRxMsg->Data[2]<<8 | hcan->pRxMsg->Data[3]);
ptr->speed_rpm = ptr->real_current; //这里是因为两种电调对应位不一样的信息
ptr->given_current = (int16_t)(hcan->pRxMsg->Data[4]<<8 | hcan->pRxMsg->Data[5])/-5;
ptr->hall = hcan->pRxMsg->Data[6];
if(ptr->angle - ptr->last_angle > 4096)
ptr->round_cnt --;
else if (ptr->angle - ptr->last_angle < -4096)
ptr->round_cnt ++;
ptr->total_angle = ptr->round_cnt * 8192 + ptr->angle - ptr->offset_angle;
return hcan->pRxMsg->DLC;
}
/*this function should be called after system+can init */
void get_moto_offset(moto_measure_t *ptr, CAN_HandleTypeDef* hcan)
{
ptr->angle = (uint16_t)(hcan->pRxMsg->Data[0]<<8 | hcan->pRxMsg->Data[1]) ;
ptr->offset_angle = ptr->angle;
}
#define ABS(x) ( (x>0) ? (x) : (-x) )
/**
*@bref 电机上电角度=0, 之后用这个函数更新3510电机的相对开机后(为0)的相对角度。
*/
void get_total_angle(moto_measure_t *p){
int res1, res2, delta;
if(p->angle < p->last_angle){ //可能的情况
res1 = p->angle + 8192 - p->last_angle; //正转,delta=+
res2 = p->angle - p->last_angle; //反转 delta=-
}else{ //angle > last
res1 = p->angle - 8192 - p->last_angle ;//反转 delta -
res2 = p->angle - p->last_angle; //正转 delta +
}
//不管正反转,肯定是转的角度小的那个是真的
if(ABS(res1)<ABS(res2))
delta = res1;
else
delta = res2;
p->total_angle += delta;
p->last_angle = p->angle;
}
u8 set_moto_current(CAN_HandleTypeDef* hcan, s16 SID, s16 iq1, s16 iq2, s16 iq3, s16 iq4)
{
hcan->pTxMsg->StdId = SID;
hcan->pTxMsg->IDE = CAN_ID_STD;
hcan->pTxMsg->RTR = CAN_RTR_DATA;
hcan->pTxMsg->DLC = 0x08;
hcan->pTxMsg->Data[0] = iq1 >> 8;
hcan->pTxMsg->Data[1] = iq1;
hcan->pTxMsg->Data[2] = iq2 >> 8;
hcan->pTxMsg->Data[3] = iq2;
hcan->pTxMsg->Data[4] = iq3 >> 8;
hcan->pTxMsg->Data[5] = iq3;
hcan->pTxMsg->Data[6] = iq4 >> 8;
hcan->pTxMsg->Data[7] = iq4;
//HAL_CAN_Transmit(hcan, 1000);
if(HAL_CAN_Transmit(&CAN1_Handler,1000)!=HAL_OK) return 1; //发送
return 0;
}
motor.c文件:
#include "can.h"
#include "motor.h"
//#include "cmsis_os.h"
moto_measure_t moto_chassis[motor_num] = {0}; //4 chassis moto
moto_measure_t moto_info;
void get_total_angle(moto_measure_t *p);
void get_moto_offset(moto_measure_t *ptr, CAN_HandleTypeDef* hcan);
/*******************************************************************************************
* @Func void get_moto_measure(moto_measure_t *ptr, CAN_HandleTypeDef* hcan)
* @Brief 接收通过CAN发过来的信息
* @Param
* @Retval None
* @Date 2015/11/24
*******************************************************************************************/
u8 get_moto_measure(moto_measure_t *ptr, CAN_HandleTypeDef* hcan)
{
if(HAL_CAN_Receive(hcan,CAN_FIFO0,0)!=HAL_OK) return 0;//接收数据
ptr->last_angle = ptr->angle;
ptr->angle = (uint16_t)(hcan->pRxMsg->Data[0]<<8 | hcan->pRxMsg->Data[1]) ;
ptr->real_current = (int16_t)(hcan->pRxMsg->Data[2]<<8 | hcan->pRxMsg->Data[3]);
ptr->speed_rpm = ptr->real_current; //这里是因为两种电调对应位不一样的信息
ptr->given_current = (int16_t)(hcan->pRxMsg->Data[4]<<8 | hcan->pRxMsg->Data[5])/-5;
ptr->hall = hcan->pRxMsg->Data[6];
if(ptr->angle - ptr->last_angle > 4096)
ptr->round_cnt --;
else if (ptr->angle - ptr->last_angle < -4096)
ptr->round_cnt ++;
ptr->total_angle = ptr->round_cnt * 8192 + ptr->angle - ptr->offset_angle;
return hcan->pRxMsg->DLC;
}
/*this function should be called after system+can init */
void get_moto_offset(moto_measure_t *ptr, CAN_HandleTypeDef* hcan)
{
ptr->angle = (uint16_t)(hcan->pRxMsg->Data[0]<<8 | hcan->pRxMsg->Data[1]) ;
ptr->offset_angle = ptr->angle;
}
#define ABS(x) ( (x>0) ? (x) : (-x) )
/**
*@bref 电机上电角度=0, 之后用这个函数更新3510电机的相对开机后(为0)的相对角度。
*/
void get_total_angle(moto_measure_t *p){
int res1, res2, delta;
if(p->angle < p->last_angle){ //可能的情况
res1 = p->angle + 8192 - p->last_angle; //正转,delta=+
res2 = p->angle - p->last_angle; //反转 delta=-
}else{ //angle > last
res1 = p->angle - 8192 - p->last_angle ;//反转 delta -
res2 = p->angle - p->last_angle; //正转 delta +
}
//不管正反转,肯定是转的角度小的那个是真的
if(ABS(res1)<ABS(res2))
delta = res1;
else
delta = res2;
p->total_angle += delta;
p->last_angle = p->angle;
}
u8 set_moto_current(CAN_HandleTypeDef* hcan, s16 SID, s16 iq1, s16 iq2, s16 iq3, s16 iq4)
{
hcan->pTxMsg->StdId = SID;
hcan->pTxMsg->IDE = CAN_ID_STD;
hcan->pTxMsg->RTR = CAN_RTR_DATA;
hcan->pTxMsg->DLC = 0x08;
hcan->pTxMsg->Data[0] = iq1 >> 8;
hcan->pTxMsg->Data[1] = iq1;
hcan->pTxMsg->Data[2] = iq2 >> 8;
hcan->pTxMsg->Data[3] = iq2;
hcan->pTxMsg->Data[4] = iq3 >> 8;
hcan->pTxMsg->Data[5] = iq3;
hcan->pTxMsg->Data[6] = iq4 >> 8;
hcan->pTxMsg->Data[7] = iq4;
//HAL_CAN_Transmit(hcan, 1000);
if(HAL_CAN_Transmit(&CAN1_Handler,1000)!=HAL_OK) return 1; //发送
return 0;
}
motor.c里面主要实现两个函数,一个是get_moto_measure(moto_measure_t *ptr, CAN_HandleTypeDef* hcan),用于获得各个电机的当前信息,一个是set_moto_current(CAN_HandleTypeDef* hcan, s16 SID, s16 iq1, s16 iq2, s16 iq3, s16 iq4),用来经过PID计算后,设置电机电流(速度PID输出的是电流)。
之后进入while(1)循环,查询电机当前速度(get_moto_measure),然后依次对四个电机进行PID计算(pid_calc),然后一次性将四个电机的PID计算结果输送到CAN总线(set_moto_current)。这里我还做了加减速处理,避免电机速度调整太快超过PID的输出限幅。