Created wacky_bully project + a bit of work towards implementing motor drivers

2026-05-08 16:38:10 +12:00
parent 52aac06b2c
commit baf2f0be9e
10 changed files with 7963 additions and 0 deletions
--- a/software/wacky_bully/components/drv8701/drv8701.c
+++ b/software/wacky_bully/components/drv8701/drv8701.c
@@ -0,0 +1,105 @@
+#include <math.h>
+#include <stdio.h>
+#include "drv8701.h"
+#include "driver/mcpwm_cmpr.h"
+#include "driver/mcpwm_gen.h"
+#include "driver/mcpwm_oper.h"
+#include "driver/mcpwm_timer.h"
+#include "driver/mcpwm_types.h"
+#include "driver/gpio.h"
+#include "esp_err.h"
+#include "freertos/idf_additions.h"
+#include "hal/gpio_types.h"
+#include "portmacro.h"
+
+#define PWM_RESOLUTION_HZ 2000000
+#define PWM_TIMEBASE_PERIOD PWM_RESOLUTION_HZ/20000
+
+
+void drv8701_task(void *args) {
+    drv8701_task_config_t *motor_cfg = ((drv8701_task_config_t *)args);
+    motor_cfg->in_ctrl_queue = xQueueCreate(3, sizeof(float));
+    motor_cfg->current_mutex = xSemaphoreCreateMutex();
+    
+    // --------------------- Configure PWM -------------------------
+    mcpwm_timer_handle_t timer = NULL;
+    mcpwm_timer_config_t timer_cfg = {
+        .group_id = motor_cfg->pwm_group_num,
+        .clk_src = MCPWM_TIMER_CLK_SRC_DEFAULT,
+        .resolution_hz = PWM_RESOLUTION_HZ,
+        .count_mode = MCPWM_TIMER_COUNT_MODE_UP,
+        .period_ticks = PWM_TIMEBASE_PERIOD,
+    };
+    ESP_ERROR_CHECK(mcpwm_new_timer(&timer_cfg, &timer));
+
+    mcpwm_oper_handle_t oper = NULL;
+    mcpwm_operator_config_t oper_cfg = {
+        .group_id = motor_cfg->pwm_group_num,
+    };
+
+    ESP_ERROR_CHECK(mcpwm_new_operator(&oper_cfg, &oper));
+    ESP_ERROR_CHECK(mcpwm_operator_connect_timer(oper, timer));
+    
+    mcpwm_cmpr_handle_t cmpr = NULL;
+    mcpwm_comparator_config_t cmpr_cfg = {
+        .flags.update_cmp_on_tep = true,
+    };
+    
+    ESP_ERROR_CHECK(mcpwm_new_comparator(oper, &cmpr_cfg, &cmpr));
+
+    mcpwm_gen_handle_t gen = NULL;
+    mcpwm_generator_config_t gen_cfg = {
+        .gen_gpio_num = motor_cfg->en_pin,
+    };
+
+    ESP_ERROR_CHECK(mcpwm_new_generator(oper, &gen_cfg, &gen));
+    ESP_ERROR_CHECK(mcpwm_comparator_set_compare_value(cmpr, 0));
+    
+    ESP_ERROR_CHECK(mcpwm_generator_set_action_on_timer_event(gen,
+                    MCPWM_GEN_TIMER_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, MCPWM_TIMER_EVENT_EMPTY, MCPWM_GEN_ACTION_HIGH)));
+    ESP_ERROR_CHECK(mcpwm_generator_set_action_on_compare_event(gen,
+                    MCPWM_GEN_COMPARE_EVENT_ACTION(MCPWM_TIMER_DIRECTION_UP, cmpr, MCPWM_GEN_ACTION_LOW)));
+    
+    ESP_ERROR_CHECK(mcpwm_timer_enable(timer));
+    ESP_ERROR_CHECK(mcpwm_timer_start_stop(timer, MCPWM_TIMER_START_NO_STOP));
+
+    // --------------------- Configure Direction -------------------------
+    
+    printf("Configuring Direction\n");
+    gpio_config_t phase_cfg = {
+        .pin_bit_mask = 1ULL << motor_cfg->ph_pin,
+        .intr_type = GPIO_INTR_DISABLE,
+        .mode = GPIO_MODE_OUTPUT,
+        .pull_down_en = GPIO_PULLDOWN_DISABLE,
+        .pull_up_en = GPIO_PULLUP_DISABLE
+    };
+    gpio_config(&phase_cfg);
+
+    // Main Loop
+    float target_velocity = 0.0;
+    while(1) {
+        
+        // Get new velocity if it has changed
+        if(xQueueReceive(motor_cfg->in_ctrl_queue, &target_velocity, 0)){
+            target_velocity = fmaxf(-1.0, fminf(1.0, target_velocity));
+        }
+        
+
+        uint32_t abs_scaled_vel = (uint32_t) (fabsf(target_velocity) * ((float)PWM_TIMEBASE_PERIOD));
+        mcpwm_comparator_set_compare_value(cmpr, abs_scaled_vel);
+
+        if (target_velocity > 0.0) {
+            gpio_set_level(motor_cfg->ph_pin, 1); 
+        }
+        else {
+            gpio_set_level(motor_cfg->ph_pin, 0); 
+        }
+
+        vTaskDelay(50 / portTICK_PERIOD_MS);
+    }
+}
+
+void set_motor_vel(drv8701_task_config_t *motor_cfg, float velocity) {
+    xQueueSend(motor_cfg->in_ctrl_queue, &velocity, 0);
+}
+