move io stuff out of display

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

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

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));
   }

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_;
 

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);

main/ugv_main.cc

@@ -4,13 +4,17 @@
 #include "ugv_display.hh"
 #include "ugv_io.hh"
 
+#include <math.h>
+
 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();
 }