140 lines
3.6 KiB
C
140 lines
3.6 KiB
C
#include <driver/ledc.h>
|
|
#include <stdint.h>
|
|
|
|
#include "config.h"
|
|
|
|
#include "servos.h"
|
|
#include "logger.h"
|
|
|
|
#define SERVO_LEDC_TIMER LEDC_TIMER_0
|
|
#define SERVO_LEDC_MODE LEDC_LOW_SPEED_MODE
|
|
#define SERVO_LEDC_CHANNEL LEDC_CHANNEL_0
|
|
#define SERVO_LEDC_DUTY_RES LEDC_TIMER_13_BIT // Set duty resolution to 13 bits
|
|
// #define SERVO_DUTY_MIN ( 205) // 2**13 * ( 500/20000) // 500us of the 20ms
|
|
#define SERVO_DUTY_MIN ( 410) // 2**13 * (1000/20000) // 1000us of the 20ms
|
|
#define SERVO_DUTY_MAX ( 819) // 2**13 * (2000/20000) // 2000us of the 20ms
|
|
// #define SERVO_DUTY_MAX (1024) // 2**13 * (2500/20000) // 2500us of the 20ms
|
|
#define SERVO_DUTY_MID ( 614) // 2**13 * (1500/20000) // 1500us of the 20ms
|
|
|
|
#define SERVO_DUTY_DIFF (SERVO_DUTY_MAX - SERVO_DUTY_MIN)
|
|
|
|
servo_config_t Servos[2];
|
|
bool Servos_ch_swap = false;
|
|
|
|
void servo_init(void)
|
|
{
|
|
Servos[0].pin = SERVOS_CH0_PIN;
|
|
Servos[0].ledc_ch = LEDC_CHANNEL_0;
|
|
Servos[0].pulse_min = SERVO_DUTY_MIN;
|
|
Servos[0].pulse_mid = SERVO_DUTY_MID;
|
|
Servos[0].pulse_max = SERVO_DUTY_MAX;
|
|
Servos[0].mid_delay = 0;
|
|
Servos[0].mid_delay_timer = 0;
|
|
Servos[0].reversed = false;
|
|
|
|
Servos[1].pin = SERVOS_CH1_PIN;
|
|
Servos[1].ledc_ch = LEDC_CHANNEL_1;
|
|
Servos[1].pulse_min = SERVO_DUTY_MIN;
|
|
Servos[1].pulse_mid = SERVO_DUTY_MID;
|
|
Servos[1].pulse_max = SERVO_DUTY_MAX;
|
|
Servos[1].mid_delay = 5;
|
|
Servos[1].mid_delay_timer = 0;
|
|
Servos[1].reversed = true;
|
|
|
|
// Prepare and then apply the LEDC PWM timer configuration
|
|
ledc_timer_config_t ledc_timer = {
|
|
.speed_mode = SERVO_LEDC_MODE,
|
|
.timer_num = SERVO_LEDC_TIMER,
|
|
.duty_resolution = SERVO_LEDC_DUTY_RES,
|
|
.freq_hz = 50, // 20ms period
|
|
.clk_cfg = LEDC_AUTO_CLK
|
|
};
|
|
ledc_timer_config(&ledc_timer);
|
|
|
|
for (int i = sizeof(Servos)/sizeof(servo_config_t)-1; i >= 0; i--)
|
|
{
|
|
// Prepare and then apply the LEDC PWM channel configuration
|
|
ledc_channel_config_t ledc_channel = {
|
|
.speed_mode = SERVO_LEDC_MODE,
|
|
.channel = Servos[i].ledc_ch,
|
|
.timer_sel = SERVO_LEDC_TIMER,
|
|
.intr_type = LEDC_INTR_DISABLE,
|
|
.gpio_num = Servos[i].pin,
|
|
.duty = Servos[i].pulse_mid,
|
|
.hpoint = 0
|
|
};
|
|
ledc_channel_config(&ledc_channel);
|
|
LOG_D("servo_init: inited servo channel %d (ledc: %u, gpio: %d)", i, Servos[i].ledc_ch, Servos[i].pin);
|
|
}
|
|
}
|
|
void servo_deinit(void)
|
|
{
|
|
ledc_timer_pause(SERVO_LEDC_MODE, SERVO_LEDC_TIMER);
|
|
for (int i = sizeof(Servos)/sizeof(servo_config_t)-1; i >= 0; i--)
|
|
{
|
|
ledc_stop(SERVO_LEDC_MODE, Servos[i].ledc_ch, 0);
|
|
}
|
|
}
|
|
|
|
void servo_set(uint8_t ch, uint8_t pos)
|
|
{
|
|
if (Servos_ch_swap)
|
|
{
|
|
ch = (ch == 0) ? 1 : 0;
|
|
}
|
|
|
|
double pos_double = ((int16_t)pos - 128) / 128.0;
|
|
|
|
if (Servos[ch].reversed)
|
|
{
|
|
pos_double = -pos_double;
|
|
}
|
|
|
|
|
|
if (pos_double < 0.0)
|
|
{
|
|
if (Servos[ch].mid_delay_timer > 0)
|
|
{
|
|
Servos[ch].mid_delay_timer--;
|
|
pos_double = 0.0;
|
|
}
|
|
else
|
|
{
|
|
Servos[ch].mid_delay_timer = -Servos[ch].mid_delay;
|
|
}
|
|
}
|
|
else if (pos_double > 0.0)
|
|
{
|
|
if (Servos[ch].mid_delay_timer < 0)
|
|
{
|
|
Servos[ch].mid_delay_timer++;
|
|
pos_double = 0.0;
|
|
}
|
|
else
|
|
{
|
|
Servos[ch].mid_delay_timer = Servos[ch].mid_delay;
|
|
}
|
|
}
|
|
|
|
uint32_t duty;
|
|
if (pos_double > 0.0)
|
|
{
|
|
duty = (pos_double * (Servos[ch].pulse_max - Servos[ch].pulse_mid)) + Servos[ch].pulse_mid;
|
|
}
|
|
else
|
|
{
|
|
duty = (pos_double * (Servos[ch].pulse_mid - Servos[ch].pulse_min)) + Servos[ch].pulse_mid;
|
|
}
|
|
|
|
if (duty < SERVO_DUTY_MIN)
|
|
{
|
|
duty = SERVO_DUTY_MIN;
|
|
}
|
|
else if (duty > SERVO_DUTY_MAX)
|
|
{
|
|
duty = SERVO_DUTY_MAX;
|
|
}
|
|
ledc_set_duty(SERVO_LEDC_MODE, Servos[ch].ledc_ch, duty);
|
|
ledc_update_duty(SERVO_LEDC_MODE, Servos[ch].ledc_ch);
|
|
}
|