平台：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;elseMotor_1.angle += Step_Angle;}else{if(GPIO_read(Encoder_1B))Motor_1.angle += Step_Angle;elseMotor_1.angle -= Step_Angle;}
}void Encoder_1B_Func(void)
{if(GPIO_read(Encoder_1B)){if(GPIO_read(Encoder_1A))Motor_1.angle += Step_Angle;elseMotor_1.angle -= Step_Angle;}else{if(GPIO_read(Encoder_1A))Motor_1.angle -= Step_Angle;elseMotor_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 10typedef 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	1uint8_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);elseUART_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

实验效果

速度滤波效果

控速效果

先加速再减速

控角效果