You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
45 lines
1.5 KiB
45 lines
1.5 KiB
#include "ugv_io.h" |
|
|
|
#include <driver/gpio.h> |
|
#include <driver/mcpwm.h> |
|
#include <math.h> |
|
|
|
#define IO_MCPWM_UNIT MCPWM_UNIT_0 |
|
#define IO_MOTOR_LEFT_PWM 0 |
|
#define IO_MOTOR_LEFT_DIR 0 |
|
#define IO_MOTOR_RIGHT_PWM 0 |
|
#define IO_MOTOR_RIGHT_DIR 0 |
|
|
|
typedef struct ugv_io_s { |
|
} ugv_io_t; |
|
|
|
static ugv_io_t io; |
|
|
|
void ugv_io_init() { |
|
gpio_set_direction(IO_MOTOR_LEFT_PWM, GPIO_MODE_OUTPUT); |
|
gpio_set_direction(IO_MOTOR_LEFT_DIR, GPIO_MODE_OUTPUT); |
|
gpio_set_direction(IO_MOTOR_RIGHT_PWM, GPIO_MODE_OUTPUT); |
|
gpio_set_direction(IO_MOTOR_RIGHT_DIR, GPIO_MODE_OUTPUT); |
|
mcpwm_gpio_init(IO_MCPWM_UNIT, MCPWM0A, IO_MOTOR_LEFT_PWM); |
|
mcpwm_gpio_init(IO_MCPWM_UNIT, MCPWM0B, IO_MOTOR_RIGHT_PWM); |
|
|
|
mcpwm_config_t mcpwm_config; |
|
mcpwm_config.frequency = 20000; // 20khz |
|
mcpwm_config.cmpr_a = 0; |
|
mcpwm_config.cmpr_b = 0; |
|
mcpwm_config.counter_mode = MCPWM_UP_DOWN_COUNTER; // for symmetric pwm |
|
mcpwm_config.duty_mode = MCPWM_DUTY_MODE_0; // active high |
|
mcpwm_init(IO_MCPWM_UNIT, MCPWM_TIMER_0, &mcpwm_config); |
|
} |
|
|
|
void ugv_io_write_outputs(ugv_outputs_t *outputs) { |
|
mcpwm_set_duty(IO_MCPWM_UNIT, MCPWM_TIMER_0, MCPWM_OPR_A, |
|
fabs(outputs->left_motor)); |
|
mcpwm_set_duty(IO_MCPWM_UNIT, MCPWM_TIMER_0, MCPWM_OPR_B, |
|
fabs(outputs->right_motor)); |
|
bool left_dir, right_dir; |
|
left_dir = outputs->left_motor < 0.f; |
|
right_dir = outputs->right_motor < 0.f; |
|
gpio_set_level(IO_MOTOR_LEFT_DIR, left_dir); |
|
gpio_set_level(IO_MOTOR_RIGHT_DIR, right_dir); |
|
} |