815浏览
查看: 815|回复: 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]


IMG_20200314_084852.jpg

【演示视频】




rzyzzxw  版主

发表于 2020-3-15 09:30:06

追剧中……
回复

使用道具 举报

gada888  版主

发表于 2020-3-25 14:37:23

算法库
回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

为本项目制作心愿单
购买心愿单
心愿单 编辑
[[wsData.name]]

硬件清单

  • [[d.name]]
btnicon
我也要做!
点击进入购买页面
上海智位机器人股份有限公司 沪ICP备09038501号-4

© 2013-2022 Comsenz Inc. Powered by Discuz! X3.4 Licensed

mail