#include "servo.h"
#include "tim.h"
#include "pwm.h"

void SERVO_Init_All(void) {
    HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_1);
    // HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_2);
    // HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_3);
    // HAL_TIM_PWM_Start(&htim8, TIM_CHANNEL_4);

}

void Servo_270_Set_Angle(float angle) {
    // 1. 角度限幅
    if (angle < 0.0f) angle = 0.0f;
    if (angle > 270.0f) angle = 270.0f;

    /* 2. 角度映射到脉宽
     * 标准舵机协议：
     * 0°   -> 0.5ms (500us)
     * 360° -> 2.5ms (2500us)
     * 计算公式：Pulse = 500 + (angle / 360.0) * (2500 - 500)
     */
    uint32_t pulse = (uint32_t)(500.0f + (angle * 2000.0f / 360.0f));

    // 3. 设置 PWM
    PWM_Set_Duty(&htim8,TIM_CHANNEL_1,pulse);
}
