wheel/components/servo/servo.c

#include <stdio.h>
#include <math.h>
#include "driver/ledc.h"
#include "esp_err.h"
#include "esp_log.h"
#include "servo.h"

static const char *TAG = "servo";

#define LEDC_TIMER              LEDC_TIMER_0
#define LEDC_MODE               LEDC_LOW_SPEED_MODE
#define LEDC_OUTPUT_IO          (12) // Define the output GPIO
#define LEDC_CHANNEL            LEDC_CHANNEL_0
#define LEDC_DUTY_RES           LEDC_TIMER_13_BIT // Set duty resolution to 13 bits
#define LEDC_DUTY               (4095) // Set duty to 50%. ((2 ** 13) - 1) * 50% = 4095
#define LEDC_FREQUENCY          (50) // Frequency in Hertz. Set frequency at 5 kHz

#define ANGLE_SET_DIV   (10.0)

void servo_ledc_init(void)
{
    // Prepare and then apply the LEDC PWM timer configuration
    ledc_timer_config_t ledc_timer = {
        .speed_mode       = LEDC_MODE,
        .timer_num        = LEDC_TIMER,
        .duty_resolution  = LEDC_DUTY_RES,
        .freq_hz          = LEDC_FREQUENCY,  // Set output frequency at 5 kHz
        .clk_cfg          = LEDC_AUTO_CLK
    };
    ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer));

    // Prepare and then apply the LEDC PWM channel configuration
    ledc_channel_config_t ledc_channel = {
        .speed_mode     = LEDC_MODE,
        .channel        = LEDC_CHANNEL,
        .timer_sel      = LEDC_TIMER,
        .intr_type      = LEDC_INTR_DISABLE,
        .gpio_num       = LEDC_OUTPUT_IO,
        .duty           = 0, // Set duty to 0%
        .hpoint         = 0
    };
    ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel));
}

/* 设置角度，正负90度，左负右正 */
void servo_set_angle(float angle)
{
    static double duty_pre_ms = pow(2, 13) / 20.0;
    static double duty_pre_angle =((2.5-0.5) / 180.0) * pow(2, 13) / 20.0;   
    static double duty_zero_offset = 0.5 * pow(2, 13) / 20.0;

    static float angle_real = 0;    /* 舵机逐渐变化，每次只加固定度数，逼近设置度数 */

    angle_real += (angle - angle_real) / ANGLE_SET_DIV;
    if (angle - angle_real < 5 || angle - angle_real > -5)  angle_real = angle;
    angle = angle_real;

    if (angle > 45 || angle < -45)
    {
        ESP_LOGE("PWM", "Angle out of range");
        return;
    }

    angle += 90; // 将角度转换为0-180度
    double duty = angle * duty_pre_angle + duty_zero_offset; 

    ESP_LOGI("PWM", "angle: %f, duty: %f", angle, duty);
    
    ESP_ERROR_CHECK(ledc_set_duty(LEDC_MODE, LEDC_CHANNEL, (int)duty));
    ESP_ERROR_CHECK(ledc_update_duty(LEDC_MODE, LEDC_CHANNEL));
}