How can we help you today?
< All Topics
Print

【演算】PID基礎 – PingPong球架

Posted2025 年 12 月 7 日
Updated2025 年 12 月 9 日
Byzerovoyager

前言

PID演算法起源可追溯至1890年代(wikipedia)，迄今在控制工程上仍是一項重要的演算與技術實務。PID經常融入其他演算，如kalman濾波、模糊、ML…等演算法融合進而形成更加智慧型的控制系統，成為了近年無人載具的工程設計上不可或缺的篇章。

歷久彌新的PID對於初學者而言，經常存在一個無形門檻。因為理解PID控制，隱含著要理解【數學(比例、微分、積分)、物理(慣性、阻尼..)與科技(實踐方法)】之間的對應關係，抽象思考力需要高些，也需要較多的先備知識與實作技能。為了讓初學者可在實作中認識並反思PID控制理論架構，一套簡易、材料容易取得的PID實驗教具，似乎有了必要性。於是我開展了這dual2s-x微專題：運用乒乓球、5mm塑料管(吸管…)、超音波、SG90伺服馬達與3D列印結構等材料，製作一套PID個人實驗教具。

👉玩家請留意：我們都知道PID的效能與系統的結構線性度、感測與制動響應速率，有著密不可分的關係。不過，結構與響應越好的元件，價格也隨之而上。此處使用的超音波、SG90伺服馬達元件響應速率有限，對於進階、高精度與響應度的PID系統要求者而言，此系統是無法滿足的呦~😊

Quick DEMO

3D模型

專題材料

控制器：dual2s系列 (read more) / esp32

元件：超音波、SG90(s)伺服馬達

螺絲：M3-16L x 2, 螺釘：M3-8L x 3

結構件：3D列印(模型下載)、5mm管材(塑料管)

範例程式

#include <Arduino.h>
#include <Servo.h>
#include <Adafruit_NeoPixel.h>

// WS2812B 設定
#define LED_PIN     2
#define NUM_LEDS    1
Adafruit_NeoPixel strip(NUM_LEDS, LED_PIN, NEO_GRB + NEO_KHZ800);

bool swTarget = false; 
int TargetCnt = 0;
bool sw = 0.0; 
bool doPID = 0.0; 

#define TRIG_PIN 27
#define ECHO_PIN 13
#define SERVO_PIN 19

#define SvoZero 80

Servo myservo;

// ====== PID 參數 ======
float Kp = 1.0;     // 比例增益  
float Ki = 0.0;     // 積分增益  
float Kd = 0.1;     // 微分增益  

// PID 變數
float target = 8.0;      // 目標距離 10 cm
float error, lastError = 0;
float integral = 0;
unsigned long lastTime;

// 非阻塞時間控制
unsigned long lastPID = 0;
const unsigned long PID_INTERVAL = 50; // 每 50ms 執行一次

enum USState {
  US_IDLE,
  US_TRIG_HIGH,
  US_WAIT_ECHO_HIGH,
  US_MEASURE_ECHO,
};

USState usState = US_IDLE;

unsigned long trigTime = 0;
unsigned long echoStart = 0;
unsigned long echoEnd = 0;

float distanceCM = 0;          //超音波量到的距離, for error calculation.
unsigned long lastMeasure = 0;


void updateUltrasonic() {
  unsigned long nowMicros = micros();
  unsigned long nowMillis = millis();

  // --- 1. 啟動一個新量測 ---
  if (usState == US_IDLE && (nowMillis - lastMeasure >= PID_INTERVAL)) { 
    digitalWrite(TRIG_PIN, HIGH);
    trigTime = nowMicros;
    usState = US_TRIG_HIGH;
  }

  // --- 2. TRIG HIGH 持續 10us ---
  if (usState == US_TRIG_HIGH) {
    if (nowMicros - trigTime >= 10) {
      digitalWrite(TRIG_PIN, LOW);
      usState = US_WAIT_ECHO_HIGH;
    }
  }

  // --- 3. 等候 Echo 變 HIGH ---
  if (usState == US_WAIT_ECHO_HIGH) {
    if (digitalRead(ECHO_PIN) == HIGH) {
      echoStart = micros();
      usState = US_MEASURE_ECHO;
    }
    else if (micros() - trigTime > 20000) {
      usState = US_IDLE;  // timeout
      lastMeasure = millis();
    }
  }

  // --- 4. 計算 Echo HIGH 時間 ---
  if (usState == US_MEASURE_ECHO) {
    if (digitalRead(ECHO_PIN) == LOW) {
      echoEnd = micros();
      unsigned long duration = echoEnd - echoStart;
      distanceCM = duration * 0.0343 / 2.0;   // 計算距離(cm)
      lastMeasure = millis();
      usState = US_IDLE;
    }
    else if (micros() - echoStart > 30000) {
      usState = US_IDLE;  // timeout
      lastMeasure = millis();
    }
  }
}

// ===== 解析 Serial 指令 =====
void parseSerialInput(String cmd) {
  cmd.trim();

  if (cmd.startsWith("kp=")) {
    Kp = cmd.substring(3).toFloat();
  }
  else if (cmd.startsWith("ki=")) {
    Ki = cmd.substring(3).toFloat();
  }
  else if (cmd.startsWith("kd=")) {
    Kd = cmd.substring(3).toFloat();
  }
  else if (cmd.startsWith("target=")) {
    target = cmd.substring(7).toFloat();
  }
  else if (cmd.startsWith("sw=")) {
    int i = cmd.substring(3).toInt();   // 讀入 0 或 1
    sw = (i != 0);                  
  }
  else if (cmd.startsWith("stop")) {
      doPID = 0;
      sw= 0;
      myservo.write(SvoZero);  // 初始水平
  }
  else if (cmd.startsWith("doPID")) {
    Serial.println("--- PID START ---");
    doPID = 1;
  }
  else {
    Serial.println("Unknown command. Use: kp=, ki=, kd=, target=");
    return;
  }
}


void setup() {
  Serial.begin(115200);

  pinMode(TRIG_PIN, OUTPUT);
  pinMode(ECHO_PIN, INPUT);

  strip.begin();
  strip.clear();
  strip.setBrightness(10); // 設定亮度 (0~255)

  myservo.attach(SERVO_PIN);
  myservo.write(SvoZero);  // 初始水平

  lastTime = millis();

  digitalWrite(TRIG_PIN, LOW);
  Serial.println("Start PID ball balance control...");
}

void loop() {
  
  //讀取序列窗命令
  if (Serial.available()) {
    String cmd = Serial.readStringUntil('\n');
    parseSerialInput(cmd);
  }

  if(!doPID) 
  {
    strip.setPixelColor(0, strip.Color(0, 255, 0));
    strip.show();
    delay(250);
    strip.setPixelColor(0, strip.Color(0, 0, 0));
    strip.show();
    delay(250);
    return;
  }

  //超音波偵測球體位置, by none-blocking method.
  updateUltrasonic();

  //PID程序主體
  if (millis() - lastPID >= PID_INTERVAL) {
    lastPID = millis();

    // 讀取距離
    if (distanceCM <= 0 || distanceCM > 35) return; //Protection for 異常值

    // PID 計算
    unsigned long now = millis();
    float dt = (now - lastTime) / 1000.0;
    if(dt <= 0) dt = 0.001;   // 避免 derivative 爆炸
    lastTime = now;

    error = target - distanceCM;   // 正值→超過目標點, 負值→未達目標點

    // 誤差在 4cm 之內才積分, 可調。
    if (abs(error) < 4.0) { integral += error * dt; } 

    float derivative = (error - lastError) / dt;
    lastError = error;

    float output = Kp * error + Ki * integral + Kd * derivative;

    // 避免 servo 劇烈震動
    output = constrain(output, -30, 30);

    // 轉換成 SG90 角度（~90° 為水平）
    // 限制 SG90 角度 0~180
    float angle = constrain(SvoZero + output, 0, 180);

    myservo.write(angle);

    //調教參考資訊輸出
    Serial.print("Target:"); Serial.print(target);
    Serial.print(" , distanceCM(cm):"); Serial.print(distanceCM);
    Serial.print(" , Error:"); Serial.print(error);
    Serial.print(" , Angle:"); Serial.print(angle);
    Serial.print(" , sw:"); Serial.print(sw);
    Serial.print(" , kp:"); Serial.print(Kp);
    Serial.print(" , ki:"); Serial.print(Ki);
    Serial.print(" , kd:"); Serial.println(Kd); 

    //兩點target調教
    if(sw){
      if(swTarget){ 
        if(TargetCnt++>400){ swTarget=false; target = 5.0;  TargetCnt = 0; }
      }
      else{
        if(TargetCnt++>400){ swTarget=true; target = 10.0;  TargetCnt = 0; }    
      }
    }
  }
}
Table of Contents