diff --git a/main/u8g2_esp32_hal.h b/main/u8g2_esp32_hal.h index dc23eaf..af42d4e 100644 --- a/main/u8g2_esp32_hal.h +++ b/main/u8g2_esp32_hal.h @@ -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 diff --git a/main/ugv_io.cc b/main/ugv_io.cc index e70d6e4..146fabc 100644 --- a/main/ugv_io.cc +++ b/main/ugv_io.cc @@ -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 diff --git a/main/ugv_io_mpu.cc b/main/ugv_io_mpu.cc index 6fe73de..0407b32 100644 --- a/main/ugv_io_mpu.cc +++ b/main/ugv_io_mpu.cc @@ -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; diff --git a/main/ugv_main.cc b/main/ugv_main.cc index 9deb038..ee4533c 100644 --- a/main/ugv_main.cc +++ b/main/ugv_main.cc @@ -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)); }