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.
95 lines
2.9 KiB
95 lines
2.9 KiB
6 years ago
|
#include "ugv_io.hh"
|
||
|
|
|
||
|
|
#include <driver/gpio.h>
|
||
|
|
#include <driver/mcpwm.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 const char *TAG = "ugv_io";
|
||
|
|
|
||
|
|
IO io;
|
||
|
|
|
||
|
|
IO::IO() { mpu_ = new mpud::MPU(); }
|
||
|
|
|
||
|
|
IO::~IO() {
|
||
|
|
delete mpu_;
|
||
|
|
}
|
||
|
|
|
||
|
|
void IO::Init() {
|
||
|
|
esp_err_t ret;
|
||
|
|
|
||
|
|
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);
|
||
|
|
|
||
|
|
do {
|
||
|
|
ret = mpu_->testConnection();
|
||
|
|
if (ret != ESP_OK) {
|
||
|
|
ESP_LOGE(TAG, "MPU not connected");
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
ret = mpu_->initialize();
|
||
|
|
if (ret != ESP_OK) {
|
||
|
|
ESP_LOGE(TAG, "MPU initialization error");
|
||
|
|
break;
|
||
|
|
}
|
||
|
|
mpu_->setAccelFullScale(MPU_ACCEL_FS);
|
||
|
|
mpu_->setGyroFullScale(MPU_GYRO_FS);
|
||
|
|
ESP_LOGI(TAG, "MPU initialized");
|
||
|
|
} while (0);
|
||
|
|
|
||
|
|
} // namespace io
|
||
|
|
|
||
|
|
void IO::ReadInputs(Inputs &inputs) {
|
||
|
|
esp_err_t ret = mpu_->motion(&accel_, &gyro_, &mag_);
|
||
|
|
if (ret != ESP_OK) {
|
||
|
|
ESP_LOGE(TAG, "error reading MPU");
|
||
|
|
}
|
||
|
|
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 IO::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
|