109 lines
3.5 KiB
C
109 lines
3.5 KiB
C
#include <stdio.h>
|
||
#include <math.h>
|
||
#include "driver/ledc.h"
|
||
#include "esp_err.h"
|
||
#include "esp_log.h"
|
||
#include "servo.h"
|
||
#include "esp_timer.h"
|
||
#include "modbus.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 FADE_TIME_MS (100)
|
||
|
||
#define SERVO_LOOP_PERIOD_MS (100)
|
||
|
||
/* 范围 */
|
||
#define BDC_SERVO_INPUT_RANGE 1000
|
||
#define BDC_SERVO_REAL_RANGE (45)
|
||
|
||
typedef struct
|
||
{
|
||
uint16_t res;
|
||
uint16_t angle;
|
||
} servo_data_t;
|
||
|
||
servo_data_t *servo_data = (servo_data_t *)&gWordVar[0];
|
||
|
||
|
||
static void servo_loop_cb(void *args)
|
||
{
|
||
float angle = (short)servo_data->angle; /* 注意：uint32_t强转float在有符号情况下会出错，需要先转成short */
|
||
angle = -1.0 * angle / BDC_SERVO_INPUT_RANGE * BDC_SERVO_REAL_RANGE; /* 将输入范围映射到-45->45度上 */
|
||
/* 前轮转向控制 */
|
||
servo_set_angle(angle);
|
||
}
|
||
|
||
|
||
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));
|
||
|
||
/* 设置渐变 */
|
||
ledc_fade_func_install(0);
|
||
|
||
/* 定时设置 */
|
||
ESP_LOGI(TAG, "Create a timer to do Servo set");
|
||
const esp_timer_create_args_t periodic_timer_args = {
|
||
.callback = servo_loop_cb,
|
||
.arg = NULL,
|
||
.name = "servo_loop"
|
||
};
|
||
esp_timer_handle_t servo_loop_timer = NULL;
|
||
ESP_ERROR_CHECK(esp_timer_create(&periodic_timer_args, &servo_loop_timer));
|
||
ESP_ERROR_CHECK(esp_timer_start_periodic(servo_loop_timer, SERVO_LOOP_PERIOD_MS * 1000));
|
||
}
|
||
|
||
/* 设置角度，正负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;
|
||
|
||
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));
|
||
|
||
ledc_set_fade_with_time(LEDC_MODE, LEDC_CHANNEL, (int)duty, FADE_TIME_MS);
|
||
ledc_fade_start(LEDC_MODE, LEDC_CHANNEL, LEDC_FADE_NO_WAIT);
|
||
}
|