【自制无人机】“辛”路历程（九）PID库
又一个通俗的介绍PID
https://www.sohu.com/a/303474526_694970
【前言】
今天桨叶回来了，有8个的桨叶尺寸太大了，我的无刷电机KV值2千多，用不了。先测试一下：
https://v.youku.com/v_show/id_XNDU4Nzg1MTA3Mg==.html
大家一定要根据PID控制做一个简单的项目，然后再慢慢研究，才会深刻理解，和灵活运用。上一贴中使用了PID算法，网上还有PID库供大家使用:
1、Arduino PID 库函数指南
2、使用PID库，轻松搞定PID（上）；使用PID库，轻松搞定PID（下）
根据以下帖子进行改进。
使用PID库，轻松搞定PID（完整版）
【PID改进】
代码：
#defineMAX_RANG (520)//模块测距极值为520cm(比有效最大量程值略大)
#defineADC_SOLUTION (1023.0)//Arduino UNO 的ADC精度为10bit
int sensityPin = A0;
int input1 = 5; // pin 5 向 input1 输出
int input2 = 6; // pin 6 向 input2 输出
int enA = 10; //pin 10 向 输出A使能端输出
int direction1 = 0;
unsigned long lastTime;
double Input, Output, Setpoint;
double ITerm, lastInput;
double kp, ki, kd;
int SampleTime=1000;
double outMin,outMax;
bool inAuto=false;
#define MANUAL 0
#define AUTOMATIC 1
void Compute()
{
if(!inAuto) return;
/*How long since we last calculated*/
unsigned long now = millis();
int timeChange = (now - lastTime);
if(timeChange>=SampleTime)
{
/*Compute all the working error variables*/
double error = Setpoint - Input;
ITerm += (ki*error);
if(ITerm>outMax) ITerm=outMax;
else if(ITerm<outMin) ITerm=outMin;
double dInput = (Input - lastInput);
/*Compute PID Output*/
Output = kp * error +ITerm - kd * dInput;
/*Remember some variables for next time*/
lastInput = Input;
lastTime = now;
if(error<0){
direction1=0;
}
else
{
direction1=1;
}
}
}
void SetOutputImits(double Min,double Max)
{
if(Min>Max) return;
outMin=Min;
outMax=Max;
if(Output>outMax) Output=outMax;
else if(Output<outMin) Output=outMin;
if(ITerm>outMax) ITerm=outMax;
else if(ITerm<outMin) ITerm=outMin;
}
void SetTunings(double Kp, double Ki, double Kd)
{
double SampleTimeInSec=((double)SampleTime)/1000;
kp = Kp;
ki = Ki*SampleTimeInSec;
kd = Kd/SampleTimeInSec;
}
void SetSampleTime(int NewSampleTime)
{
if(NewSampleTime>0)
{
double ratio=(double)NewSampleTime/(double)SampleTime;
ki*=ratio;
kd/=ratio;
SampleTime=(unsigned long)NewSampleTime;
}
}
void SetMode(int Mode)
{
bool newAuto=(Mode==AUTOMATIC);
if(newAuto && !inAuto)
{
Initialize();
}
inAuto=newAuto;
}
void Initialize()
{
lastInput=Input;
ITerm=Output;
if(ITerm>outMax) ITerm=outMax;
else if(ITerm<outMin) ITerm=outMin;
}
void setup() {
// Serial init
Serial.begin(9600);
pinMode(input1,OUTPUT);
pinMode(input2,OUTPUT);
pinMode(enA,OUTPUT);
Setpoint=30;
SetMode(AUTOMATIC);
SetTunings(2, 0.001, 0.01);
SetOutputImits(0,200);
}
float dist_t, sensity_t;
void loop() {
// read the value from the sensor:
sensity_t = analogRead(sensityPin);
// turn the ledPin on
dist_t = sensity_t * MAX_RANG/ ADC_SOLUTION;//
Input=dist_t;
/*Serial.print(dist_t,0);*/
Compute();
if(direction1==0)
{
digitalWrite(input1,HIGH); //给高电平
digitalWrite(input2,LOW);//给低电平
analogWrite(enA,50-Output);
}
else
{
digitalWrite(input1,LOW); //给高电平
digitalWrite(input2,HIGH);//给低电平
analogWrite(enA,50+Output);
}
Serial.println(Output,0);
}
【演示视频】
https://v.youku.com/v_show/id_XNDU4ODAyNTczMg==.html
追剧中……{:7_218:} 算法库
页:
[1]