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.
93 lines
3.2 KiB
93 lines
3.2 KiB
#include "ugv_io.hh" |
|
|
|
#include <driver/gpio.h> |
|
#include <driver/mcpwm.h> |
|
#include <esp_log.h> |
|
|
|
#include <math.h> |
|
|
|
namespace ugv { |
|
namespace io { |
|
|
|
static const char *TAG = "ugv_io"; |
|
|
|
#define ERROR_CHECK(ret) \ |
|
{ \ |
|
esp_err_t _error_code = (ret); \ |
|
if (_error_code != ESP_OK) { \ |
|
const char *_error_name = esp_err_to_name(_error_code); \ |
|
ESP_LOGE(TAG, "%s:%d error: %s (%d)", __FUNCTION__, __LINE__, \ |
|
_error_name, _error_code); \ |
|
return; \ |
|
} \ |
|
} |
|
|
|
static constexpr mcpwm_unit_t MCPWM_UNIT = MCPWM_UNIT_0; |
|
static constexpr gpio_num_t MOTOR_LEFT_PWM = GPIO_NUM_0; |
|
static constexpr gpio_num_t MOTOR_LEFT_DIR = GPIO_NUM_2; |
|
static constexpr gpio_num_t MOTOR_RIGHT_PWM = GPIO_NUM_12; |
|
static constexpr gpio_num_t MOTOR_RIGHT_DIR = GPIO_NUM_16; |
|
|
|
IOClass IO; |
|
|
|
IOClass::IOClass() {} |
|
|
|
IOClass::~IOClass() {} |
|
|
|
void IOClass::Init() { |
|
InitMotors(); |
|
mpu_.Init(); |
|
gps_.Init(); |
|
} |
|
|
|
void IOClass::InitMotors() { |
|
ERROR_CHECK(gpio_set_direction(MOTOR_LEFT_PWM, GPIO_MODE_OUTPUT)); |
|
ERROR_CHECK(gpio_set_direction(MOTOR_LEFT_DIR, GPIO_MODE_OUTPUT)); |
|
ERROR_CHECK(gpio_set_pull_mode(MOTOR_LEFT_DIR, GPIO_FLOATING)); |
|
ERROR_CHECK(gpio_set_direction(MOTOR_RIGHT_PWM, GPIO_MODE_OUTPUT)); |
|
ERROR_CHECK(gpio_set_direction(MOTOR_RIGHT_DIR, GPIO_MODE_OUTPUT)); |
|
ERROR_CHECK(gpio_set_pull_mode(MOTOR_RIGHT_DIR, GPIO_FLOATING)); |
|
ERROR_CHECK(mcpwm_gpio_init(MCPWM_UNIT, MCPWM0A, MOTOR_LEFT_PWM)); |
|
ERROR_CHECK(mcpwm_gpio_init(MCPWM_UNIT, MCPWM1A, MOTOR_RIGHT_PWM)); |
|
|
|
mcpwm_config_t mcpwm_config; |
|
mcpwm_config.frequency = 10000; // 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 |
|
ERROR_CHECK(mcpwm_init(MCPWM_UNIT, MCPWM_TIMER_0, &mcpwm_config)); |
|
ERROR_CHECK(mcpwm_init(MCPWM_UNIT, MCPWM_TIMER_1, &mcpwm_config)); |
|
} |
|
|
|
void IOClass::ReadInputs(Inputs &inputs) { |
|
gps_.GetData(inputs.gps); |
|
mpu_.GetData(inputs.mpu); |
|
} |
|
|
|
void IOClass::WriteOutputs(const Outputs &outputs) { |
|
esp_err_t ret; |
|
ret = mcpwm_set_duty(MCPWM_UNIT, MCPWM_TIMER_0, MCPWM_OPR_A, |
|
fabsf(outputs.left_motor * 100.0f)); |
|
ERROR_CHECK(ret); |
|
ret = mcpwm_start(MCPWM_UNIT, MCPWM_TIMER_0); |
|
ERROR_CHECK(ret); |
|
ret = mcpwm_set_duty(MCPWM_UNIT, MCPWM_TIMER_1, MCPWM_OPR_A, |
|
fabsf(outputs.right_motor * 100.0f)); |
|
ERROR_CHECK(ret); |
|
ret = mcpwm_start(MCPWM_UNIT, MCPWM_TIMER_1); |
|
ERROR_CHECK(ret); |
|
bool left_dir, right_dir; |
|
left_dir = outputs.left_motor < 0.f; |
|
right_dir = outputs.right_motor < 0.f; |
|
|
|
ret = gpio_set_level(MOTOR_LEFT_DIR, left_dir); |
|
ERROR_CHECK(ret); |
|
ret = gpio_set_level(MOTOR_RIGHT_DIR, right_dir); |
|
ERROR_CHECK(ret); |
|
ESP_LOGV(TAG, "motor outputs: (%f, %f)", outputs.left_motor, |
|
outputs.right_motor); |
|
} |
|
|
|
}; // namespace io |
|
}; // namespace ugv
|
|
|