目录
平台: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
实验效果
速度滤波效果
控速效果
先加速再减速
控角效果