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.
112 lines
3.7 KiB
112 lines
3.7 KiB
#include "ugv_io.hh" |
|
|
|
#include <driver/gpio.h> |
|
#include <driver/mcpwm.h> |
|
#include <driver/uart.h> |
|
#include <esp_log.h> |
|
#include <math.h> |
|
|
|
#include "MPU.hpp" |
|
#include "mpu/math.hpp" |
|
|
|
namespace ugv { |
|
namespace io { |
|
|
|
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_0; |
|
static constexpr gpio_num_t MOTOR_RIGHT_PWM = GPIO_NUM_0; |
|
static constexpr gpio_num_t MOTOR_RIGHT_DIR = GPIO_NUM_0; |
|
static constexpr mpud::accel_fs_t MPU_ACCEL_FS = mpud::ACCEL_FS_2G; |
|
static constexpr mpud::gyro_fs_t MPU_GYRO_FS = mpud::GYRO_FS_500DPS; |
|
static constexpr float MPU_MAG_TO_FLUX = (4912.f) / (32760.f); |
|
|
|
static constexpr size_t GPS_BUF_SIZE = 1024; |
|
static constexpr uart_port_t GPS_UART = UART_NUM_1; |
|
static constexpr int GPS_UART_TX_PIN = 17; |
|
static constexpr int GPS_UART_RX_PIN = 23; |
|
static constexpr int GPS_UART_BAUD = 9600; |
|
static constexpr size_t GPS_UART_RX_BUF_SIZE = 1024; |
|
static constexpr size_t GPS_UART_TX_BUF_SIZE = 0; |
|
static constexpr size_t GPS_UART_QUEUE_SIZE = 8; |
|
|
|
const char NMEA_OUTPUT_RMCGGA[] = |
|
"$PMTK314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0*28\r\n"; |
|
const char NMEA_SET_UPDATE_1HZ[] = "$PMTK220,1000*1F\r\n"; |
|
const char NMEA_Q_RELEASE[] = "$PMTK605*31\r\n"; |
|
|
|
static const char *TAG = "ugv_io"; |
|
|
|
IOClass IO; |
|
|
|
IOClass::IOClass() { mpu_ = new mpud::MPU(); gps_ = new UART_GPS(); } |
|
|
|
IOClass::~IOClass() { delete mpu_; delete gps_; } |
|
|
|
void IOClass::Init() { |
|
InitMotors(); |
|
InitMPU(); |
|
gps_->Init(); |
|
} |
|
|
|
void IOClass::InitMotors() { |
|
gpio_set_direction(MOTOR_LEFT_PWM, GPIO_MODE_OUTPUT); |
|
gpio_set_direction(MOTOR_LEFT_DIR, GPIO_MODE_OUTPUT); |
|
gpio_set_direction(MOTOR_RIGHT_PWM, GPIO_MODE_OUTPUT); |
|
gpio_set_direction(MOTOR_RIGHT_DIR, GPIO_MODE_OUTPUT); |
|
mcpwm_gpio_init(MCPWM_UNIT, MCPWM0A, MOTOR_LEFT_PWM); |
|
mcpwm_gpio_init(MCPWM_UNIT, MCPWM0B, 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(MCPWM_UNIT, MCPWM_TIMER_0, &mcpwm_config); |
|
} |
|
|
|
void IOClass::InitMPU() { |
|
esp_err_t ret; |
|
ret = mpu_->testConnection(); |
|
if (ret != ESP_OK) { |
|
ESP_LOGE(TAG, "MPU not connected"); |
|
return; |
|
} |
|
ret = mpu_->initialize(); |
|
if (ret != ESP_OK) { |
|
ESP_LOGE(TAG, "MPU initialization error"); |
|
return; |
|
} |
|
mpu_->setAccelFullScale(MPU_ACCEL_FS); |
|
mpu_->setGyroFullScale(MPU_GYRO_FS); |
|
ESP_LOGI(TAG, "MPU initialized"); |
|
} |
|
|
|
void IOClass::ReadInputs(Inputs &inputs) { |
|
esp_err_t ret = mpu_->motion(&accel_, &gyro_, &mag_); |
|
if (ret != ESP_OK) { |
|
ESP_LOGE(TAG, "error reading MPU"); |
|
} |
|
gps_->GetInfo(inputs.gps_info); |
|
inputs.accel = mpud::accelGravity(accel_, MPU_ACCEL_FS); |
|
inputs.gyro_rate = mpud::gyroDegPerSec(gyro_, MPU_GYRO_FS); |
|
inputs.mag.x = ((float)mag_.x) * MPU_MAG_TO_FLUX; |
|
inputs.mag.y = ((float)mag_.y) * MPU_MAG_TO_FLUX; |
|
inputs.mag.z = ((float)mag_.z) * MPU_MAG_TO_FLUX; |
|
} |
|
|
|
void IOClass::WriteOutputs(const Outputs &outputs) { |
|
mcpwm_set_duty(MCPWM_UNIT, MCPWM_TIMER_0, MCPWM_OPR_A, |
|
fabs(outputs.left_motor)); |
|
mcpwm_set_duty(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(MOTOR_LEFT_DIR, left_dir); |
|
gpio_set_level(MOTOR_RIGHT_DIR, right_dir); |
|
} |
|
|
|
}; // namespace io |
|
}; // namespace ugv
|
|
|