서보 모터 제어 — PWM 1ms~2ms·Closed Loop·PID

1
Period: 20 ms (50 Hz)
2

3
Pulse width → angle:
4
  0.5 ms (2.5%)  → -90° (또는 0°)
5
  1.0 ms (5%)    → -45°
6
  1.5 ms (7.5%)  →   0° (center)
7
  2.0 ms (10%)   →  +45°
8
  2.5 ms (12.5%) → +90° (또는 180°)

1
TIM3 CH1-4: servo 0-3 (50 Hz, 4 CCR로 duty)
2
TIM4 CH1-4: servo 4-7
3
TIM2 CH1-4: servo 8-11

1
void servo_init(void) {
2
    RCC->APB1ENR |= RCC_APB1ENR_TIM3EN;
3
    RCC->AHB1ENR |= RCC_AHB1ENR_GPIOAEN;
4

5
    // PA6 = TIM3_CH1
6
    gpio_init(GPIOA, 6, &(gpio_config_t){.mode=GPIO_MODE_AF, .af=2});
7

8
    // 50 Hz, resolution 1 µs
9
    // TIM3 clock = 84 MHz; PSC = 84-1 → 1 MHz tick (1 µs)
10
    // ARR = 20000-1 → 20 ms period (50 Hz)
11
    TIM3->PSC = 84 - 1;
12
    TIM3->ARR = 20000 - 1;
13
    TIM3->CCMR1 = (6u << 4) | TIM_CCMR1_OC1PE;   // PWM mode 1
14
    TIM3->CCER  = TIM_CCER_CC1E;
15
    TIM3->CCR1  = 1500;                           // center (1.5 ms)
16
    TIM3->CR1   = TIM_CR1_ARPE | TIM_CR1_CEN;
17
    TIM3->EGR   = TIM_EGR_UG;
18
}
19

20
void servo_set_us(uint16_t pulse_us) {
21
    if (pulse_us < 500)  pulse_us = 500;          // clamp
22
    if (pulse_us > 2500) pulse_us = 2500;
23
    TIM3->CCR1 = pulse_us;
24
}
25

26
void servo_set_angle(int16_t deg) {
27
    // -90~+90 → 500~2500 µs
28
    if (deg < -90) deg = -90;
29
    if (deg >  90) deg =  90;
30
    uint16_t pulse = 1500 + (deg * 1000) / 90;
31
    servo_set_us(pulse);
32
}

1
void servo4_init(void) {
2
    RCC->APB1ENR |= RCC_APB1ENR_TIM3EN;
3

4
    // PA6/7/B0/B1 = TIM3 CH1-4 (AF2)
5
    gpio_init(GPIOA, 6, &(gpio_config_t){.mode=GPIO_MODE_AF, .af=2});
6
    gpio_init(GPIOA, 7, &(gpio_config_t){.mode=GPIO_MODE_AF, .af=2});
7
    gpio_init(GPIOB, 0, &(gpio_config_t){.mode=GPIO_MODE_AF, .af=2});
8
    gpio_init(GPIOB, 1, &(gpio_config_t){.mode=GPIO_MODE_AF, .af=2});
9

10
    TIM3->PSC = 84 - 1;
11
    TIM3->ARR = 20000 - 1;
12

13
    TIM3->CCMR1 = (6u << 4)  | TIM_CCMR1_OC1PE
14
                | (6u << 12) | TIM_CCMR1_OC2PE;
15
    TIM3->CCMR2 = (6u << 4)  | TIM_CCMR2_OC3PE
16
                | (6u << 12) | TIM_CCMR2_OC4PE;
17
    TIM3->CCER  = TIM_CCER_CC1E | TIM_CCER_CC2E
18
                | TIM_CCER_CC3E | TIM_CCER_CC4E;
19

20
    TIM3->CCR1 = TIM3->CCR2 = TIM3->CCR3 = TIM3->CCR4 = 1500;
21
    TIM3->CR1 = TIM_CR1_ARPE | TIM_CR1_CEN;
22
    TIM3->EGR = TIM_EGR_UG;
23
}
24

25
void servo4_set(uint8_t ch, uint16_t pulse_us) {
26
    if (pulse_us < 500) pulse_us = 500;
27
    if (pulse_us > 2500) pulse_us = 2500;
28
    switch (ch) {
29
        case 0: TIM3->CCR1 = pulse_us; break;
30
        case 1: TIM3->CCR2 = pulse_us; break;
31
        case 2: TIM3->CCR3 = pulse_us; break;
32
        case 3: TIM3->CCR4 = pulse_us; break;
33
    }
34
}

1
typedef struct {
2
    int16_t current_us;
3
    int16_t target_us;
4
    int16_t step;        // per tick
5
} servo_smooth_t;
6

7
static servo_smooth_t s = {1500, 1500, 5};
8

9
void servo_smooth_set(int16_t target) { s.target_us = target; }
10

11
// 10 ms 주기 호출
12
void servo_smooth_tick(void) {
13
    if (s.current_us < s.target_us) {
14
        s.current_us += s.step;
15
        if (s.current_us > s.target_us) s.current_us = s.target_us;
16
    } else if (s.current_us > s.target_us) {
17
        s.current_us -= s.step;
18
        if (s.current_us < s.target_us) s.current_us = s.target_us;
19
    }
20
    servo_set_us(s.current_us);
21
}

1
typedef struct {
2
    uint16_t min_us;     // 0° or -90°
3
    uint16_t center_us;  // 0° (center)
4
    uint16_t max_us;     // 180° or +90°
5
} servo_cal_t;
6

7
static servo_cal_t cal = {550, 1480, 2400};  // 실측값
8

9
void servo_set_angle_cal(int16_t deg) {
10
    uint16_t pulse;
11
    if (deg < 0)
12
        pulse = cal.center_us + (deg * (cal.center_us - cal.min_us)) / 90;
13
    else
14
        pulse = cal.center_us + (deg * (cal.max_us - cal.center_us)) / 90;
15
    servo_set_us(pulse);
16
}