generated from sirlilpanda/kicad-project-template-actionless
106 lines
3.5 KiB
C
106 lines
3.5 KiB
C
#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);
|
|
}
|
|
|