amikhalev
/
uas-ugv
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity
Browse Source

Fix IO allocations

try-fix-comms-errors
Alex Mikhalev 6 years ago
parent
a36caa53f1
commit
697f46cd21
4 changed files with 57 additions and 23 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      main/u8g2_esp32_hal.h
  2. 58
      main/ugv_io.cc
  3. 7
      main/ugv_io_mpu.cc
  4. 9
      main/ugv_main.cc

2
main/u8g2_esp32_hal.h
Unescape View File

@ -18,7 +18,7 @@ extern "C" { @@ -18,7 +18,7 @@ extern "C" {
#include "driver/i2c.h"
#include "driver/spi_master.h"
#define I2C_MASTER_NUM I2C_NUM_1 // I2C port number for master dev
#define I2C_MASTER_NUM I2C_NUM_0 // I2C port number for master dev
#define I2C_MASTER_TX_BUF_DISABLE 0 // I2C master do not need buffer
#define I2C_MASTER_RX_BUF_DISABLE 0 // I2C master do not need buffer
#define I2C_MASTER_FREQ_HZ 400000 // I2C master clock frequency

58
main/ugv_io.cc
Unescape View File

@ -12,11 +12,24 @@ @@ -12,11 +12,24 @@
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_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 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_14;
IOClass IO;
@ -31,12 +44,12 @@ void IOClass::Init() { @@ -31,12 +44,12 @@ void IOClass::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);
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_direction(MOTOR_RIGHT_PWM, GPIO_MODE_OUTPUT));
ERROR_CHECK(gpio_set_direction(MOTOR_RIGHT_DIR, GPIO_MODE_OUTPUT));
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 = 20000; // 20khz
@ -44,7 +57,8 @@ void IOClass::InitMotors() { @@ -44,7 +57,8 @@ void IOClass::InitMotors() {
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);
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) {
@ -53,15 +67,27 @@ void IOClass::ReadInputs(Inputs &inputs) { @@ -53,15 +67,27 @@ void IOClass::ReadInputs(Inputs &inputs) {
}
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));
esp_err_t ret;
ret = mcpwm_set_duty(MCPWM_UNIT, MCPWM_TIMER_0, MCPWM_OPR_A,
fabs(outputs.left_motor * 100.0));
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,
fabs(outputs.right_motor * 100.0));
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;
gpio_set_level(MOTOR_LEFT_DIR, left_dir);
gpio_set_level(MOTOR_RIGHT_DIR, right_dir);
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_LOGD(TAG, "motor outputs: (%f, %f)", outputs.left_motor,
outputs.right_motor);
}
}; // namespace io

7
main/ugv_io_mpu.cc
Unescape View File

@ -9,8 +9,8 @@ @@ -9,8 +9,8 @@
namespace ugv {
namespace io {
static constexpr gpio_num_t MPU_SDA = GPIO_NUM_12;
static constexpr gpio_num_t MPU_SCL = GPIO_NUM_14;
// static constexpr gpio_num_t MPU_SDA = GPIO_NUM_12;
// static constexpr gpio_num_t MPU_SCL = GPIO_NUM_14;
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);
@ -31,7 +31,8 @@ void MPU::Init() { @@ -31,7 +31,8 @@ void MPU::Init() {
mpu_data_ = MpuData{};
mpu_bus_ = &i2c0;
mpu_bus_->begin(MPU_SDA, MPU_SCL);
// This is shared with the oled, so just use those pins
// mpu_bus_->begin(MPU_SDA, MPU_SCL);
mpu_ = new mpud::MPU(*mpu_bus_);
esp_err_t ret;

9
main/ugv_main.cc
Unescape View File

@ -20,7 +20,7 @@ void setup_oled(void) { @@ -20,7 +20,7 @@ void setup_oled(void) {
// For Heltec ESP32 LoRa
// oled = new U8G2_SSD1306_128X64_NONAME_F_HW_I2C(U8G2_R0, 16, 15, 4);
// For wemos Lolin ESP32
oled = new U8G2_SSD1306_128X64_NONAME_F_HW_I2C(U8G2_R0, 16, 4, 5);
oled = new U8G2_SSD1306_128X64_NONAME_F_HW_I2C(U8G2_R0, U8X8_PIN_NONE, 4, 5);
oled->initDisplay();
oled->clearDisplay();
oled->setPowerSave(false);
@ -45,8 +45,12 @@ void loop(void) { @@ -45,8 +45,12 @@ void loop(void) {
static char buf[BUF_SZ];
static io::Inputs inputs;
static io::Outputs outputs;
IO.ReadInputs(inputs);
outputs.left_motor = outputs.left_motor > 0 ? -0.5 : 1.0;
outputs.right_motor = outputs.right_motor > 0 ? -0.5 : 1.0;
IO.WriteOutputs(outputs);
// ESP_LOGI(TAG, "inputs %s", inputs.ToString());
oled->firstPage();
@ -84,6 +88,9 @@ void loop(void) { @@ -84,6 +88,9 @@ void loop(void) {
} else {
oled->drawStr(4, 4 * 8, "no pkt rx");
}
snprintf(buf, BUF_SZ, "motors: (%f, %f)", outputs.left_motor,
outputs.right_motor);
oled->drawStr(4, 6 * 8, buf);
} while (oled->nextPage());
vTaskDelay(pdMS_TO_TICKS(1000));
}

Write Preview
Loading…
Cancel
Save