MSP432 库函数实现 PID 电机调角度、调速

平台：Code Composer Studio 10.4.0
MSP432P401R SimpleLink™ 微控制器 LaunchPad™ 开发套件
(MSP-EXP432P401R)

---

编码器及所用改进型PID知识见【电赛PID半天入门】从接触编码器到调出好康的PID波形
工程示例

引脚配置

PWM引脚

外部中断测量编码器引脚配置

代码部分

初始化

/*
 *  ======== mainThread ========
 */
void *mainThread(void *arg0)
{
	My_Task_Init(Key_Task, 1, 1024);
	My_Task_Init(LED_Task, 1, 1024);
    GPIO_enableInt(Encoder_1A);
    GPIO_enableInt(Encoder_1B);
    My_Uart_Init(&huart1, USB_UART, 115200);
    My_PWM_Hz_Init(&hpwm1, PWM_1A, 1000);
    My_PWM_Hz_Init(&hpwm2, PWM_1B, 1000);
    My_Task_Init(Motor_1_Task, 2, 1024);
    while(1)
    {
    	usleep(1000);
    }
}

编码器解读

由于使用的是外部中断检测，该程序仅适用于13线的霍尔编码器电机，
不适用500线的光电编码器电机，实测光电编码器电机在占空比大于44%的时候会由于高强度进入中断使得控制线程不能正常控制电机。

Encoder.c

/*
 * Encoder.c
 *
 *  Created on: 2021年8月2日
 *      Author: Royic
 */

#include "./inc/Encoder.h"

motor_type_def Motor_1;

int32_t Encoder_1_Count = 0;

const float Step_Angle = 360. / (Encoder_1_PPR * Encoder_1_Ratio * 2);

void Encoder_1A_Func(void)
{
	if(GPIO_read(Encoder_1A))
	{
		if(GPIO_read(Encoder_1B))
			Motor_1.angle -= Step_Angle;
		else
			Motor_1.angle += Step_Angle;
	}
	else
	{
		if(GPIO_read(Encoder_1B))
			Motor_1.angle += Step_Angle;
		else
			Motor_1.angle -= Step_Angle;
	}
}

void Encoder_1B_Func(void)
{
	if(GPIO_read(Encoder_1B))
	{
		if(GPIO_read(Encoder_1A))
			Motor_1.angle += Step_Angle;
		else
			Motor_1.angle -= Step_Angle;
	}
	else
	{
		if(GPIO_read(Encoder_1A))
			Motor_1.angle -= Step_Angle;
		else
			Motor_1.angle += Step_Angle;
	}
}

void Motor_Get_Speed(motor_type_def *Motor, float Angle_Now, float Delta_S)
{
	Motor->angle = Angle_Now;
	Motor->speed = (Motor->angle - Motor->angle_old)/Delta_S;
	Motor->angle_old = Angle_Now;
}

Encoder.h

/*
 * Encoder.h
 *
 *  Created on: 2021年8月2日
 *      Author: Royic
 */

#ifndef INC_ENCODER_H_
#define INC_ENCODER_H_

#include "./inc/main.h"

#include <ti/drivers/GPIO.h>

#define Encoder_1_PPR 	13
#define Encoder_1_Ratio 10

typedef struct
{
	double angle;
	double angle_old;
	double speed;
} motor_type_def;

void Motor_Get_Speed(motor_type_def *Motor, float Angle_Now, float Delta_S);

extern int32_t Encoder_1_Count;
extern float Motor_1_Angle;
extern motor_type_def Motor_1;

#endif

测速和控制部分

#define Motor_Angle_KP	0.5
#define Motor_Angle_KI	0.0025
#define Motor_Angle_KD	15

#define Motor_Speed_KP	0.1
#define Motor_Speed_KI	0.001
#define Motor_Speed_KD	1

uint8_t Angle_Or_Speed = 1;

float Target_Angle = 0;
float Target_Speed = 0;

pid_type_def Motor_Angle_PID;
pid_type_def Motor_Speed_PID;
const static fp32 motor_angle_pid[3]	= {Motor_Angle_KP, Motor_Angle_KI, Motor_Angle_KD};
const static fp32 motor_speed_pid[3]	= {Motor_Speed_KP, Motor_Speed_KI, Motor_Speed_KD};

Kalman_Typedef Speed_Kalman;

void *Motor_1_Task(void *arg0)
{
	float Control_Var = 0;
	PID_init(&Motor_Angle_PID, PID_POSITION, motor_angle_pid, 100, 100, 30, 2);
	PID_init(&Motor_Speed_PID, PID_POSITION, motor_speed_pid, 100, 100, 500, 0);
	Kalman_Init(&Speed_Kalman, 1e-6, 0.001);
	while(1)
	{
		Motor_Get_Speed(&Motor_1, Motor_1.angle, 0.001);
		KalmanFilter(&Speed_Kalman, Motor_1.speed);
		if(Angle_Or_Speed)
		{
			PID_calc(&Motor_Angle_PID, Motor_1.angle, Target_Angle);
			Control_Var = Motor_Angle_PID.out;
		}
		else
		{
			PID_calc(&Motor_Speed_PID, Speed_Kalman.out, Target_Speed);
			Control_Var = Motor_Speed_PID.out;
		}
		if(Control_Var > 0)
		{
			My_PWM_setDuty(&hpwm1, Control_Var);
			My_PWM_setDuty(&hpwm2, 0);
		}
		else
		{
			My_PWM_setDuty(&hpwm1, 0);
			My_PWM_setDuty(&hpwm2, -Control_Var);
		}
		if(Angle_Or_Speed)
			UART_printf(huart1, "%d.%d, %d.%d\r\n", (int)Motor_1.angle, (int)(Motor_1.angle * 100) % 100 , (int)Target_Angle, (int)(Target_Angle * 100) % 100);
		else
			UART_printf(huart1, "%d.%d, %d.%d, %d.%d\r\n", (int)Speed_Kalman.out, (int)(Speed_Kalman.out * 100) % 100 , (int)Target_Speed, (int)(Target_Speed * 100) % 100, (int)Motor_1.speed, (int)(Motor_1.speed * 100) % 100);
		usleep(1000);
	}
}

卡尔曼滤波器，用于对所测速度进行滤波

kalman.c

/*
卡尔曼滤波器
整理By 乙酸氧铍
*/
#include "kalman.h"

double KalmanFilter(Kalman_Typedef *klm, double input)
{
    //预测协方差方程：k时刻系统估算协方差 = k-1时刻的系统协方差 + 过程噪声协方差
    klm->Now_P = klm->LastP + klm->Q;
    //卡尔曼增益方程：卡尔曼增益 = k时刻系统估算协方差 / （k时刻系统估算协方差 + 观测噪声协方差）
    klm->Kg = klm->Now_P / (klm->Now_P + klm->R);
    //更新最优值方程：k时刻状态变量的最优值 = 状态变量的预测值 + 卡尔曼增益 * （测量值 - 状态变量的预测值）
    klm->out = klm->out + klm->Kg * (input -klm->out);//因为这一次的预测值就是上一次的输出值
    //更新协方差方程: 本次的系统协方差赋给 klm->LastP 为下一次运算准备。
    klm->LastP = (1-klm->Kg) * klm->Now_P;
	
	return (klm->out);
}

void Kalman_Init(Kalman_Typedef *klm, const double klm_Q, const double klm_R)//温度klm_Q=0.01 klm_R=0.25
{
	klm->LastP=0.02;		//上次估算协方差
	klm->Now_P=0;			//当前估算协方差
	klm->out=0;				//卡尔曼滤波器输出
	klm->Kg=0;				//卡尔曼增益
	klm->Q=klm_Q;			//Q:过程噪声协方差 Q参数调滤波后的曲线平滑程度，Q越小越平滑;
	klm->R=klm_R;			//R:观测噪声协方差 R参数调整滤波后的曲线与实测曲线的相近程度，R越小越接近(收敛越快)
}

kalman.h

#ifndef __KALMAN_H__
#define __KALMAN_H__

typedef struct
{
    /*不用动*/
    double LastP;//上次估算协方差
    double Now_P;//当前估算协方差
    double out;//卡尔曼滤波器输出
    double Kg;//卡尔曼增益
	double Q;
	double R;
}Kalman_Typedef;

void Kalman_Init(Kalman_Typedef *klm, const double klm_Q, const double klm_R);
double KalmanFilter(Kalman_Typedef *klm, double input);

#endif

实验效果

速度滤波效果

控速效果

先加速再减速

控角效果