STM32实现四驱小车（五）电机控制任务——电机速度PID控制算法

其他 2021-03-31 20:52:22 阅读次数: 0

目录

一. 绪论

本文接上一篇STM32实现四驱小车（四）姿态控制任务——偏航角串级PID控制算法，在本文中介绍电机的控制原理和使用CAN总线实现电机速度闭环的代码实操，最后实现电机的速度控制任务。

二. 电机速度环PID原理

电机的种类特别多，小型机器人（无人机、无人车）上普遍使用直流无刷电机，航模使用的一般是高速低扭矩，车模一般通过减速器低速实现高扭矩。一般电机与电机驱动器配套使用，用的话只需要知道如何供电和如何给控制信号就行了。本文使用大疆的M3508电机，使用CAN总线进行控制，长相如图所示。
在这里插入图片描述
CAN总线是汽车行业的应用标准，稍大型的电机很多是CAN总线控制的。CAN总线是一种现场总线，具有速度快、传输距离远、连接节点多的优点，具有错误检测、错误通知和错误恢复功能，使得其特别适合工业过程监控设备的互联。CAN总线的知识点很复杂，现在升级版的CAN-Open更是乖乖不得了，我啃了好几天才大致弄懂一些。大家主要要会用就行了。

回到我们的电机，众所周知伺服电机有位置伺服、速度伺服、力伺服，大家一定也听过电机的三闭环控制，也就是位置环——速度环——电流环，如图所示，根据控制目标的不同，从外环到内环分别对应电机的位置伺服、速度伺服与力伺服，也可以说三种模式：位置模式、速度模式、转矩模式。
在这里插入图片描述上一篇我们讲过姿态环的角度、角速度串级PID，所以看到这里的位置环——速度环——电流环大家一定不陌生了，而且似乎发现了什么有趣的东西，是不是外环的控制变量和内环的控制变量依次成导数关系？（电流决定的就是力，也就是角加速度）。

电机一般要配合驱动控制器使用，有的驱动器很牛皮，里面做好了三种模式，控制器给位置信号、速度信号就行了。有的次一点，只做了速度伺服，有的更次，啥也没做。我们今天用的大疆M33508电机就啥也没做，，，驱动器只是上报电机的速度、电流、转角、温度等信息，需要控制器去读这些信息，然后做闭环计算，控制器将计算结果传输给驱动器，再去控制电机电流。这里我们主要是做速度伺服。上图变成这样。
在这里插入图片描述

三. STM32使用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里面主要实现两个函数，一个是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输出的是电流）。

四. UCOS-III电机控制任务的实现

写完了驱动函数和初级应用函数，下面可以写核心代码了，在上一章STM32实现四驱小车（四）姿态控制任务——偏航角串级PID控制算法的基础上，补充完整motor_task.

//motor任务
u8 motor_task(void *p_arg)
{
    
    
	OS_ERR err;
	CPU_SR_ALLOC();

	u8 i;
	u16 retry;
	u8 flag_motor[motor_num];		//电机信息接受成功标志
	pid_t pid_speed[motor_num];		//电机速度PID环
	float set_speed_temp;			//加减速时的临时设定速度
	int16_t delta;					//设定速度与实际速度只差值
	int16_t max_speed_change = 400; //电机单次最大变化速度，加减速用
	//PID初始化
	for (i = 0; i < 4; i++)
	{
    
    
		PID_struct_init(&pid_speed[i], POSITION_PID, 16384, 16384,
						1.5f, 0.1f, 0.0f); //4 motos angular rate closeloop.
	}

	while (1)
	{
    
    
		//PID采样，获取电机数据
		retry = 0;
		flag_motor[0] = flag_motor[1] = flag_motor[2] = flag_motor[3] = 0;
		while (retry < 20)
		{
    
    
			get_moto_measure(&moto_info, &CAN1_Handler);
			if (CAN1_Handler.pRxMsg->StdId == 0x201)
			{
    
    
				moto_chassis[0] = moto_info;
				flag_motor[0] = 1;
			}
			if (CAN1_Handler.pRxMsg->StdId == 0x202)
			{
    
    
				moto_chassis[1] = moto_info;
				flag_motor[1] = 1;
			}
			if (CAN1_Handler.pRxMsg->StdId == 0x203)
			{
    
    
				moto_chassis[2] = moto_info;
				flag_motor[2] = 1;
			}
			if (CAN1_Handler.pRxMsg->StdId == 0x204)
			{
    
    
				moto_chassis[3] = moto_info;
				flag_motor[3] = 1;
			}
			
			if (flag_motor[0] && flag_motor[2]&& flag_motor[1]&& flag_motor[3])
				break;
			else
				retry++;
		}

		//PID计算输出

		//OS_CRITICAL_ENTER();
		for (i = 0; i < 5; i++)
		{
    
    
			//PID计算

			//加减速
			delta = (int16_t)set_speed[i] - moto_chassis[i].speed_rpm;
			if (delta > max_speed_change)
				set_speed_temp = (float)(moto_chassis[i].speed_rpm + max_speed_change);
			else if (delta < -max_speed_change)
				set_speed_temp = (float)(moto_chassis[i].speed_rpm - max_speed_change);
			else
				set_speed_temp = set_speed[i];
			//pid_calc(&pid_speed[i], (float)moto_chassis[i].speed_rpm, set_speed[i]);
			pid_calc(&pid_speed[i], (float)moto_chassis[i].speed_rpm, set_speed_temp);
		}

//		printf("PID out: %f\r\n", pid_speed[1].pos_out);
//		printf("real speed: %d \r\n", moto_chassis[1].speed_rpm);
//		printf("set speed: %f \r\n", set_speed[1]);
		//PID 输出

		set_moto_current(&CAN1_Handler, 0x200, (s16)(pid_speed[0].pos_out),
						 (s16)(pid_speed[1].pos_out),
						 (s16)(pid_speed[2].pos_out),
						 (s16)(pid_speed[3].pos_out));

		//OS_CRITICAL_EXIT();
		delay_ms(5);
	}
}

电机任务首先进行PID的初始化，PID_struct_init(&pid_speed[i], POSITION_PID, 16384, 16384, 1.5f, 0.1f, 0.0f)。初始化四个电机为位置式PID、PID输出限幅16384，PID参数为Kp=1.5， Ki=0.1, Kd=0。

之后进入while（1）循环，查询电机当前速度（get_moto_measure），然后依次对四个电机进行PID计算(pid_calc)，然后一次性将四个电机的PID计算结果输送到CAN总线(set_moto_current)。这里我还做了加减速处理，避免电机速度调整太快超过PID的输出限幅。

到此完整的链路就实现了。建议大家再把前面的四篇文章读一下，对于整个控制逻辑和数据传输链路就会更清楚了。整体来说四个任务是一环套一环的，但是又是独立的。

这样的代码架构在程序设计与代码调试上会非常快，能够快速定位Bug，所以建议还在裸奔的小伙伴们去学一学操作系统，会大大提升程序设计水平。

写作不易，能够看到这里的一定是真真爱了，麻烦给个赞与关注呗~

猜你喜欢

转载自blog.csdn.net/qq_30267617/article/details/113562604
今日推荐
面壁智能发布 Eurux-8x22B 开源大模型 —— 堪称「理科状元」
开源日报 | 谷歌扶持鸿蒙上位；开源Rabbit R1；Docker加持的安卓手机；微软的焦虑和野心；海尔电器把开放平台关了
中国码农的“35岁魔咒”
蘭雅 CorelDRAW 插件 2024.5.1 国际劳动节版，免费下载
Arc Browser for Windows 1.0 正式 GA
90后程序员开发视频搬运软件、不到一年获利超 700 万，结局很刑！
周排行
【转】spring中对控制反转和依赖注入的理解
tms webcore 安装和使用
java程序员进阶相关书籍
SpringMVC接受请求参数、
如何保存训练好的机器学习模型
MyEclipse、Eclipse设置项目JDK的三个地方
商超行业微信小程序开发定制一般多少钱 （行业技术人员解读）
Markdown编辑器语言——30分钟入门到到精通
Linux系统下MongoDB的简单安装与基本操作
Power Strings
每日归档
更多
2024-05-07(14)
2024-05-06(40)
2024-05-05(0)
2024-05-04(7)
2024-05-03(19)
2024-05-02(0)
2024-05-01(4)
2024-04-30(1)
2024-04-29(40)
2024-04-28(0)

代码天地 © 2018 codetd.com

境外服务器   最新电视剧2022