/**********************************************************************
|
* 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);
|
}
|
