查看: 2478|回复: 2

【自制无人机】“辛”路历程（九）PID库

又一个通俗的介绍PID
https://www.sohu.com/a/303474526_694970

【前言】

今天桨叶回来了，有8个的桨叶尺寸太大了，我的无刷电机KV值2千多，用不了。先测试一下：

大家一定要根据PID控制做一个简单的项目，然后再慢慢研究，才会深刻理解，和灵活运用。上一贴中使用了PID算法，网上还有PID库供大家使用:

1、Arduino PID 库函数指南
2、使用PID库，轻松搞定PID（上）；使用PID库，轻松搞定PID（下）

根据以下帖子进行改进。
使用PID库，轻松搞定PID（完整版）

【PID改进】
代码：

[mw_shl_code=java,false]#define  MAX_RANG    (520)//模块测距极值为520cm(比有效最大量程值略大)
#define  ADC_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);

}[/mw_shl_code]

【自制无人机】“辛”路历程（九）PID库图1