目錄

• 引腳配置
• • PWM引腳
  • 外部中斷測量編碼器引腳配置
• 代碼部分
• • 初始化
  • 編碼器解讀
  • • Encoder.c
    • Encoder.h
  • 測速和控制部分
  • 卡爾曼濾波器，用于對所測速度進(jìn)行濾波
  • • kalman.c
    • kalman.h
• 實驗效果
• • 速度濾波效果
  • 控速效果
  • 控角效果

平臺：Code Composer Studio 10.4.0
MSP432P401R SimpleLink? 微控制器 LaunchPad? 開發(fā)套件
(MSP-EXP432P401R)

---

編碼器及所用改進(jìn)型PID知識見【電賽PID半天入門】從接觸編碼器到調(diào)出好康的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線的霍爾編碼器電機(jī)，
不適用500線的光電編碼器電機(jī)，實測光電編碼器電機(jī)在占空比大于44%的時候會由于高強(qiáng)度進(jìn)入中斷使得控制線程不能正常控制電機(jī)。

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);} }

卡爾曼濾波器，用于對所測速度進(jìn)行濾波

kalman.c

/* 卡爾曼濾波器 整理By 乙酸氧鈹 */ #include "kalman.h"double KalmanFilter(Kalman_Typedef *klm, double input) {//預(yù)測協(xié)方差方程：k時刻系統(tǒng)估算協(xié)方差 = k-1時刻的系統(tǒng)協(xié)方差 + 過程噪聲協(xié)方差klm->Now_P = klm->LastP + klm->Q;//卡爾曼增益方程：卡爾曼增益 = k時刻系統(tǒng)估算協(xié)方差 / （k時刻系統(tǒng)估算協(xié)方差 + 觀測噪聲協(xié)方差）klm->Kg = klm->Now_P / (klm->Now_P + klm->R);//更新最優(yōu)值方程：k時刻狀態(tài)變量的最優(yōu)值 = 狀態(tài)變量的預(yù)測值 + 卡爾曼增益 * （測量值 - 狀態(tài)變量的預(yù)測值）klm->out = klm->out + klm->Kg * (input -klm->out);//因為這一次的預(yù)測值就是上一次的輸出值//更新協(xié)方差方程: 本次的系統(tǒng)協(xié)方差賦給 klm->LastP 為下一次運(yùn)算準(zhǔn)備。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; //上次估算協(xié)方差klm->Now_P=0; //當(dāng)前估算協(xié)方差klm->out=0; //卡爾曼濾波器輸出klm->Kg=0; //卡爾曼增益klm->Q=klm_Q; //Q:過程噪聲協(xié)方差 Q參數(shù)調(diào)濾波后的曲線平滑程度，Q越小越平滑;klm->R=klm_R; //R:觀測噪聲協(xié)方差 R參數(shù)調(diào)整濾波后的曲線與實測曲線的相近程度，R越小越接近(收斂越快) }

kalman.h

#ifndef __KALMAN_H__ #define __KALMAN_H__typedef struct {/*不用動*/double LastP;//上次估算協(xié)方差double Now_P;//當(dāng)前估算協(xié)方差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

實驗效果

速度濾波效果

控速效果

先加速再減速

控角效果

總結(jié)

以上是生活随笔為你收集整理的MSP432 库函数实现 PID 电机调角度、调速的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。