Servo control

.gitignore

@@ -3,3 +3,4 @@
 .DS_Store
 sdkconfig.old
 .vscode/*
+/.cache

components/constants/include/constants/module.h

@@ -9,8 +9,10 @@
 
 #include "flatbuffers_generated/RobotModule_generated.h"
 
-inline std::unordered_map<int, int> MODULE_TO_NUM_CHANNELS_MAP {{ModuleType_SPLITTER, 4}, {ModuleType_SERVO_1, 2}, {ModuleType_DC_MOTOR, 1}};
+inline std::unordered_map<int, int> MODULE_TO_NUM_CHANNELS_MAP {{ModuleType_SPLITTER, 2}, {ModuleType_SERVO_1, 2}, {ModuleType_DC_MOTOR, 1}};
 
 #define PC_ADDR 0
 
+#define SERVO_GPIO 18
+
 #endif //MODULE_H

components/flatbuffers/AngleControlMessageBuilder.cpp

@@ -0,0 +1,11 @@
+//
+// Created by Johnathon Slightham on 2025-06-30.
+//
+
+#include "AngleControlMessageBuilder.h"
+
+namespace Flatbuffers {
+    const Messaging::AngleControlMessage* AngleControlMessageBuilder::parse_angle_control_message(const uint8_t* buffer) {
+        return flatbuffers::GetRoot<Messaging::AngleControlMessage>(buffer);
+    }
+}

components/flatbuffers/CMakeLists.txt

@@ -1,2 +1,2 @@
-idf_component_register(SRCS "MPIMessageBuilder.cpp"
+idf_component_register(SRCS "MPIMessageBuilder.cpp" "AngleControlMessageBuilder.cpp"
         INCLUDE_DIRS "include")

components/flatbuffers/include/AngleControlMessageBuilder.h

@@ -0,0 +1,22 @@
+//
+// Created by Johnathon Slightham on 2025-06-30.
+//
+
+#ifndef ANGLECONTROLMESSAGEBUILDER_H_
+#define ANGLECONTROLMESSAGEBUILDER_H_
+
+#include <string>
+#include <vector>
+
+#include "SerializedMessage.h"
+#include "flatbuffers_generated/AngleControlMessage_generated.h"
+#include "flatbuffers/flatbuffers.h"
+
+namespace Flatbuffers {
+    class AngleControlMessageBuilder {
+    public:
+        static const Messaging::AngleControlMessage* parse_angle_control_message(const uint8_t* buffer);
+    };
+}
+
+#endif

components/flatbuffers/include/flatbuffers_generated/AngleControlMessage_generated.h

@@ -0,0 +1,94 @@
+// automatically generated by the FlatBuffers compiler, do not modify
+
+
+#ifndef FLATBUFFERS_GENERATED_ANGLECONTROLMESSAGE_MESSAGING_H_
+#define FLATBUFFERS_GENERATED_ANGLECONTROLMESSAGE_MESSAGING_H_
+
+#include "flatbuffers/flatbuffers.h"
+
+// Ensure the included flatbuffers.h is the same version as when this file was
+// generated, otherwise it may not be compatible.
+//static_assert(FLATBUFFERS_VERSION_MAJOR == 25 &&
+              //FLATBUFFERS_VERSION_MINOR == 2 &&
+              //FLATBUFFERS_VERSION_REVISION == 10,
+             //"Non-compatible flatbuffers version included");
+
+namespace Messaging {
+
+struct AngleControlMessage;
+struct AngleControlMessageBuilder;
+
+struct AngleControlMessage FLATBUFFERS_FINAL_CLASS : private ::flatbuffers::Table {
+  typedef AngleControlMessageBuilder Builder;
+  enum FlatBuffersVTableOffset FLATBUFFERS_VTABLE_UNDERLYING_TYPE {
+    VT_ANGLE = 4
+  };
+  int16_t angle() const {
+    return GetField<int16_t>(VT_ANGLE, 0);
+  }
+  bool Verify(::flatbuffers::Verifier &verifier) const {
+    return VerifyTableStart(verifier) &&
+           VerifyField<int16_t>(verifier, VT_ANGLE, 2) &&
+           verifier.EndTable();
+  }
+};
+
+struct AngleControlMessageBuilder {
+  typedef AngleControlMessage Table;
+  ::flatbuffers::FlatBufferBuilder &fbb_;
+  ::flatbuffers::uoffset_t start_;
+  void add_angle(int16_t angle) {
+    fbb_.AddElement<int16_t>(AngleControlMessage::VT_ANGLE, angle, 0);
+  }
+  explicit AngleControlMessageBuilder(::flatbuffers::FlatBufferBuilder &_fbb)
+        : fbb_(_fbb) {
+    start_ = fbb_.StartTable();
+  }
+  ::flatbuffers::Offset<AngleControlMessage> Finish() {
+    const auto end = fbb_.EndTable(start_);
+    auto o = ::flatbuffers::Offset<AngleControlMessage>(end);
+    return o;
+  }
+};
+
+inline ::flatbuffers::Offset<AngleControlMessage> CreateAngleControlMessage(
+    ::flatbuffers::FlatBufferBuilder &_fbb,
+    int16_t angle = 0) {
+  AngleControlMessageBuilder builder_(_fbb);
+  builder_.add_angle(angle);
+  return builder_.Finish();
+}
+
+inline const Messaging::AngleControlMessage *GetAngleControlMessage(const void *buf) {
+  return ::flatbuffers::GetRoot<Messaging::AngleControlMessage>(buf);
+}
+
+inline const Messaging::AngleControlMessage *GetSizePrefixedAngleControlMessage(const void *buf) {
+  return ::flatbuffers::GetSizePrefixedRoot<Messaging::AngleControlMessage>(buf);
+}
+
+inline bool VerifyAngleControlMessageBuffer(
+    ::flatbuffers::Verifier &verifier) {
+  return verifier.VerifyBuffer<Messaging::AngleControlMessage>(nullptr);
+}
+
+inline bool VerifySizePrefixedAngleControlMessageBuffer(
+    ::flatbuffers::Verifier &verifier) {
+  return verifier.VerifySizePrefixedBuffer<Messaging::AngleControlMessage>(nullptr);
+}
+
+inline void FinishAngleControlMessageBuffer(
+    ::flatbuffers::FlatBufferBuilder &fbb,
+    ::flatbuffers::Offset<Messaging::AngleControlMessage> root) {
+  fbb.Finish(root);
+}
+
+inline void FinishSizePrefixedAngleControlMessageBuffer(
+    ::flatbuffers::FlatBufferBuilder &fbb,
+    ::flatbuffers::Offset<Messaging::AngleControlMessage> root) {
+  fbb.FinishSizePrefixed(root);
+}
+
+}  // namespace Messaging
+
+#endif  // FLATBUFFERS_GENERATED_ANGLECONTROLMESSAGE_MESSAGING_H_

components/rpc/CommunicationRouter.cpp

@@ -1,5 +1,6 @@
 #include "CommunicationRouter.h"
 
+#include <AngleControlMessageBuilder.h>
 #include <iostream>
 #include "mDNSDiscoveryService.h"
 #include "MPIMessageBuilder.h"
@@ -101,6 +102,11 @@ void CommunicationRouter::route(uint8_t* buffer, const size_t length) const {
 
     if (mpi_message->destination() == m_module_id) {
         std::cout << "Routing to this module [dest:" << static_cast<int>(mpi_message->destination()) << ", length: " << length << "]" << std::endl;
+
+        const auto temp = Flatbuffers::AngleControlMessageBuilder::parse_angle_control_message(reinterpret_cast<const uint8_t *>(mpi_message->payload()))->angle();
+
+        std::cout << "angle from before router" << temp << std::endl;
+
         this->m_rx_callback(reinterpret_cast<char *>(buffer), 512);
     } else if (mpi_message->destination() == PC_ADDR && this->m_leader == m_module_id) {
         std::cout << "Routing to wifi [dest:" << static_cast<int>(mpi_message->destination()) << ", length: " << length << "]" << std::endl;

components/rpc/MessagingInterface.cpp

@@ -4,6 +4,7 @@
 
 #include "MessagingInterface.h"
 
+#include <AngleControlMessageBuilder.h>
 #include <ConfigManager.h>
 #include <freertos/queue.h>
 #include <freertos/semphr.h>
@@ -53,7 +54,11 @@ void MessagingInterface::handleRecv(const char* recv_buffer, int recv_size) {
 
     checkOrInsertTag(mpi_message->tag());
 
-    xQueueSendToBack(m_tag_to_queue.at(mpi_message->tag()), mpi_message->payload(), 0);
+    const auto temp = Flatbuffers::AngleControlMessageBuilder::parse_angle_control_message(reinterpret_cast<const uint8_t *>(mpi_message->payload()))->angle();
+
+    std::cout << "angle from before queue " << temp << std::endl;
+
+    xQueueSendToBack(m_tag_to_queue.at(mpi_message->tag()), mpi_message->payload()->data(), 0);
 }
 
 void MessagingInterface::checkOrInsertTag(const uint8_t tag) {

main/CMakeLists.txt

@@ -1,6 +1,12 @@
-idf_component_register(SRCS "main.cpp"  "LoopManager.cpp"
-                       PRIV_REQUIRES esp_psram spi_flash nvs_flash esp_event rpc constants config rmt esp_driver_gptimer dataLink
-                       INCLUDE_DIRS "")
+file(GLOB_RECURSE ALL_SRCS
+        "${CMAKE_CURRENT_SOURCE_DIR}/*.c"
+        "${CMAKE_CURRENT_SOURCE_DIR}/*.cpp"
+        "${CMAKE_CURRENT_SOURCE_DIR}/*.S"
+)
+
+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
+                       INCLUDE_DIRS "include")
 
 if(DEFINED BOARD_NAME AND BOARD_NAME)
     message(STATUS "Building for board name: " ${BOARD_NAME})

main/LoopManager.cpp

@@ -10,19 +10,16 @@
 
 #include "esp_log.h"
 
+#define ACTUATOR_CMD_TAG 5
+
 [[noreturn]] void LoopManager::control_loop() {
     const auto messaging_interface = std::make_unique<MessagingInterface>(std::make_unique<WifiManager>());
 
-    char buffer[512];
+    const auto actuator = ActuatorFactory::create_actuator(ConfigManager::get_module_type());
+
+    uint8_t buffer[512];
     while (true) {
-        messaging_interface->recv(buffer, 512, 0, 1);
-        //std::cout << buffer << std::endl;
-
-        std::string s = std::format("num {} bo", ConfigManager::get_module_id());
-        messaging_interface->send(s.data(), s.size(), 0, 2, true);
-
-        ESP_LOGI("MEM", "Free internal RAM: %d", heap_caps_get_free_size(MALLOC_CAP_8BIT));
-        ESP_LOGI("MEM", "Free PSRAM: %d", heap_caps_get_free_size(MALLOC_CAP_SPIRAM));
-
+        messaging_interface->recv(reinterpret_cast<char *>(buffer), 512, PC_ADDR, ACTUATOR_CMD_TAG);
+        actuator->actuate(buffer);
     }
 }

main/control/ActuatorFactory.cpp

@@ -0,0 +1,16 @@
+//
+// Created by Johnathon Slightham on 2025-07-15.
+//
+
+#include "control/ActuatorFactory.h"
+
+#include <control/Servo1Actuator.h>
+
+std::unique_ptr<IActuator> ActuatorFactory::create_actuator(ModuleType type) {
+    switch (type) {
+        case ModuleType_SERVO_1:
+            return std::make_unique<Servo1Actuator>();
+        default:
+            return nullptr;
+    }
+}

main/control/Servo1Actuator.cpp

@@ -0,0 +1,52 @@
+//
+// Created by Johnathon Slightham on 2025-07-15.
+//
+
+#include "control/Servo1Actuator.h"
+#include "util/number_utils.h"
+
+#include "driver/ledc.h"
+#include "constants/module.h"
+
+#include "AngleControlMessageBuilder.h"
+
+#define LOW_DUTY 200
+#define HIGH_DUTY 1000
+
+Servo1Actuator::Servo1Actuator() {
+    ledc_timer_config_t ledc_timer = {
+        .speed_mode = LEDC_LOW_SPEED_MODE,
+        .duty_resolution = LEDC_TIMER_13_BIT,
+        .timer_num = LEDC_TIMER_0,
+        .freq_hz = 50, // 4kHz
+        .clk_cfg = LEDC_AUTO_CLK,
+    };
+    ESP_ERROR_CHECK(ledc_timer_config(&ledc_timer));
+
+    ledc_channel_config_t ledc_channel = {
+        .gpio_num = SERVO_GPIO,
+        .speed_mode = LEDC_LOW_SPEED_MODE,
+        .channel = LEDC_CHANNEL_0,
+        .intr_type = LEDC_INTR_DISABLE,
+        .timer_sel = LEDC_TIMER_0,
+        .duty = 600, // midpoint
+        .hpoint = 0,
+    };
+
+    ESP_ERROR_CHECK(ledc_channel_config(&ledc_channel));
+}
+
+void Servo1Actuator::actuate(uint8_t *cmd) {
+    for (int i = 0; i < 512; i++) {
+        printf("%x ", cmd[i]);
+    }
+    printf("\n");
+    const auto* angleControlCmd = Flatbuffers::AngleControlMessageBuilder::parse_angle_control_message(cmd);
+    std::cout << "cmd: " << angleControlCmd->angle() << std::endl;
+    const auto newDuty = util::mapRange<int32_t>(angleControlCmd->angle(), 0, 180, LOW_DUTY, HIGH_DUTY);
+    std::cout << "newDuty: " << newDuty << std::endl;
+
+    ESP_ERROR_CHECK(ledc_set_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0, newDuty));
+    ESP_ERROR_CHECK(ledc_update_duty(LEDC_LOW_SPEED_MODE, LEDC_CHANNEL_0));
+}
+

main/include/LoopManager.h

@@ -5,6 +5,9 @@
 #ifndef LOOPMANAGER_H
 #define LOOPMANAGER_H
 
+#include "control/IActuator.h"
+#include "control/ActuatorFactory.h"
+
 class LoopManager {
 public:
     [[noreturn]] static void control_loop();

main/include/control/ActuatorFactory.h

@@ -0,0 +1,18 @@
+//
+// Created by Johnathon Slightham on 2025-07-15.
+//
+
+#ifndef ACTUATORFACTORY_H
+#define ACTUATORFACTORY_H
+
+#include "IActuator.h"
+#include "flatbuffers_generated/RobotModule_generated.h"
+
+class ActuatorFactory {
+public:
+    static std::unique_ptr<IActuator> create_actuator(ModuleType type);
+};
+
+
+
+#endif //ACTUATORFACTORY_H

main/include/control/IActuator.h

@@ -0,0 +1,15 @@
+//
+// Created by Johnathon Slightham on 2025-07-15.
+//
+
+#ifndef IACTUATOR_H
+#define IACTUATOR_H
+#include <cstdint>
+
+class IActuator {
+public:
+    virtual ~IActuator() {}
+    virtual void actuate(uint8_t *cmd) = 0;
+};
+
+#endif //IACTUATOR_H

main/include/control/Servo1Actuator.h

@@ -0,0 +1,18 @@
+//
+// Created by Johnathon Slightham on 2025-07-15.
+//
+
+// 180 deg servo
+
+#ifndef SERVO1ACTUATOR_H
+#define SERVO1ACTUATOR_H
+
+#include "IActuator.h"
+
+class Servo1Actuator final : public IActuator {
+public:
+    Servo1Actuator();
+    void actuate(uint8_t *cmd) override;
+};
+
+#endif //SERVO1ACTUATOR_H

main/include/util/number_utils.h

@@ -0,0 +1,24 @@
+//
+// Created by Johnathon Slightham on 2025-07-15.
+//
+
+#ifndef INT_UTILS_H
+#define INT_UTILS_H
+
+#include <iostream>
+#include <type_traits>
+
+namespace util {
+    template<typename T>
+    T mapRange(T value, T inMin, T inMax, T outMin, T outMax) {
+        static_assert(std::is_arithmetic<T>::value, "Template parameter must be a numeric type");
+
+        if (inMin == inMax) {
+            return value;
+        }
+
+        return (value - inMin) * (outMax - outMin) / (inMax - inMin) + outMin;
+    }
+}
+
+#endif //INT_UTILS_H

main/link_layer_main.cpp

@@ -1,278 +0,0 @@
-//Used for link layer testing (change name to main.cpp to use)
-#include <cstdio>
-#include <memory>
-
-#include "sdkconfig.h"
-#include "freertos/FreeRTOS.h"
-#include "freertos/task.h"
-#include "esp_flash.h"
-#include "nvs_flash.h"
-
-#include "RMTManager.h"
-#include "DataLinkManager.h"
-
-#include <esp_netif.h>
-#include <esp_event.h>
-#include <freertos/semphr.h>
-#include "driver/gptimer.h"
-#include "esp_log.h"
-
-#define DATA_SIZE_TEST 270
-
-struct TaskArgs{
-    DataLinkManager* link_layer_obj;
-    uint8_t task_id;
-    uint8_t receiver_id;
-    QueueHandle_t receive_queue;
-};
-
-struct ReceviedFrame{
-    uint8_t buf[MAX_CONTROL_DATA_LEN + CONTROL_FRAME_OVERHEAD]; //max 41B
-    size_t len;
-};
-
-void receive_frames(void* arg){
-    TaskArgs* args = (TaskArgs*)arg;
-    
-    DataLinkManager* obj = args->link_layer_obj;
-    
-    if (obj == nullptr){
-        ESP_LOGE("thread", "bad pointer\n");
-        vTaskDelete(NULL); //should never get here
-    }
-
-    QueueHandle_t shared_queue = (QueueHandle_t)args->receive_queue;
-
-    uint8_t curr_channel = args-> task_id;
-
-    printf("RX JOB for task %d starting...\n", curr_channel);
-    esp_err_t res;
-
-    uint8_t recv_buf[DATA_SIZE_TEST];
-    memset(recv_buf, 0, DATA_SIZE_TEST);
-    size_t recv_len = 0;
-
-    ReceviedFrame recv_frame = {};
-
-    while(true){
-        res = obj->start_receive_frames(curr_channel); // this will be moved to a separate thread with a shared queue
-        if (res != ESP_OK){
-            ESP_LOGE("thread", "Failed to start rx async job on thread %d", curr_channel);
-            continue;
-        }
-
-        res = obj->receive(recv_buf, sizeof(recv_buf), &recv_len, curr_channel);
-        if (res != ESP_OK){
-            ESP_LOGE("thread", "Failed to receive message on thread %d", curr_channel);
-            continue;
-        } else {
-            // printf("Successfully receive message\n");
-        }
-
-        if (recv_len == 0){
-            continue;
-        }
-
-        recv_frame.len = recv_len;
-        memcpy((void*)recv_frame.buf, (void*)recv_buf, recv_len);
-        
-        if (xQueueSendToBack(shared_queue, (void*)&recv_frame, (TickType_t) 10) != pdPASS){
-            ESP_LOGE("RX Job", "Failed to push received frame onto shared queue for channel %d", curr_channel);
-        }
-    }
-
-}
-
-void multi_transceiver(void* arg) {
-    TaskArgs* args = (TaskArgs*)arg;
-
-    DataLinkManager* obj = args->link_layer_obj;
-
-    if (obj == nullptr){
-        ESP_LOGE("thread", "bad pointer\n");
-        vTaskDelete(NULL); //should never get here
-    }
-
-    xTaskCreate(receive_frames, "receive_frame_job", 4096, arg, 5, NULL);
-
-    uint8_t dest_board_id = args->receiver_id; //using a dummy number for now - there is no board with id 2 right now
-    
-    const char* message = "FROM BEST BOARD";
-    
-    uint8_t curr_channel = args->task_id;
-    QueueHandle_t shared_queue = (QueueHandle_t)args->receive_queue;
-    
-    uint8_t send_buf[DATA_SIZE_TEST];
-    uint8_t recv_buf[DATA_SIZE_TEST];
-    memset(recv_buf, 0, DATA_SIZE_TEST);
-    memset(send_buf, 0, DATA_SIZE_TEST);
-    
-    size_t recv_len = 0;
-    uint8_t iteration = 0;
-    esp_err_t res;
-    
-    gptimer_handle_t gptimer = NULL;
-    gptimer_config_t timer_config = {
-        .clk_src = GPTIMER_CLK_SRC_DEFAULT,
-        .direction = GPTIMER_COUNT_UP,
-        .resolution_hz = 1 * 1000 * 1000, // 1MHz, 1 tick = 1us
-    };
-    ESP_ERROR_CHECK(gptimer_new_timer(&timer_config, &gptimer));
-    ESP_ERROR_CHECK(gptimer_enable(gptimer));
-    ESP_ERROR_CHECK(gptimer_start(gptimer));
-    uint64_t start_count = 0, end_count = 0;
-    
-    uint32_t num_incorrect = 0;
-    uint32_t total_transactions = 0;
-
-    RIPRow_public_matrix matrix[RIP_MAX_ROUTES];
-    size_t matrix_size = RIP_MAX_ROUTES;
-
-    for (int i = 0; i < RIP_MAX_ROUTES; i++){
-        RIPRow_public* table = (RIPRow_public*)pvPortMalloc(sizeof(RIPRow_public)*RIP_MAX_ROUTES);
-        matrix[i] = {
-            .table = table,
-            .size = RIP_MAX_ROUTES
-        };
-    }
-    
-    ReceviedFrame recv_frame = {};
-    printf("task %d starting...\n", curr_channel);
-    vTaskDelay(3000 /    portTICK_PERIOD_MS);
-
-    bool receive_only = false;
-
-    while(1){
-        if(!receive_only){
-            vTaskDelay(1000 /    portTICK_PERIOD_MS); // wait 1 second before trying to send again
-    
-            snprintf(reinterpret_cast<char*>(send_buf), sizeof(send_buf), "%s %d CH. %d", message, 4, curr_channel);
-            
-            // ESP_ERROR_CHECK(gptimer_get_raw_count(gptimer, &start_count));
-            res = obj->send(dest_board_id, send_buf, strlen(reinterpret_cast<char*>(send_buf)), FrameType::DEBUG_CONTROL_TYPE, 0x0);
-            // ESP_ERROR_CHECK(gptimer_get_raw_count(gptimer, &end_count));
-    
-            snprintf(reinterpret_cast<char*>(send_buf), sizeof(send_buf), "%s RANDOM", message); //modifying the data while it transmits shouldn't affect the actual transmission here
-            
-            if (res != ESP_OK){
-                ESP_LOGE("thread", "Failed to send message on thread %d", curr_channel);
-                continue;
-            } else {
-                printf("Successfully sent message\n");
-                // printf("Sent %zu B sized in %" PRIu64 " us\n", strlen(reinterpret_cast<char*>(send_buf)) + CONTROL_FRAME_OVERHEAD, end_count-start_count);
-            }
-        }
-        
-        if (receive_only){
-            //wait on a queue for a few ms (if there's nothing, just send another frame. otherwise pop from queue and read it)
-            if (xQueueReceive(shared_queue, (void*)&recv_frame, (TickType_t) 50) != pdPASS){
-                memset(send_buf, 0, 256);
-                continue; //nothing or failed to pop from queue
-            }
-    
-            res = obj->print_frame_info(recv_frame.buf, recv_frame.len, recv_buf);
-            
-            if (res != ESP_OK){
-                num_incorrect++;
-                printf("Received %ld bad frames on tx/rx round %ld for thread %d\n", num_incorrect, total_transactions, curr_channel);
-            } else {
-                printf("Received message %s on channel %d on round %ld. Total bad frames %ld\n", recv_buf, curr_channel, total_transactions, num_incorrect);
-            }
-        }
-        
-        total_transactions++;
-
-        iteration++;
-        if (iteration == 10){
-            iteration = 0;
-
-        memset(recv_buf, 0, DATA_SIZE_TEST);
-        memset(send_buf, 0, DATA_SIZE_TEST);
-        
-    }
-
-    while(true){
-        vTaskDelay(2000 / portTICK_PERIOD_MS);
-    }
-
-}
-
-void print_binary(unsigned char c) {
-    for (int i = 7; i >= 0; i--) {
-        printf("%d", (c >> i) & 1);
-    }
-}
-
-void print_string_binary(const char *str) {
-    while (*str) {
-        print_binary((unsigned char)*str);
-        printf(" "); // space between bytes for readability
-        str++;
-    }
-    printf("\n");
-}
-
-extern "C" [[noreturn]] void app_main(void) {
-    esp_err_t ret = nvs_flash_init();
-    if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
-        ESP_ERROR_CHECK(nvs_flash_erase());
-        ret = nvs_flash_init();
-    }
-    ESP_ERROR_CHECK(ret);
-
-    esp_netif_init();
-    esp_event_loop_create_default();
-
-    printf("finished esp init\n");
-
-    printf("Hello world!\n");
-    
-    // uint8_t iteration = 0;
-    // const char* message = "THIS IS A TEXT MESSAGE";
-    uint8_t num_channels = 1;
-    uint8_t board_id = 4;
-    std::unique_ptr<DataLinkManager> obj = std::make_unique<DataLinkManager>(board_id, num_channels);
-    
-    // uint8_t dest_board_id = 2; //using a dummy number for now - there is no board with id 2 right now
-    
-    // esp_err_t res;
-    
-    // uint8_t send_buf[256];
-    // uint8_t recv_buf[256];
-    // size_t recv_len = 0;
-    
-    // uint8_t curr_channel = 0;
-
-    DataLinkManager* obj_to_send = obj.release();
-    
-    TaskArgs args[4] = {};
-
-    for (uint8_t i = 0; i < num_channels; i++){
-        args[i].link_layer_obj = obj_to_send;
-        args[i].task_id = i;
-        args[i].receiver_id = 69;
-        args[i].receive_queue = xQueueCreate(10, sizeof(ReceviedFrame)); //queue storing up to 10 control frames
-        xTaskCreate(multi_transceiver, "multi_transceiver", 4096, static_cast<void*>(&args[i]), 5, NULL);
-        vTaskDelay(500 / portTICK_PERIOD_MS);
-    }
-
-    printf("Tasks have been created\n");
-
-    while(true){
-        //do nothing
-        vTaskDelay(1000 /    portTICK_PERIOD_MS);
-    }
-
-    for (int i = 5; i >= 0; i--) {
-        printf("Restarting in %d seconds...\n", i);
-        vTaskDelay(1000 /    portTICK_PERIOD_MS);
-    }
-    printf("Restarting now.\n");
-    fflush(stdout);
-    esp_restart();
-
-    while(true){
-        //dummy wait
-        vTaskDelay(2000 / portTICK_PERIOD_MS);
-    }
-}

main/main.cpp

@@ -16,5 +16,8 @@ extern "C" [[noreturn]] void app_main(void) {
     ESP_LOGI("MEM", "Free PSRAM: %d", heap_caps_get_free_size(MALLOC_CAP_SPIRAM));
 
     ConfigManager::init_config();
+
+    ConfigManager::set_module_type(ModuleType_SERVO_1);
+
     LoopManager::control_loop();
 }

main/main_rmt_timing.cpp

@@ -1,237 +0,0 @@
-#include <cstdio>
-#include <memory>
-
-#include "sdkconfig.h"
-#include "freertos/FreeRTOS.h"
-#include "freertos/task.h"
-#include "esp_flash.h"
-#include "nvs_flash.h"
-
-#include "RMTManager.h"
-
-#include <esp_netif.h>
-#include <esp_event.h>
-#include <freertos/semphr.h>
-
-#define BOARD_A_MESSAGE "MESSAGE FROM BOARD A"
-#define BOARD_B_MESSAGE "MESSAGE FROM BOARD B"
-
-#ifdef TIME_TEST
-    #include <inttypes.h>
-    #include "driver/gptimer.h"
-#endif //TIME_TEST
-
-// void rmt_task(void* arg) {
-//     vTaskDelay(pdMS_TO_TICKS(3000)); // wait 3 seconds to stabilize heap
-//     const auto obj = std::make_unique<RMTManager>();
-
-//     const char* message = "THIS IS A SAMPLE TEXT MESSAGE";
-//     rmt_transmit_config_t tx_config = {
-//         .loop_count = 0,
-//         .flags = {
-//             .eot_level = 0   // typically 0 or 1, depending on your output idle level
-//         }
-//     };
-
-//     int res = obj->send(message, strlen(message), &tx_config);
-
-//     if (res == ESP_OK){
-//         printf("Successfully sent '%s'\n", message);
-//     } else{
-//         printf("Failed to send '%s'\n", message);
-//     }
-
-//     vTaskDelete(NULL);
-// }
-
-void print_binary(unsigned char c) {
-    for (int i = 7; i >= 0; i--) {
-        printf("%d", (c >> i) & 1);
-    }
-}
-
-void print_string_binary(const char *str) {
-    while (*str) {
-        print_binary((unsigned char)*str);
-        printf(" "); // space between bytes for readability
-        str++;
-    }
-    printf("\n");
-}
-
-/**
- * @brief This main function shows the RMT TX and RX working by sending a message string in `message` over a GPIO pin and receiving on another pin
- * 
- */
-extern "C" [[noreturn]] void app_main(void) {
-    esp_err_t ret = nvs_flash_init();
-    if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
-        ESP_ERROR_CHECK(nvs_flash_erase());
-        ret = nvs_flash_init();
-    }
-    ESP_ERROR_CHECK(ret);
-
-    esp_netif_init();
-    esp_event_loop_create_default();
-
-    printf("finished esp init\n");
-
-    printf("Hello world!\n");
-
-    const auto obj = std::make_unique<RMTManager>();
-    
-    #ifdef TIME_TEST
-        gptimer_handle_t gptimer = NULL;
-        gptimer_config_t timer_config = {
-            .clk_src = GPTIMER_CLK_SRC_DEFAULT,
-            .direction = GPTIMER_COUNT_UP,
-            .resolution_hz = 1 * 1000 * 1000, // 1MHz, 1 tick = 1us
-        };
-        ESP_ERROR_CHECK(gptimer_new_timer(&timer_config, &gptimer));
-        ESP_ERROR_CHECK(gptimer_enable(gptimer));
-        ESP_ERROR_CHECK(gptimer_start(gptimer));
-        uint64_t start_count = 0, end_count = 0;
-        uint64_t sum = 0; //used to calculate the average send time
-        uint64_t num_iterations = 0;
-    #endif //TIME_TEST
-        
-    #ifdef BOARD_A
-        const char* message = BOARD_A_MESSAGE;
-    #elif BOARD_B
-        const char* message = BOARD_B_MESSAGE;
-    #else
-        const char* message = "THIS IS A SAMPLE TEXT MESSAGE";
-    #endif
-
-    #ifdef VERIFY_RECEIVE
-        uint64_t num_received = 0;
-        uint64_t num_corrupted = 0;
-    #endif //VERIFY_RECEIVE
-
-    // const char* message = "t";
-    rmt_transmit_config_t tx_config = {
-        .loop_count = 0,
-        .flags = {
-            .eot_level = 0   // typically 0 or 1, depending on your output idle level
-        }
-    };
-    
-    int res = ESP_OK;
-    
-    char recv_message[256];
-
-    // xTaskCreate(rmt_task, "rmt_task", 4096, NULL, 5, NULL);
-    while(true){
-        #ifndef TIME_TEST
-            printf("Starting RX receive\n");
-            res = obj->start_receiving();
-            if (res != ESP_OK){
-                printf("Something went wrong... terminating..\n");
-                continue;
-            }
-        #endif //TIME_TEST
-
-        printf("sending message %s - binary:\n", message);
-        print_string_binary(message);
-
-        #ifdef TIME_TEST
-            ESP_ERROR_CHECK(gptimer_get_raw_count(gptimer, &start_count));
-        #endif //TIME_TEST
-
-        res = obj->send(message, strlen(message), &tx_config); 
-        
-        if (res == ESP_OK){
-            // printf("Successfully started send job for message '%s'\n", message);
-        } else{
-            printf("Failed to start send job for message '%s'\n", message);
-            // continue; //do not continue on
-        }
-
-        res = obj->wait_until_send_complete(); //will wait until the the message is sent
-
-        #ifdef TIME_TEST
-            ESP_ERROR_CHECK(gptimer_get_raw_count(gptimer, &end_count));
-        #endif //TIME_TEST
-
-        if (res == ESP_OK){
-            #ifndef TIME_TEST
-            printf("Successfully sent message '%s'\n", message);
-            #else
-            printf("Sent %zu B sized message %s in %" PRIu64 " us on iteration %" PRIu64 "\n", strlen(message), message, end_count-start_count, num_iterations);
-            sum += (end_count - start_count);
-            #endif //TIME_TEST
-        } else{
-            printf("Failed to send '%s'\n", message);
-            continue;
-        }
-        
-        #ifndef TIME_TEST    
-            res = obj->receive(recv_message, sizeof(recv_message));
-            
-            if (res != 0){
-                printf("Failed to receive message\n");
-            } else {
-                printf("Received message %s\n", recv_message);
-            }
-            #ifdef VERIFY_RECEIVE
-                printf("Checking message for corruption on iteration %lld\n", num_received);
-                #ifdef BOARD_A
-                    //check if BOARD_B_MESSAGE was received correctly
-                    if (strcmp(recv_message, BOARD_B_MESSAGE) != 0){
-                        num_corrupted++;
-                    }
-                #elif BOARD_B
-                    if (strcmp(recv_message, BOARD_A_MESSAGE) != 0){
-                        num_corrupted++;
-                    }
-                #endif //BOARD_B
-
-                num_received++;
-
-            #endif //VERIFY_RECEIVE
-
-            memset(recv_message, 0, sizeof(recv_message));
-        #endif //TIME_TEST
-            
-        vTaskDelay(2000 / portTICK_PERIOD_MS);
-        #ifdef TIME_TEST
-        
-            num_iterations++;
-            if (num_iterations > 100){
-                break;
-            }
-        #endif //TIME_TEST
-
-        #ifdef VERIFY_RECEIVE
-            if (num_received > 100){
-                break;
-            }
-        #endif //VERIFY_RECEIVE
-    }
-
-    #ifdef TIME_TEST
-        float avg = (sum/num_iterations) / 1e6; //avg send time us to s
-        printf("Average Transmission Rate is: %.9f bits per second\n", (float)((strlen(message) * 8)/avg));
-        printf("Average sent time is: %.9f seconds\n", avg);
-    #endif //TIME_TEST
-
-    #ifdef VERIFY_RECEIVE
-        float avg_received_corrupted = (num_corrupted * 100) / (num_received-1);
-        printf("Average corruption rate is: %.6f %% \n", avg_received_corrupted);
-        printf("Total number of corrupted messages over %lld iterations is: %lld\n", num_received-1, num_corrupted);
-    #endif //VERIFY_RECEIVE
-
-    for (int i = 5; i >= 0; i--) {
-        printf("Restarting in %d seconds...\n", i);
-        vTaskDelay(1000 /    portTICK_PERIOD_MS);
-    }
-    printf("Restarting now.\n");
-    fflush(stdout);
-    esp_restart();
-
-    while(true){
-        //dummy wait
-        vTaskDelay(2000 / portTICK_PERIOD_MS);
-
-    }
-}