»論壇 › 嵌入式/單片機論壇 › STM32/8 › 風力擺控制系統賽題解析與操作說明及STM32源碼(重要的是 ...

風力擺控制系統賽題解析與操作說明及STM32源碼(重要的是PID算法)

查看數: 9883 | 評論數: 16 | 收藏 8

關燈 | 提示：支持鍵盤翻頁<-左右->

帖子模式

陳某

發布時間: 2018-8-8 16:30

正文摘要:

電子競賽題目《風力擺控制系統》本人的課程設計；重要的是PID算法。

回復

ID:328014 發表于 2021-3-14 15:21

可樂我要檸檬味發表于 2021-3-14 14:58
為什么壓縮包里面的程序沒有看到PID部分哇

是這個嗎Subject.c

#include "Subject.h"
#include "Data.h"
#include "math.h"
#include "moto_PWM.h"
#include "math.h"
#define H 79.0 //萬向節距離地面的高度
float R=25.0; //擺動弧度的大小半徑
#define T 1410.0 //擺動的周期
float angle = 45.0;
extern Data PID_data;
extern Data PID_data1;
uint8_t RoundDir=1;
//const float Phase;
void Subject_1(void)
{
float A=0.0;
static uint32_t MoveTimeCnt = 0;
float set_y = 0.0;
float Normalization = 0.0;
float Omega = 0.0;
MoveTimeCnt+=5;
Normalization=(float)MoveTimeCnt/T;
Omega=2*3.1415926*Normalization; //求一個角度隨時間變化的每5毫秒加2*PI*5/T度來求相位
A=atan((R/79.0f))*57.2958f; //最大角度擺的最大高度
set_y=A*sin(Omega); //軌跡函數
PID_data.setPoint=0;
PID_data.P=170;
PID_data.I=5.8;
PID_data.D=500;
PID_data1.setPoint=set_y;
PID_data1.P=150;
PID_data1.I=6.8;
PID_data1.D=500;
PID_data.PWM=PID_Calc1();
PID_data1.PWM=PID_Calc2();
if(PID_data.PWM>1500)
PID_data.PWM=1500;
/*else if(PID_data.PWM>0&&PID_data.PWM<450)
PID_data.PWM=450;*/
if(PID_data.PWM<-1500)
PID_data.PWM=-1500;
/*else if(PID_data.PWM<0&&PID_data.PWM>-450)
PID_data.PWM=-450;*/
if(PID_data1.PWM>1500)
PID_data1.PWM=1500;
if(PID_data1.PWM<-1500)
PID_data1.PWM=-1500;
moto_PWM(PID_data.PWM,PID_data1.PWM);
}
void Subject_2(void)
{
float A=0.0;
static uint32_t MoveTimeCnt = 0;
float set_x = 0.0;
float Normalization = 0.0;
float Omega = 0.0;
MoveTimeCnt+=5;
Normalization=(float)MoveTimeCnt/T;
Omega=2*3.1415926*Normalization;
A=atan((R/79.0f))*57.2958f;
set_x=A*sin(Omega);
PID_data.setPoint=set_x;
PID_data.P=191;
PID_data.I=5.9;
PID_data.D=420;
PID_data1.setPoint=0;
PID_data1.P=100;
PID_data1.I=5.8;
PID_data1.D=500;
PID_data.PWM=PID_Calc1();
PID_data1.PWM=PID_Calc2();
if(PID_data.PWM>1500)
PID_data.PWM=1500;
if(PID_data.PWM<-1500)
PID_data.PWM=-1500;
if(PID_data1.PWM>1500)
PID_data1.PWM=1500;
if(PID_data1.PWM<-1500)
PID_data1.PWM=-1500;
moto_PWM(PID_data.PWM,PID_data1.PWM);
}
void Subject_3(void)
{
//const float priod = 1410.0; //單擺周期(毫秒)
//相位補償 0, 10 20 30 40 50 60 70 80 90 100 110 120 130 140 150 160 170 180
const float Phase[19]= {0,-0.1,-0.05,0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,0.05,0.05,0.05,0.07,0};
static uint32_t MoveTimeCnt = 0;
float set_x = 0.0;
float set_y = 0.0;
float Ax = 0.0;
float Ay = 0.0;
float A = 0.0;
uint32_t pOffset = 0;
float Normalization = 0.0;
float Omega = 0.0;
pOffset = (uint32_t)(angle/10.0f); //相位補償數組下標
MoveTimeCnt += 5; //每5ms運算1次
Normalization = (float)MoveTimeCnt/T; //對單擺周期歸一化
Omega = 2.0*3.14159*Normalization; //對2π進行歸一化處理
A = atan((R/79.0f))*57.2958f;//根據擺幅求出角度A,88為擺桿離地高度
Ax = A*cos(angle*0.017453); //計算出X方向擺幅分量0.017453為弧度轉換
Ay = A*sin(angle*0.017453); //計算出Y方向擺幅分量
set_x = Ax*sin(Omega); //計算出X方向當前擺角
set_y = Ay*sin(Omega+Phase[pOffset]); //計算出Y方向當前擺角
PID_data.setPoint=set_y;
PID_data.P=180;
PID_data.I=6.5;
PID_data.D=500;
PID_data1.setPoint=set_x;
PID_data1.P=110;
PID_data1.I=5.9;
PID_data1.D=500;
PID_data.PWM=PID_Calc1();
PID_data1.PWM=PID_Calc2();
if(PID_data.PWM>1700)
PID_data.PWM=1700;
if(PID_data.PWM<-1700)
PID_data.PWM=-1700;
if(PID_data1.PWM>1700)
PID_data1.PWM=1700;
if(PID_data1.PWM<-1700)
PID_data1.PWM=-1700;
moto_PWM(PID_data.PWM,PID_data1.PWM);
}