/**********************************************************************
*   Copyright: (C)2024 LingYun IoT System Studio
*      Author: GuoWenxue<guowenxue@gmail.com>
* Description: SG90 Servo Motor Driver for ISKBoard
*
*   ChangeLog:
*        Version    Date       Author            Description
*        V1.0.0  2024.08.29    GuoWenxue      Release initial version
***********************************************************************/

/**
 * SG90 舵机控制(0° ~ 180°) : https://detail.tmall.com/item.htm?id=615779197448
 * - 正极(红色) <-> 5V
 * - 负极(棕色) <-> GND
 * - 信号(黄色) <-> PA1(PWM/TIM2_CH2)
 *
 * 舵机控制原理:
 * - 舵机的控制需要一个 PWM 信号，频率一般为 50Hz (周期 20ms)
 * - 脉宽范围约为 0.5ms ~ 2.5ms。以 180° 角为例，对应的控制关系为：
 *     角度  脉宽
 *     0°    500 µs
 *     45°   700 µs
 *     90°  1500 µs
 *    135°  2000 µs
 *    180°  2500 µs
 * - 角度与脉宽的线性关系公式为：pulse = 500 + angle * 2000 / 180
 *
 * STM32CubeMX 定时器 TIM 参数配置:
 *   预分频(htim.Init.Prescaler) = SYS_CLK-1 -> 定时器时钟 1 MHz (1 µs/tick)
 *   自动重装(htim.Init.Period)  = 20000-1 -> PWM 周期 20 ms (20000 µs)
 *   htim.Init.AutoReloadPreload = Enable
 *   脉宽范围(sConfigOC.Pulse)   = 1500 -> 默认 90° (1500 µs)
 */

#include "stm32l4xx_hal.h"
#include "miscdev.h"
#include "servo_sg90.h"
#include "tim.h"

/* SG90 舵机控制连接单片机引脚: PA1/TIM2_CH2 */
#define SERVO_TIMER       &htim2
#define SERVO_CHANNEL     TIM_CHANNEL_2

/**
 * @brief  初始化舵机, 启动 TIM PWM 输出。
 */
void Servo_Init(void)
{
    /* 启动前先设定一个明确的初始角度，避免上电盲目抖动 */
    Servo_SetAngle(SERVO_DEFAULT_ANG);

    HAL_TIM_PWM_Start(SERVO_TIMER, SERVO_CHANNEL);
}

/**
 * @brief  设置舵机角度
 * @param  angle  可传 0 ~ 180，但超出钳位值范围自动钳位
 */
void Servo_SetAngle(int angle)
{
    int pulse;

    angle = CLAMP_ANGLE(angle);

    /* 脉宽线性公式：利用 32 位 int 计算，彻底免除溢出隐患 */
    pulse = SERVO_MIN_PULSE + (angle * (SERVO_MAX_PULSE - SERVO_MIN_PULSE)) / SERVO_MAX_ANGLE;

    __HAL_TIM_SET_COMPARE(SERVO_TIMER, SERVO_CHANNEL, (uint16_t)pulse);
}

/**
 * @brief  直接停止 PWM 输出, 立即停止舵机。
 */
void Servo_Stop(void)
{
    HAL_TIM_PWM_Stop(SERVO_TIMER, SERVO_CHANNEL);
}

/**
 * @brief  转到安全角度后，再停止舵机
 * @param  safe_angle  停止前舵机转到的角度 (超出钳位值范围设置默认角度)
 */
void Servo_SafeStop(int safe_angle)
{
    /* 超出钳位值范围设置默认角度 */
    if (safe_angle <= SERVO_MIN_ANGLE || safe_angle > SERVO_MAX_ANGLE) {
        safe_angle = SERVO_DEFAULT_ANG;
    }

    Servo_SetAngle(safe_angle);

    /* SG90 空载约 100 ms/60°，转 180° 约需 300ms */
    mdelay(1000);

    Servo_Stop();
}

/**
 * @brief 舵机测试函数
 */
void Servo_Test(void)
{
    printf("Servo Start\r\n");
    Servo_Init();
    mdelay(2000);

    Servo_SetAngle(0);
    mdelay(2000);

    Servo_SetAngle(90);
    mdelay(2000);

    Servo_SetAngle(180);
    mdelay(2000);

    Servo_SetAngle(90);
    mdelay(2000);

    Servo_SetAngle(0);
    mdelay(2000);

    printf("Servo Stop\r\n");
    Servo_SafeStop(SERVO_DEFAULT_ANG);
}