5상 마이크로 스탭 모터를 제어하기 위한 PID 제어기의 C 코드 예제

// 5상 마이크로 스텝 모터를 제어할 수 있으며, 각 마이크로 스텝이 5단계로 나뉘어 모터의 정밀 제어가 가능하며, 실제 하드웨어 제어 코드(GPIO 또는 드라이버 제어)가 추가될 수 있도록 구조가 마련되어 있습니다

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

// PID 구조체 정의
typedef struct {
    double kp;        // 비례 이득 (Proportional gain)
    double ki;        // 적분 이득 (Integral gain)
    double kd;        // 미분 이득 (Derivative gain)
    double prev_error; // 이전 오차값
    double integral;   // 적분 합산값
} PIDController;

// PID 컨트롤러 초기화 함수
void PID_Init(PIDController *pid, double kp, double ki, double kd) {
    pid->kp = kp;
    pid->ki = ki;
    pid->kd = kd;
    pid->prev_error = 0.0;
    pid->integral = 0.0;
}

// PID 계산 함수
double PID_Compute(PIDController *pid, double setpoint, double measured_value, double dt) {
    double error = setpoint - measured_value; // 현재 오차 계산
    pid->integral += error * dt;             // 적분값 갱신
    double derivative = (error - pid->prev_error) / dt; // 미분값 계산

    // PID 출력 계산
    double output = (pid->kp * error) + (pid->ki * pid->integral) + (pid->kd * derivative);

    pid->prev_error = error; // 이전 오차 업데이트
    return output;
}

// 5상 마이크로 스텝 모터 제어 함수
void StepMotor_Control(double control_signal) {
    double step_size = 0.002; // 마이크로 스텝 크기 (5상 모터 정밀 제어)
    int steps = (int)round(control_signal / step_size); // 제어 신호에 따라 스텝 수 계산

    if (steps > 0) {
        for (int i = 0; i < steps; i++) {
            printf("Step Motor: Forward micro-step %d/5\n", (i % 5) + 1);
            // 실제 5상 모터 전압 제어 코드 추가 (예: GPIO 제어)
        }
    } else if (steps < 0) {
        for (int i = 0; i < -steps; i++) {
            printf("Step Motor: Backward micro-step %d/5\n", (i % 5) + 1);
            // 실제 5상 모터 전압 제어 코드 추가 (예: GPIO 제어)
        }
    } else {
        printf("Step Motor: No movement\n");
    }
}

// 메인 함수 예제
int main() {
    PIDController pid;
    PID_Init(&pid, 2.0, 0.5, 0.1); // Kp, Ki, Kd 설정

    double setpoint = 100.0;       // 목표 위치
    double measured_value = 0.0;   // 현재 위치 (초기값)
    double dt = 0.1;               // 시간 간격 (초 단위)

    for (int i = 0; i < 100; i++) {
        double control_signal = PID_Compute(&pid, setpoint, measured_value, dt);

        // 5상 마이크로 스텝 모터 제어 신호 적용
        StepMotor_Control(control_signal);

        // 현재 위치 갱신 (시뮬레이션)
        measured_value += control_signal * dt;

        printf("Time: %.1f, Setpoint: %.2f, Measured: %.2f, Control: %.2f\n",
               i * dt, setpoint, measured_value, control_signal);
    }

    return 0;
}