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

software/wacky_bully/components/drv8701/CMakeLists.txt

@@ -0,0 +1,3 @@
+idf_component_register(SRCS "drv8701.c"
+                    INCLUDE_DIRS "include"
+                    REQUIRES esp_driver_mcpwm esp_driver_gpio)

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);
+}
+

software/wacky_bully/components/drv8701/include/drv8701.h

@@ -0,0 +1,38 @@
+#ifndef DRV8701_H
+#define DRV8701_H
+
+#include "freertos/idf_additions.h"
+
+// Enable PWM for speed control
+// Phase chooses direction
+
+typedef struct {
+    uint8_t en_pin;
+    uint8_t ph_pin;
+
+    uint8_t pwm_group_num; 
+
+    // Do not populate, done as part of task creation
+    QueueHandle_t in_ctrl_queue;
+    SemaphoreHandle_t current_mutex; 
+
+} drv8701_task_config_t;
+
+
+/**
+ * @brief Launch the motor driver task
+ *
+ * @param motor_cfg The configuration for the motor driver
+ */
+void drv8701_task(void *args);
+
+/**
+ * @brief Set the velocity of a specific motor driver  
+ *
+ * @param motor_cfg The configuration for the specific motor driver
+ * @param velocity -1.0 to 1.0
+ */
+void set_motor_vel(drv8701_task_config_t *motor_cfg, float velocity);
+
+
+#endif