Working IMU with polling

main/motors.c

@@ -0,0 +1,126 @@
+#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));
+}
+
+void 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);
+}
+
+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);
+}