diff --git a/main/e32_driver.cc b/main/e32_driver.cc index d2549f4..2cbff17 100644 --- a/main/e32_driver.cc +++ b/main/e32_driver.cc @@ -104,12 +104,15 @@ esp_err_t E32_Driver::Free() { esp_err_t E32_Driver::ReadParams(Params& params) { esp_err_t ret; ret = RawWrite(CMD_READ_PARAMS, sizeof(CMD_READ_PARAMS)); + ret = WaitWriteDone(); if (ret != ESP_OK) { + ESP_LOGE(TAG, "error writing read params cmd"); return ret; } uint8_t param_data[PARAMS_LEN]; ret = Read(param_data, PARAMS_LEN, pdMS_TO_TICKS(1000)); - if (ret != ESP_OK) { + if (ret != PARAMS_LEN) { + ESP_LOGE(TAG, "error reading params"); return ret; } @@ -241,7 +244,8 @@ esp_err_t E32_Driver::WriteParams(const Params& params) { param_data[5] |= 3; } - ret = RawWrite(param_data, PARAMS_LEN); + RawWrite(param_data, PARAMS_LEN); + ret = WaitWriteDone(); if (ret != ESP_OK) { return ret; } @@ -251,8 +255,8 @@ esp_err_t E32_Driver::WriteParams(const Params& params) { return ESP_OK; } -int E32_Driver::Write(Address address, Channel channel, - const uint8_t* data, size_t data_size) { +int E32_Driver::Write(Address address, Channel channel, const uint8_t* data, + size_t data_size) { int written; if (params_.tx_mode == TxFixed) { uint8_t header[3]; @@ -260,7 +264,8 @@ int E32_Driver::Write(Address address, Channel channel, header[1] = address & 0xFF; header[2] = channel; - written = uart_write_bytes(config_.uart_port, (char*)header, sizeof(header)); + written = + uart_write_bytes(config_.uart_port, (char*)header, sizeof(header)); if (written < 0) { return written; } @@ -272,13 +277,17 @@ int E32_Driver::Write(const uint8_t* data, size_t data_size) { return Write(params_.address, params_.comm_channel, data, data_size); } int E32_Driver::Write(Address address, Channel channel, - const std::string& data) { + const std::string& data) { return Write(address, channel, (uint8_t*)data.c_str(), data.size()); } int E32_Driver::Write(const std::string& data) { return Write((uint8_t*)data.c_str(), data.size()); } +esp_err_t E32_Driver::WaitWriteDone(TickType_t ticks_to_wait) { + return uart_wait_tx_done(config_.uart_port, ticks_to_wait); +} + int E32_Driver::Read(uint8_t* data, int max_len, TickType_t ticks_to_wait) { return uart_read_bytes(config_.uart_port, (uint8_t*)data, max_len, ticks_to_wait); @@ -304,8 +313,13 @@ int E32_Driver::Read(std::string& data, TickType_t ticks_to_wait) { return total_read; } -esp_err_t E32_Driver::RawWrite(const uint8_t* data, size_t data_size) { - return uart_write_bytes(config_.uart_port, (const char*)data, data_size); +int E32_Driver::RawWrite(const uint8_t* data, size_t data_size) { + int written = + uart_write_bytes(config_.uart_port, (const char*)data, data_size); + if (written < 0) { + ESP_LOGE(TAG, "RawWrite error: %d", written); + } + return written; } } // namespace e32 diff --git a/main/e32_driver.hh b/main/e32_driver.hh index 29f5d9a..a4c610e 100644 --- a/main/e32_driver.hh +++ b/main/e32_driver.hh @@ -67,6 +67,8 @@ class E32_Driver { int Write(Address address, Channel channel, const std::string& data); int Write(const std::string& data); + esp_err_t WaitWriteDone(TickType_t ticks_to_wait = portMAX_DELAY); + int Read(uint8_t* data, int max_len, TickType_t ticks_to_wait = portMAX_DELAY); int Read(std::string& data, TickType_t ticks_to_wait = portMAX_DELAY); @@ -76,7 +78,7 @@ class E32_Driver { Config config_; Params params_; - esp_err_t RawWrite(const uint8_t* data, size_t data_size); + int RawWrite(const uint8_t* data, size_t data_size); }; } // namespace e32 diff --git a/main/ugv_display.cc b/main/ugv_display.cc index 5d39bfb..15ffd7d 100644 --- a/main/ugv_display.cc +++ b/main/ugv_display.cc @@ -4,17 +4,15 @@ #include "U8g2lib.hh" #include "ugv_comms.hh" #include "ugv_config.h" -#include "ugv_io.hh" namespace ugv { static const char* TAG = "ugv_display"; using comms::CommsClass; -using io::IOClass; -DisplayClass::DisplayClass(CommsClass* comms, IOClass* io) - : comms_(comms), io_(io) {} +DisplayClass::DisplayClass(CommsClass* comms) + : comms_(comms) {} void DisplayClass::Init() { // For Heltec ESP32 LoRa @@ -39,14 +37,7 @@ void DisplayClass::Run() { int32_t last_packet_rssi; int8_t last_packet_snr; - io::Inputs inputs; - io::Outputs outputs; - while (true) { - 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(); @@ -86,8 +77,8 @@ void DisplayClass::Run() { oled->setCursor(4, 4 * 8); oled->print("no pkt rx"); } - oled->setCursor(4, 6 * 8); - oled->printf("motors: (%f, %f)", outputs.left_motor, outputs.right_motor); + // oled->setCursor(4, 6 * 8); + // oled->printf("motors: (%f, %f)", outputs.left_motor, outputs.right_motor); } while (oled->nextPage()); vTaskDelay(pdMS_TO_TICKS(1000)); } diff --git a/main/ugv_display.hh b/main/ugv_display.hh index 53e10d6..ec53f47 100644 --- a/main/ugv_display.hh +++ b/main/ugv_display.hh @@ -10,20 +10,16 @@ namespace ugv { namespace comms { class CommsClass; } -namespace io { -class IOClass; -} class DisplayClass { public: DisplayClass() = delete; - DisplayClass(comms::CommsClass *comms, io::IOClass *io); + DisplayClass(comms::CommsClass *comms); void Init(); private: comms::CommsClass *comms_; - io::IOClass *io_; U8G2 *oled; TaskHandle_t task_handle_; diff --git a/main/ugv_io.cc b/main/ugv_io.cc index bf5e9de..2b9356f 100644 --- a/main/ugv_io.cc +++ b/main/ugv_io.cc @@ -71,12 +71,12 @@ void IOClass::ReadInputs(Inputs &inputs) { void IOClass::WriteOutputs(const Outputs &outputs) { esp_err_t ret; ret = mcpwm_set_duty(MCPWM_UNIT, MCPWM_TIMER_0, MCPWM_OPR_A, - fabs(outputs.left_motor * 100.0)); + fabsf(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)); + fabsf(outputs.right_motor * 100.0)); ERROR_CHECK(ret); ret = mcpwm_start(MCPWM_UNIT, MCPWM_TIMER_1); ERROR_CHECK(ret); diff --git a/main/ugv_main.cc b/main/ugv_main.cc index e4905d7..7688614 100644 --- a/main/ugv_main.cc +++ b/main/ugv_main.cc @@ -4,13 +4,17 @@ #include "ugv_display.hh" #include "ugv_io.hh" +#include + namespace ugv { using ugv::comms::CommsClass; using ugv::io::IOClass; static const char *TAG = "ugv_main"; -constexpr uint64_t LOOP_PERIOD_US = 1e6 / 1; + +constexpr uint64_t LOOP_PERIOD_US = 1e6 / 100; +static const float PI = atanf(1.0) * 4.0; extern "C" void OnTimeout(void *arg); @@ -26,7 +30,7 @@ struct State { State() { comms = new CommsClass(); io = new IOClass(); - display = new DisplayClass(comms, io); + display = new DisplayClass(comms); } void Init() { @@ -37,21 +41,27 @@ struct State { esp_timer_init(); esp_timer_create_args_t timer_args; - timer_args.callback = OnTimeout; - timer_args.arg = this; + timer_args.callback = OnTimeout; + timer_args.arg = this; timer_args.dispatch_method = ESP_TIMER_TASK; - timer_args.name = "ugv_main_loop"; + timer_args.name = "ugv_main_loop"; esp_timer_create(&timer_args, &this->timer_handle); esp_timer_start_periodic(timer_handle, LOOP_PERIOD_US); } void OnTick() { ESP_LOGD(TAG, "OnTick"); + int64_t time_us = esp_timer_get_time(); + float time_s = ((float)time_us) / 1e6; + io->ReadInputs(inputs); + outputs.left_motor = sinf(time_s * PI); + outputs.right_motor = cosf(time_s * PI); + io->WriteOutputs(outputs); } }; extern "C" void OnTimeout(void *arg) { - State *state = (State*)arg; + State *state = (State *)arg; state->OnTick(); }