»论坛 › 嵌入式/单片机论坛 › STM32/8 › 风力摆控制系统赛题解析与操作说明及STM32源码(重要的是 ...

风力摆控制系统赛题解析与操作说明及STM32源码(重要的是PID算法)

查看数: 9808 | 评论数: 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);
}