Add distance sensor, rewrite OLED to use new i2c library

main/CMakeLists.txt

@@ -17,7 +17,7 @@ else()
 endif()
 
 idf_component_register(SRCS ${ALL_SRCS}
-                       PRIV_REQUIRES esp_psram spi_flash nvs_flash esp_event rpc constants config rmt esp_driver_gptimer dataLink flatbuffers esp_driver_ledc oled app_update
+                       PRIV_REQUIRES esp_psram spi_flash nvs_flash esp_event rpc constants config rmt esp_driver_gptimer dataLink flatbuffers esp_driver_ledc oled app_update vl53l0x
                        INCLUDE_DIRS "include")
 
 if(DEFINED SRC_BOARD AND SRC_BOARD)

main/LoopManager.cpp

@@ -8,6 +8,7 @@
 #include "SensorMessageBuilder.h"
 #include "TopologyMessageBuilder.h"
 #include "esp_wifi.h"
+#include "freertos/projdefs.h"
 
 #define ACTUATOR_CMD_TAG 5
 #define TOPOLOGY_CMD_TAG 6
@@ -16,16 +17,22 @@
 
 #define METADATA_PERIOD_MS 1000
 #define SENSOR_DATA_PERIOD_MS 1000
+#define SLEEP_WITH_NO_ACTUATOR_MS 2000
 
 [[noreturn]] void LoopManager::control_loop() const {
     uint8_t buffer[512];
     while (true) {
+        if (m_actuator == nullptr) {
+            vTaskDelay(pdMS_TO_TICKS(SLEEP_WITH_NO_ACTUATOR_MS));
+            continue;
+        }
+
         if (m_messaging_interface->recv(buffer, 512,
                                     ACTUATOR_CMD_TAG)) {
                                         m_actuator->actuate(buffer);
                                         send_sensor_reading(false);
                                     }
-            }
+    }
 }
 
 [[noreturn]] void LoopManager::sensor_loop(char *args) {
@@ -72,10 +79,20 @@
 
 void LoopManager::send_sensor_reading(bool durable) const {
     Flatbuffers::SensorMessageBuilder smb{};
-    // todo: get data from sensor
+    std::vector<Flatbuffers::sensor_value> sensor_values;
+
     if (m_actuator) {
         auto data = m_actuator->get_sensor_data();
-        const auto [ptr, size] = smb.build_sensor_message(data);
+        sensor_values.insert(sensor_values.end(), data.begin(), data.end());
+    }
+
+    if (m_sensor) {
+        auto data = m_sensor->get_sensor_data();
+        sensor_values.insert(sensor_values.end(), data.begin(), data.end());
+    }
+
+    if (sensor_values.size() > 0) {
+        const auto [ptr, size] = smb.build_sensor_message(sensor_values);
         m_messaging_interface->send((uint8_t *)ptr, size, PC_ADDR, SENSOR_TAG, durable);
     }
 }

main/control/DistanceSensor.cpp

@@ -0,0 +1,70 @@
+
+#include "SensorMessageBuilder.h"
+#include "constants/module.h"
+#include "control/DistanceSensor.h"
+#include "esp_log.h"
+
+#define I2C_PORT 1
+#define XSHUT_PIN -1
+#define TAG "DistanceSensor"
+
+DistanceSensor::DistanceSensor() {
+    i2c_master_bus_config_t bus_config = {
+        .i2c_port = I2C_PORT,
+        .sda_io_num = (gpio_num_t)DISTANCE_SDA,
+        .scl_io_num = (gpio_num_t)DISTANCE_SCL,
+        .clk_source = I2C_CLK_SRC_DEFAULT,
+        .glitch_ignore_cnt = 7,
+        .flags = {
+            .enable_internal_pullup = true,
+        }
+    };
+
+    i2c_master_bus_handle_t bus_handle;
+    i2c_new_master_bus(&bus_config, &bus_handle);
+
+    // Initialize first sensor with default address
+    m_sensor= vl53l0x_config_with_bus(bus_handle, XSHUT_PIN, 0x29, 1);
+    vl53l0x_init(m_sensor);
+    vl53l0x_setAddress(m_sensor, 0x30);  // Change address
+
+    // Start continuous ranging on both
+    vl53l0x_startContinuous(m_sensor, 0);
+
+
+    // m_sensor = vl53l0x_config(I2C_PORT, DISTANCE_SCL, DISTANCE_SDA, XSHUT_PIN, 0x29, 1);
+    // if (!m_sensor) {
+    //     ESP_LOGE(TAG, "Failed to configure sensor");
+    //     return;
+    // }
+
+    // const char *err = vl53l0x_init(m_sensor);
+    // if (err) {
+    //     ESP_LOGE(TAG, "Init failed: %s", err);
+    //     vl53l0x_end(m_sensor);
+    //     m_sensor = nullptr;
+    //     return;
+    // }
+
+    // // Start continuous ranging
+    // vl53l0x_startContinuous(m_sensor, 0);
+}
+
+DistanceSensor::~DistanceSensor() {
+    if (m_sensor == nullptr) {
+        return;
+    }
+
+    vl53l0x_end(m_sensor);
+}
+
+std::vector<Flatbuffers::sensor_value> DistanceSensor::get_sensor_data() {
+    uint16_t range = vl53l0x_readRangeContinuousMillimeters(m_sensor);
+    if (range == 65535) {
+        ESP_LOGW(TAG, "Out of range or timeout");
+    } else {
+        ESP_LOGI(TAG, "Range: %d mm", range);
+        return {Flatbuffers::distance{(float)range}};
+    }
+    return {{}};
+}

main/control/OledActuator.cpp

@@ -19,6 +19,7 @@ void OledActuator::actuate(uint8_t *cmd) {
     const auto new_str = text_control_message->message()->str();
 
     if (m_display_str != new_str) {
+
         m_display_str = new_str;
 
         m_oled->clear_display();

main/control/SensorFactory.cpp

@@ -0,0 +1,15 @@
+
+#include <memory>
+
+#include "control/SensorFactory.h"
+#include "control/DistanceSensor.h"
+#include "flatbuffers_generated/RobotModule_generated.h"
+
+std::unique_ptr<ISensor> SensorFactory::create_sensor(const ModuleType type) {
+  switch (type) {
+  case ModuleType_DISTANCE_SENSOR:
+    return std::make_unique<DistanceSensor>();
+  default:
+    return nullptr;
+  }
+}

main/include/LoopManager.h

@@ -11,13 +11,15 @@
 
 #include "MessagingInterface.h"
 #include "control/ActuatorFactory.h"
+#include "control/SensorFactory.h"
 #include "control/IActuator.h"
 
 class LoopManager {
 public:
     LoopManager() : m_config_manager(ConfigManager::get_instance()),
         m_messaging_interface(std::make_unique<MessagingInterface>()),
-       m_actuator(ActuatorFactory::create_actuator(m_config_manager.get_module_type())) {
+       m_actuator(ActuatorFactory::create_actuator(m_config_manager.get_module_type())),
+       m_sensor(SensorFactory::create_sensor(m_config_manager.get_module_type())) {
            xTaskCreate(reinterpret_cast<TaskFunction_t>(LoopManager::metadata_tx_loop),
                    "metadata_tx", 3096, this, 3, nullptr);
            xTaskCreate(reinterpret_cast<TaskFunction_t>(LoopManager::sensor_loop),
@@ -43,6 +45,7 @@ private:
     ConfigManager& m_config_manager;
     std::unique_ptr<MessagingInterface> m_messaging_interface;
     std::unique_ptr<IActuator> m_actuator;
+    std::unique_ptr<ISensor> m_sensor;
 
     void send_sensor_reading(bool durable) const;
 };

main/include/control/DistanceSensor.h

@@ -0,0 +1,21 @@
+
+#ifndef DCMOTORACTUATOR_H
+#define DCMOTORACTUATOR_H
+
+#include "control/ISensor.h"
+#include "freertos/FreeRTOS.h"
+#include "freertos/task.h"
+#include "vl53l0x.h"
+
+class DistanceSensor final : public ISensor {
+  public:
+    DistanceSensor();
+    ~DistanceSensor() override;
+    std::vector<Flatbuffers::sensor_value> get_sensor_data() override;
+
+  private:
+    vl53l0x_t *m_sensor = nullptr;
+
+};
+
+#endif //SERVO1ACTUATOR_H

main/include/control/ISensor.h

@@ -5,10 +5,13 @@
 #ifndef ISENSOR_H
 #define ISENSOR_H
 
+#include <vector>
+#include "SensorMessageBuilder.h"
+
 class ISensor {
 public:
     virtual ~ISensor() {}
-    virtual void get_reading() = 0; // todo: return a flatbuffer object
+    virtual std::vector<Flatbuffers::sensor_value> get_sensor_data() = 0;
 };
 
 #endif //ISENSOR_H

main/include/control/SensorFactory.h

@@ -5,6 +5,9 @@
 #ifndef SENSORFACTORY_H
 #define SENSORFACTORY_H
 
+#include "flatbuffers_generated/RobotModule_generated.h"
+#include "control/ISensor.h"
+
 class SensorFactory {
 public:
     static std::unique_ptr<ISensor> create_sensor(ModuleType type);

main/main.cpp

@@ -1,3 +1,4 @@
+#include "flatbuffers_generated/RobotModule_generated.h"
 #if !defined(RMT_TEST) || (defined(RMT_TEST) && RMT_TEST == 0)
 // #include <cstdio>
 // #include <memory>