NUCLEO-F103RB

#include "stm32f1xx_hal.h"
#include <stdio.h>

UART_HandleTypeDef huart2;
TIM_HandleTypeDef htim1;

// PID 제어 변수
double setpoint = 90.0, input = 90.0, output;
double Kp = 2.0, Ki = 0.5, Kd = 0.1;
double previous_error = 0, integral = 0;

void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_USART2_UART_Init(void);
static void MX_TIM1_Init(void);
void writeServo(int angle);

int main(void) {
    HAL_Init();
    SystemClock_Config();
    MX_GPIO_Init();
    MX_USART2_UART_Init();
    MX_TIM1_Init();
   
    HAL_TIM_PWM_Start(&htim1, TIM_CHANNEL_1);
   
    char msg[50];
    while (1) {
        // PID 제어 연산
        double error = setpoint - input;
        integral += error;
        double derivative = error - previous_error;
        output = (Kp * error) + (Ki * integral) + (Kd * derivative);
        previous_error = error;
       
        double newPosition = input + output;
        if (newPosition < 0) newPosition = 0;
        if (newPosition > 180) newPosition = 180;
        writeServo((int)newPosition);
        input = newPosition; // 현재 각도 갱신
       
        sprintf(msg, "Target: %.1f | Current: %.1f | Output: %.1f\r\n", setpoint, input, output);
        HAL_UART_Transmit(&huart2, (uint8_t*)msg, strlen(msg), HAL_MAX_DELAY);
       
        HAL_Delay(20);
    }
}

void writeServo(int angle) {
    uint32_t pulseWidth = (angle * 2000 / 180) + 500; // 500~2500us
    __HAL_TIM_SET_COMPARE(&htim1, TIM_CHANNEL_1, pulseWidth);
}

// HAL 설정 함수들 (클럭, GPIO, USART, TIM 초기화)