【Arduino 101】五分钟搞懂PID控制算法

---

演示视频

【Arduino 101】五分钟搞懂PID控制算法

---

物料清单

Arduino Uno x 1
超声波模块（HC-SR04）x 1
舵机（Tower Pro MG996R）x 1
舵机供电： 5V 稳压管(LM7805) x 1 , 滤波电容 (0.33uF, 0.1uF) x 2， 9V 电池 x 1
滑轨：泡沫板
支架：乐高
滚动物体：胶带（Up试过网球，但是测距模块的表现不理想）

---

尺寸 & 设计缺陷

设计缺陷：
1： 测距模块读数不稳定，存在+/-5mm 的浮动。
2： 整体制作比较粗糙，轨道摩擦不均匀。对稳态误差的出现原因仅仅是个猜测，所以视频中用了“可能”两个字。

---

接线

---

Arduino IDE 代码

/* PID control Demo, PID 控制算法演示
 * Last Edited: March.12th.2021 by Mun Kim
 * Contact: Robotix.Kim@gmail.com
 */

#include <Servo.h>

Servo servo;   
int angle = 0;                           //舵机角度

#define echo 2                           //HC-SR04的Echo接 Arduino D2
#define trig 3                           //HC-SR04的Trig接 Arduino D3
float time, duration, distance;

//PID constants****************************************************************
float setPoint=19;                       //滑轨中心与测距模块的距离
float error;                             //当前误差
float previous_error;                    //上一时刻误差，用来计算D
float kp=0;                              //10
float ki=0;                              //0.05
float kd=0;                              //200 
int   dt=50;                             //每50毫秒进行一次计算                       
float P,I,D, PID;
//*****************************************************************************

void setup() {
  Serial.begin(9600);
  pinMode(trig, OUTPUT); 
  pinMode(echo, INPUT); 
  servo.attach(9);                 
  time = millis();
}

void loop() {
    if (millis() > time + dt)
  {
    time = millis();    

    // HC-SR04 Distance Measuremnt, 通过超声波模块测量胶带的位置*******************
    digitalWrite(trig, LOW); 
    delayMicroseconds(2); 
    digitalWrite(trig, HIGH); 
    delayMicroseconds(10); 
    digitalWrite(trig, LOW); 

    duration = pulseIn(echo, HIGH);        //读取反射信号 
    distance = (duration*0.0343)/2;        //测距公式，单位为cm
    
    // PID Calculation, PID计算*************************************************
    error = distance-setPoint;             //计算误差
    P = kp*error;                          //P项
    if(-4 < error && error < 4){
       I += ki*error;    }                 //I项
    else{
       I = 0;            }   
    D = kd*((error - previous_error)/dt);  //D项，
    PID=P + I + D ;                        //PID
    if (PID>200){PID=200;}                 //限幅
    if (PID<-200){PID=-200;}               //限幅

    previous_error=error;                  //更新上一时刻误差
    
    // Serial Display,在串口监视器显示每个项，有助于调试*****************************
     Serial.print("Distance: "); Serial.print(distance); Serial.print(" cm  "); 
     Serial.print("Error: ");    Serial.print(error);    Serial.print(" cm  ");
     Serial.print("   P: ");     Serial.print(P); 
     Serial.print("   D: ");     Serial.print(D);
     Serial.print("   I: ");     Serial.print(I);
     Serial.print("   PID: ");   Serial.println(PID);
    
    // Servo Control，用计算结果控制舵机*******************************************
    angle = map(PID, -200,200,150,20);
    servo.write(angle); 
  }
}

关于霍尔电机的转速控制

Up只是把上面有关PID计算的代码块搬到上期的项目中，替代了起停式控制的那一部分。dt=1s。详细的接线以及代码请大家参考这个博文：
【Arduino 101】霍尔编码器（增量，正交）与起停式闭环控制