제8부 실시간 시스템 응용

8.4 실전 — PID 모터 제어 루프

8.1~8.3에서 다진 개념(결정론적 타이밍, 인터럽트·타이머, 통신 지연)을 실제로 동작하는 코드로 묶는 장이다. 엔코더가 장착된 DC 모터를 STM32U585(MCU) 위에서 실행되는 고정 주기 PID 루프로 속도 제어한다.

검증 필요: 이 장의 코드는 8.2에서 확인한 Arduino Core API(attachInterrupt, analogWrite, millis/micros)를 기반으로 작성한 참조 구현이며, 실제 UNO Q 보드·모터 드라이버 조합에서 실물 검증을 거치지 않았다. 핀 번호·PWM 극성·타이머 세부 API는 실제 하드웨어에 맞춰 조정이 필요하다.

8.4.1 하드웨어 구성

이 예제가 가정하는 최소 구성:

• DC 모터 + 쿼드러처 인코더(A상·B상 두 채널) — 위치가 아니라 속도 제어를 목표로 한다.
• H-브리지 모터 드라이버(예: L298P, TB6612FNG류) — 방향 핀 2개 + PWM 핀 1개, 또는 PWM 2채널(정/역 각각) 구성.
• UNO Q의 Arduino 헤더(2.2, 4.3)에서 인터럽트 지원 디지털 핀 2개(엔코더용)와 PWM 출력 핀(모터 드라이버용)을 사용한다.

핀 배정(예시 — 실제 배선에 맞춰 바꾼다):

8.4.2 제어 루프 설계

8.4.2.1 주기 선택

DC 모터 속도 제어는 보통 수백 Hz~수 kHz 사이에서 안정적으로 동작한다. 이 예제는 1kHz(1ms 주기)를 목표로 한다 — 8.2.3.1의 소프트 스케줄(micros() 기반)로 구현하되, loop() 안에 Bridge 호출이나 부하가 큰 로직을 섞지 않아 지터를 최소화한다.

8.4.2.2 엔코더 → 속도 환산

엔코더는 인터럽트로 펄스 카운트만 누적한다(8.2.2). 속도는 제어 주기마다 "이번 주기 동안 늘어난 카운트"를 시간으로 나눠 구한다.

$$ \omega_k = \frac{(N_k - N_{k-1}) \cdot 2\pi}{\text{PPR} \cdot T} $$

여기서 $N_k$는 이번 스텝의 누적 엔코더 카운트, $\text{PPR}$은 회전당 펄스 수(모터·인코더 사양에서 확인), $T$는 제어 주기(초)다.

8.4.2.3 PID 제어식

목표 각속도 $\omega_{ref}$와 측정 각속도 $\omega_k$의 오차 $e_k = \omega_{ref} - \omega_k$에 대해:

$$ u_k = K_p e_k + K_i \sum_{j=0}^{k} e_j T + K_d \frac{e_k - e_{k-1}}{T} $$

$u_k$를 PWM 듀티(보통 $-255 \sim 255$ 또는 $-100\% \sim 100\%$)로 매핑하고, 부호로 회전 방향을 정한다.

8.4.3 전체 스케치

// ===== 핀 정의 =====
const uint8_t ENCODER_A_PIN = 2;
const uint8_t ENCODER_B_PIN = 3;
const uint8_t MOTOR_PWM_PIN = 9;
const uint8_t MOTOR_IN1_PIN = 7;
const uint8_t MOTOR_IN2_PIN = 8;

// ===== 엔코더 사양 (실제 모터에 맞게 조정) =====
const long ENCODER_PPR = 360;  // 회전당 펄스 수 — 확인 필요(모터 데이터시트)

// ===== 제어 주기 =====
const unsigned long CONTROL_PERIOD_US = 1000;  // 1kHz

// ===== PID 게인 (초기값 — 실물에서 튜닝 필요) =====
float Kp = 2.0f;
float Ki = 8.0f;
float Kd = 0.02f;

// ===== 상태 변수 =====
volatile long encoderCount = 0;
long lastEncoderCount = 0;

float targetSpeedRadS = 0.0f;   // Bridge로 MPU에서 갱신되는 목표 속도(8.4.5 참고)
float integral = 0.0f;
float lastError = 0.0f;

unsigned long lastControlTime = 0;

void onEncoderA() {
  if (digitalRead(ENCODER_B_PIN) == HIGH) {
    encoderCount++;
  } else {
    encoderCount--;
  }
}

void setup() {
  pinMode(ENCODER_A_PIN, INPUT_PULLUP);
  pinMode(ENCODER_B_PIN, INPUT_PULLUP);
  pinMode(MOTOR_PWM_PIN, OUTPUT);
  pinMode(MOTOR_IN1_PIN, OUTPUT);
  pinMode(MOTOR_IN2_PIN, OUTPUT);

  attachInterrupt(digitalPinToInterrupt(ENCODER_A_PIN), onEncoderA, RISING);

  Serial.begin(115200);
  lastControlTime = micros();
}

void setMotorOutput(float u) {
  // u: -255.0 ~ 255.0 범위로 클램프된 PWM 명령
  if (u >= 0) {
    digitalWrite(MOTOR_IN1_PIN, HIGH);
    digitalWrite(MOTOR_IN2_PIN, LOW);
  } else {
    digitalWrite(MOTOR_IN1_PIN, LOW);
    digitalWrite(MOTOR_IN2_PIN, HIGH);
    u = -u;
  }
  analogWrite(MOTOR_PWM_PIN, (int)u);
}

void controlStep(float dt) {
  // 1. 인터럽트로 누적된 카운트를 원자적으로 읽는다
  noInterrupts();
  long currentCount = encoderCount;
  interrupts();

  long deltaCount = currentCount - lastEncoderCount;
  lastEncoderCount = currentCount;

  // 2. 속도 환산 (8.4.2.2)
  float measuredSpeed = (deltaCount * 2.0f * PI) / (ENCODER_PPR * dt);

  // 3. PID 연산 (8.4.2.3)
  float error = targetSpeedRadS - measuredSpeed;

  integral += error * dt;
  // 적분 와인드업 방지 — 출력 한계에 맞춰 적분항도 클램프한다
  const float INTEGRAL_LIMIT = 255.0f / max(Ki, 0.001f);
  if (integral > INTEGRAL_LIMIT) integral = INTEGRAL_LIMIT;
  if (integral < -INTEGRAL_LIMIT) integral = -INTEGRAL_LIMIT;

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

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

  // 4. PWM 출력 범위로 클램프
  if (u > 255.0f) u = 255.0f;
  if (u < -255.0f) u = -255.0f;

  setMotorOutput(u);
}

void loop() {
  unsigned long now = micros();
  if (now - lastControlTime >= CONTROL_PERIOD_US) {
    float dt = (now - lastControlTime) / 1000000.0f;
    lastControlTime += CONTROL_PERIOD_US;
    controlStep(dt);
  }

  // Bridge 폴링 등 비-hard-real-time 작업은 여기서 하되,
  // controlStep() 호출이 지연되지 않도록 실행 시간을 짧게 유지한다.
  // pollBridgeForTargetSpeed();  // 8.4.5, 8.3.5 참고 — 확인 필요
}

8.4.4 튜닝 팁

• 비례항(Kp)부터 높인다. 적분·미분을 0으로 두고 Kp만 높여 가며 진동이 시작되는 지점을 찾은 뒤, 그보다 낮은 값에서 시작한다(Ziegler-Nichols류 절차를 참고할 수 있다 — 제어이론 일반서 참고, 이 책 범위 밖).
• 적분 와인드업에 주의한다. 목표가 갑자기 크게 바뀌거나 모터가 물리적으로 막혀 있으면(스톨) 적분항이 무한정 커질 수 있다 — 위 코드처럼 출력 한계에 맞춰 적분항 자체를 클램프한다.
• 미분항은 노이즈에 민감하다. 엔코더 카운트 기반 속도 추정에는 양자화 노이즈가 섞이므로, 미분항에 저역통과 필터를 얹거나 Kd를 작게 유지하는 경우가 많다.
• PWM 데드존을 확인한다. 낮은 듀티에서는 모터가 전혀 회전하지 않는 구간(정지 마찰)이 있다 — 필요하면 최소 구동 듀티를 보정한다.

8.4.5 MPU와의 연결 — 목표 속도는 어떻게 들어오는가

targetSpeedRadS는 이 스케치 안에서 상수로 둘 수도 있지만, 실전에서는 MPU(App Lab, Python)가 Bridge(3.4·8.3)로 목표 속도를 갱신하는 구성이 자연스럽다. 이때 8.3.5의 원칙을 그대로 적용한다.

• Bridge로 받은 목표값은 로컬 변수(targetSpeedRadS)에 반영만 하고, controlStep()은 그 변수를 읽을 뿐 Bridge 응답을 기다리지 않는다.
• Bridge 폴링·RPC 콜백은 loop()에서 controlStep()을 방해하지 않을 만큼 부하가 작아야 한다 — 부하가 큰 처리가 필요하면 플래그만 설정하고 다음 사이클로 미룬다(8.2.2.1의 ISR 원칙과 같은 사고방식).
• 목표값이 오랫동안 갱신되지 않을 때(연결 끊김) 마지막 값을 유지할지, 안전하게 정지(targetSpeedRadS = 0)할지는 애플리케이션이 결정해야 한다 — 이 판단은 8.5.7의 워치독·페일세이프 설계와 같은 패턴이다.

8.4.6 엣지케이스

• 엔코더 튐(글리치): 배선이 길거나 노이즈가 있으면 인터럽트가 잘못 발생해 카운트 오차가 생길 수 있다. A상·B상 두 채널을 모두 인터럽트로 받아 정식 쿼드러처 디코딩(양쪽 에지를 모두 카운트)을 하면 노이즈 내성과 해상도가 개선되지만 인터럽트 빈도가 늘어난다 — 8.2.5의 인터럽트 폭주 주의사항과 상충하지 않게 균형을 맞춘다.
• noInterrupts()/interrupts() 구간을 최소화한다. controlStep()에서 인코더 카운트를 읽는 순간만 짧게 인터럽트를 막는다 — 오래 막으면 그 사이 들어온 엔코더 펄스를 놓친다.
• 모터가 정지된 상태에서 시작(콜드 스타트)할 때 미분항이 큰 스파이크를 낼 수 있다 — 첫 스텝에서는 lastError를 현재 오차로 초기화하는 등의 보정을 고려한다.
• PWM 주파수와 모터 드라이버 호환성: analogWrite()의 기본 PWM 주파수가 모터 드라이버·모터의 가청 노이즈나 발열에 영향을 줄 수 있다. 필요하면 하드웨어 타이머로 PWM 주파수를 직접 조정한다(8.2.3.2) — 구체적 API는 확인 필요.

8.4.7 다음 장으로

이 장에서 만든 "MCU 로컬 폐루프 제어" 패턴은 8.5에서 그대로 확장된다 — 대상이 DC 모터가 아니라 별도의 비행 컨트롤러(PX4/ArduPilot)로 바뀌고, PWM 대신 MAVLink setpoint 메시지가 출력이 되는 구성이다.