140 lines
3.5 KiB
C
140 lines
3.5 KiB
C
#include "motors.h"
|
|
|
|
#include <stdlib.h>
|
|
|
|
#include "driver/ledc.h"
|
|
#include "esp_err.h"
|
|
#include "esp_log.h"
|
|
|
|
#define MOTOR_PWM_MODE LEDC_LOW_SPEED_MODE
|
|
#define MOTOR_PWM_TIMER LEDC_TIMER_0
|
|
#define MOTOR_PWM_FREQ_HZ 20000
|
|
#define MOTOR_PWM_RESOLUTION LEDC_TIMER_10_BIT
|
|
#define MOTOR_PWM_MAX_DUTY ((1 << 10) - 1)
|
|
#define MOTOR_MIN_VALUE 60
|
|
|
|
typedef struct
|
|
{
|
|
int fwd_pin;
|
|
int bak_pin;
|
|
ledc_channel_t fwd_channel;
|
|
ledc_channel_t bak_channel;
|
|
} motor_config_t;
|
|
|
|
static const motor_config_t s_motor_configs[] = {
|
|
[MOTOR1] = {
|
|
.fwd_pin = MOTOR1_FWD_PIN,
|
|
.bak_pin = MOTOR1_BAK_PIN,
|
|
.fwd_channel = LEDC_CHANNEL_0,
|
|
.bak_channel = LEDC_CHANNEL_1,
|
|
},
|
|
[MOTOR2] = {
|
|
.fwd_pin = MOTOR2_FWD_PIN,
|
|
.bak_pin = MOTOR2_BAK_PIN,
|
|
.fwd_channel = LEDC_CHANNEL_2,
|
|
.bak_channel = LEDC_CHANNEL_3,
|
|
},
|
|
};
|
|
|
|
static uint32_t percentile_to_duty(percentile_t percentile)
|
|
{
|
|
int clamped = percentile;
|
|
const uint32_t min_duty = (MOTOR_PWM_MAX_DUTY * MOTOR_MIN_VALUE) / 100;
|
|
if (clamped > 100)
|
|
{
|
|
clamped = 100;
|
|
}
|
|
else if (clamped < -100)
|
|
{
|
|
clamped = -100;
|
|
}
|
|
|
|
if (clamped == 0)
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
return min_duty + (((uint32_t)abs(clamped) * (MOTOR_PWM_MAX_DUTY - min_duty)) / 100);
|
|
}
|
|
|
|
static void set_channel_duty(ledc_channel_t channel, uint32_t duty)
|
|
{
|
|
ESP_ERROR_CHECK(ledc_set_duty(MOTOR_PWM_MODE, channel, duty));
|
|
ESP_ERROR_CHECK(ledc_update_duty(MOTOR_PWM_MODE, channel));
|
|
}
|
|
|
|
esp_err_t init_motors(void)
|
|
{
|
|
const ledc_timer_config_t timer_config = {
|
|
.speed_mode = MOTOR_PWM_MODE,
|
|
.timer_num = MOTOR_PWM_TIMER,
|
|
.duty_resolution = MOTOR_PWM_RESOLUTION,
|
|
.freq_hz = MOTOR_PWM_FREQ_HZ,
|
|
.clk_cfg = LEDC_AUTO_CLK,
|
|
};
|
|
|
|
ESP_ERROR_CHECK(ledc_timer_config(&timer_config));
|
|
|
|
for (size_t i = 0; i < (sizeof(s_motor_configs) / sizeof(s_motor_configs[0])); ++i)
|
|
{
|
|
const motor_config_t *motor = &s_motor_configs[i];
|
|
const ledc_channel_config_t fwd_channel_config = {
|
|
.gpio_num = motor->fwd_pin,
|
|
.speed_mode = MOTOR_PWM_MODE,
|
|
.channel = motor->fwd_channel,
|
|
.intr_type = LEDC_INTR_DISABLE,
|
|
.timer_sel = MOTOR_PWM_TIMER,
|
|
.duty = 0,
|
|
.hpoint = 0,
|
|
};
|
|
const ledc_channel_config_t bak_channel_config = {
|
|
.gpio_num = motor->bak_pin,
|
|
.speed_mode = MOTOR_PWM_MODE,
|
|
.channel = motor->bak_channel,
|
|
.intr_type = LEDC_INTR_DISABLE,
|
|
.timer_sel = MOTOR_PWM_TIMER,
|
|
.duty = 0,
|
|
.hpoint = 0,
|
|
};
|
|
|
|
ESP_ERROR_CHECK(ledc_channel_config(&fwd_channel_config));
|
|
ESP_ERROR_CHECK(ledc_channel_config(&bak_channel_config));
|
|
}
|
|
|
|
set_motors(0, 0);
|
|
return ESP_OK;
|
|
}
|
|
|
|
void set_motor(motor_t motor, percentile_t percentile)
|
|
{
|
|
if (motor < MOTOR1 || motor > MOTOR2)
|
|
{
|
|
return;
|
|
}
|
|
|
|
const motor_config_t *config = &s_motor_configs[motor];
|
|
const uint32_t duty = percentile_to_duty(percentile);
|
|
|
|
if (percentile > 0)
|
|
{
|
|
set_channel_duty(config->bak_channel, 0);
|
|
set_channel_duty(config->fwd_channel, duty);
|
|
}
|
|
else if (percentile < 0)
|
|
{
|
|
set_channel_duty(config->fwd_channel, 0);
|
|
set_channel_duty(config->bak_channel, duty);
|
|
}
|
|
else
|
|
{
|
|
set_channel_duty(config->fwd_channel, 0);
|
|
set_channel_duty(config->bak_channel, 0);
|
|
}
|
|
}
|
|
|
|
void set_motors(percentile_t left, percentile_t right)
|
|
{
|
|
set_motor(MOTOR1, left);
|
|
set_motor(MOTOR2, right);
|
|
}
|