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
6 years ago
|
#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);
|
||
|
|
}
|