引脚配置
- PWM引脚
- 外部中断测量编码器引脚配置
代码部分
- 初始化
- 编码器解读
- - Encoder.c
  - Encoder.h
- 测速和控制部分
- 卡尔曼滤波器，用于对所测速度进行滤波
- - kalman.c
  - kalman.h
实验效果
- 速度滤波效果
- 控速效果
- 控角效果

平台：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

实验效果

速度滤波效果

控速效果

先加速再减速

控角效果

MSP432 库函数实现 PID 电机调角度、调速相关推荐

航模基础知识之电机，电调，电池的选择
在设计一款小型无人车,无人船,多旋翼,固定翼的时候.一定会面对的一个问题是怎么选择合适的电机,电池,电调.这三者配合的好,机器才可以发挥优秀的性能,否则会有烧坏设备的可能.所以接下来介绍如何设计电机, ...
《无人机电机与电调技术》可以看看
你不是一直不懂电机和电调的选型么,只知道别人说是这样就是这样.
连接电机和电调时，那三根线可以先任意顺序接
连接电机和电调时,那三根线可以先任意顺序接,后面旋转方向不对,可以再把任意两根线交换. 电机应该往哪个方向旋转,你可以直接看桨叶,你看桨叶往哪个方向转才会是升力,这个很好看得出来. 刚刚更换无名的一个 ...
MSP432库函数输出多路任意占空比PWM波
一.在电赛开始的前一天中午,突然得知2021电赛电赛推迟至8月11号至14号,现在晚上又通知变成了择期,比赛时间遥遥无期既然如此那就再继续拼搏吧,顺便也分享一下备赛以来对msp432的学习二.前情 ...
weblogic java虚拟机_weblogic server 性能及调优-调优 java 虚拟机.doc
weblogic server 性能及调优-调优 java 虚拟机.doc 还剩 8页未读, 继续阅读下载文档到电脑,马上远离加班熬夜! 亲,喜欢就下载吧,价低环保! 内容要点: Sun 已针对 W ...
调岗调薪带来的法律风险
企业是否可以调岗调薪? 答:以下情况可以调岗调薪: 1.协商调岗调薪.一般情况下,未经劳动者同意,用人单位不得单方调岗调薪. 2.单方变更: (1)劳动者由于不能胜任工作而被用人单位单方调整工作岗位 ...
p620光耦接单片机_基于51单片机的可控硅调压调光程序-带过零检测
标签: 单片机单片机(Microcontrollers,亦称MCU)包括了CPU.随机存储器RAM.只读存储器ROM.多种I/O口和中断系统.定时器/计数器等功能集成到一块硅片上构成的一个小而完善的 ...
基于51单片机的可控硅调压调光程序-带过零检测
基于51单片机的可控硅调压调光程序-带过零检测 #include<reg52.h> #include<intrins.h> sbitledRS=P1^0; sbitledRW= ...
不要抱怨网速慢，只怪自己不会调快网速（一分钟解决网速问题）
不要抱怨网速慢,只怪你自己不会调快网速 (一分钟解决网速问题)点击"分享"保存起来慢慢学! 电脑系统都会默认限制20%的网速,我们可以很轻松地解除这个限制,使你的上网速度达到100 ...

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

目录