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

Fix IO allocations

Browse Source
This commit is contained in:
Alex Mikhalev 2019-01-24 18:10:38 -08:00
parent a36caa53f1
commit 697f46cd21
4 changed files with 57 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
main/u8g2_esp32_hal.h
View File

@ -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
View File

@ -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() {
}
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() {
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) {
}
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
View File

@ -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() {
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;

13
main/ugv_main.cc
View File

@ -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);
@ -43,10 +43,14 @@ void loop(void) {
static int32_t last_packet_rssi;
static int8_t last_packet_snr;
static char buf[BUF_SZ];
static io::Inputs inputs;
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) {
} 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));
}