diff --git a/components/constants/include/constants/module.h b/components/constants/include/constants/module.h index e6a79b6..394ae92 100644 --- a/components/constants/include/constants/module.h +++ b/components/constants/include/constants/module.h @@ -9,7 +9,7 @@ #include "flatbuffers_generated/RobotModule_generated.h" -inline std::unordered_map MODULE_TO_NUM_CHANNELS_MAP {{ModuleType_SPLITTER, 4}, {ModuleType_SERVO_1, 2}, {ModuleType_DC_MOTOR, 1}}; +inline std::unordered_map MODULE_TO_NUM_CHANNELS_MAP {{ModuleType_SPLITTER, 4}, {ModuleType_SERVO_1, 2}, {ModuleType_DC_MOTOR, 1}, {ModuleType_SERVO_2, 2}}; #define PC_ADDR 0 @@ -22,4 +22,9 @@ inline std::unordered_map MODULE_TO_NUM_CHANNELS_MAP {{ModuleType_SPLI #define DC_ENCODER_A 15 #define DC_ENCODER_B 16 +inline std::unordered_map CHANNEL_TO_0_DEG_MAP{{0, 9} }; +inline std::unordered_map CHANNEL_TO_90_DEG_MAP{{0, 7} }; +inline std::unordered_map CHANNEL_TO_180_DEG_MAP{{0, 8} }; +inline std::unordered_map CHANNEL_TO_270_DEG_MAP{{0, 10} }; + #endif //MODULE_H diff --git a/components/dataLink/DataLinkManager.cpp b/components/dataLink/DataLinkManager.cpp index d613db0..0c596c5 100644 --- a/components/dataLink/DataLinkManager.cpp +++ b/components/dataLink/DataLinkManager.cpp @@ -176,7 +176,7 @@ esp_err_t DataLinkManager::send(uint8_t dest_board, uint8_t* data, uint16_t data } }; - // ESP_LOGI(DEBUG_LINK_TAG, "Sending %d bytes", offset); + ESP_LOGI(DEBUG_LINK_TAG, "Sending %d bytes", offset); // print_buffer_binary(send_data, frame_size); uint8_t channel_to_route = MAX_CHANNELS; @@ -192,7 +192,7 @@ esp_err_t DataLinkManager::send(uint8_t dest_board, uint8_t* data, uint16_t data ESP_LOGE(DEBUG_LINK_TAG, "Failed to find entry for %d", new_frame.receiver_id); return ESP_FAIL; } - // ESP_LOGI(DEBUG_LINK_TAG, "Sending message to %d", new_frame.receiver_id); + ESP_LOGI(DEBUG_LINK_TAG, "Sending message to %d", new_frame.receiver_id); phys_comms->send(send_data, offset, &config, channel_to_route); } @@ -271,6 +271,10 @@ esp_err_t DataLinkManager::receive(uint8_t* data, size_t data_len, size_t* recv_ return ESP_FAIL; } + if (*recv_len < CONTROL_FRAME_OVERHEAD) { + return ESP_FAIL; + } + uint8_t* message = (uint8_t*)pvPortMalloc(CONTROL_FRAME_OVERHEAD + MAX_CONTROL_DATA_LEN); if (message == nullptr){ ESP_LOGE(DEBUG_LINK_TAG, "Failed to malloc for receive"); @@ -286,7 +290,7 @@ esp_err_t DataLinkManager::receive(uint8_t* data, size_t data_len, size_t* recv_ } *recv_len = message_size; memcpy(data, message, message_size); - // ESP_LOGI(DEBUG_LINK_TAG, "Received frame of type 0x%X destined for board %d", GET_TYPE(header.type_flag), header.receiver_id); + ESP_LOGI(DEBUG_LINK_TAG, "Received frame of type 0x%X destined for board %d", GET_TYPE(header.type_flag), header.receiver_id); //check for a rip frame if (static_cast(GET_TYPE(header.type_flag)) == FrameType::RIP_TABLE_CONTROL){ @@ -295,7 +299,7 @@ esp_err_t DataLinkManager::receive(uint8_t* data, size_t data_len, size_t* recv_ for (size_t i = 0; i < message_size-1; i+=2){ uint8_t board_id = message[i]; uint8_t hops = message[i+1]; - // ESP_LOGI(DEBUG_LINK_TAG, "Received: board_id %d and number of hops %d on channel %d", board_id, hops, curr_channel); + ESP_LOGI(DEBUG_LINK_TAG, "Received: board_id %d and number of hops %d on channel %d", board_id, hops, curr_channel); RIPRow* entry = nullptr; @@ -689,7 +693,7 @@ esp_err_t DataLinkManager::send_rip_frame(bool broadcast, uint8_t dest_id){ if (broadcast){ res = send(BROADCAST_ADDR, rip_message, message_idx, FrameType::RIP_TABLE_CONTROL, 0); } else { - // ESP_LOGI(DEBUG_LINK_TAG, "replying to discovery request to board %d", dest_id); + ESP_LOGI(DEBUG_LINK_TAG, "replying to discovery request to board %d", dest_id); res = send(dest_id, rip_message, message_idx, FrameType::RIP_TABLE_CONTROL, FLAG_DISCOVERY); } if (res != ESP_OK){ @@ -838,7 +842,7 @@ esp_err_t DataLinkManager::get_network_toplogy(RIPRow_public_matrix* matrix, siz while(true){ bool dummy; xQueueReceive(link_layer_obj->manual_broadcasts, &dummy, pdMS_TO_TICKS(RIP_BROADCAST_INTERVAL)); //wait up to RIP_BROADCAST_INTERVAL ms - // ESP_LOGI(DEBUG_LINK_TAG, "Broadcasting table..."); //debug + ESP_LOGI(DEBUG_LINK_TAG, "Broadcasting table..."); //debug res = link_layer_obj->send_rip_frame(true, 0); if (res != ESP_OK){ ESP_LOGE(DEBUG_LINK_TAG, "Failed to broadcast rip frame"); diff --git a/components/flatbuffers/include/flatbuffers_generated/RobotModule_generated.h b/components/flatbuffers/include/flatbuffers_generated/RobotModule_generated.h index 012ce86..087e271 100644 --- a/components/flatbuffers/include/flatbuffers_generated/RobotModule_generated.h +++ b/components/flatbuffers/include/flatbuffers_generated/RobotModule_generated.h @@ -7,11 +7,11 @@ #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"); +// // 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"); struct MotorState; struct MotorStateBuilder; @@ -24,33 +24,36 @@ enum ModuleType : int8_t { ModuleType_SERVO_1 = 1, ModuleType_DC_MOTOR = 2, ModuleType_BATTERY = 3, + ModuleType_SERVO_2 = 4, ModuleType_MIN = ModuleType_SPLITTER, - ModuleType_MAX = ModuleType_BATTERY + ModuleType_MAX = ModuleType_SERVO_2 }; -inline const ModuleType (&EnumValuesModuleType())[4] { +inline const ModuleType (&EnumValuesModuleType())[5] { static const ModuleType values[] = { ModuleType_SPLITTER, ModuleType_SERVO_1, ModuleType_DC_MOTOR, - ModuleType_BATTERY + ModuleType_BATTERY, + ModuleType_SERVO_2 }; return values; } inline const char * const *EnumNamesModuleType() { - static const char * const names[5] = { + static const char * const names[6] = { "SPLITTER", "SERVO_1", "DC_MOTOR", "BATTERY", + "SERVO_2", nullptr }; return names; } inline const char *EnumNameModuleType(ModuleType e) { - if (::flatbuffers::IsOutRange(e, ModuleType_SPLITTER, ModuleType_BATTERY)) return ""; + if (::flatbuffers::IsOutRange(e, ModuleType_SPLITTER, ModuleType_SERVO_2)) return ""; const size_t index = static_cast(e); return EnumNamesModuleType()[index]; } diff --git a/components/rmt/RMTManager.cpp b/components/rmt/RMTManager.cpp index 23d2008..d73c1cd 100644 --- a/components/rmt/RMTManager.cpp +++ b/components/rmt/RMTManager.cpp @@ -642,7 +642,7 @@ int RMTManager::convert_symbols_to_char(rmt_symbol_word_t* symbols, size_t num, } i++; } - printf("output_index %d\n", output_index); + // printf("output_index %d\n", output_index); return (int)output_index; } diff --git a/components/rmt/include/RMTManager.h b/components/rmt/include/RMTManager.h index 06197b0..d901cb2 100644 --- a/components/rmt/include/RMTManager.h +++ b/components/rmt/include/RMTManager.h @@ -114,7 +114,7 @@ class RMTManager{ //rx_receive_config rmt_receive_config_t receive_config = { - .signal_range_min_ns = 100, + .signal_range_min_ns = 200, .signal_range_max_ns = 200 * 1000, .flags = { .en_partial_rx = true diff --git a/components/rpc/CMakeLists.txt b/components/rpc/CMakeLists.txt index 6700259..cea7bd6 100644 --- a/components/rpc/CMakeLists.txt +++ b/components/rpc/CMakeLists.txt @@ -1,4 +1,4 @@ -idf_component_register(SRCS "WifiManager.cpp" "mDNSDiscoveryService.cpp" "TCPServer.cpp" "CommunicationRouter.cpp" "MessagingInterface.cpp" +idf_component_register(SRCS "WifiManager.cpp" "mDNSDiscoveryService.cpp" "TCPServer.cpp" "CommunicationRouter.cpp" "MessagingInterface.cpp" "OrientationDetection.cpp" PRIV_REQUIRES driver esp_event nvs_flash esp_netif esp_wifi espressif__mdns constants config flatbuffers dataLink rmt REQUIRES ptrQueue INCLUDE_DIRS "include") diff --git a/components/rpc/CommunicationRouter.cpp b/components/rpc/CommunicationRouter.cpp index 4024ace..3717aa2 100644 --- a/components/rpc/CommunicationRouter.cpp +++ b/components/rpc/CommunicationRouter.cpp @@ -5,13 +5,11 @@ #include "mDNSDiscoveryService.h" #include "MPIMessageBuilder.h" #include "WifiManager.h" - #include "freertos/FreeRTOS.h" #include "freertos/queue.h" - #include "Tables.h" - #include "PtrQueue.h" +#include "OrientationDetection.h" CommunicationRouter::~CommunicationRouter() { vTaskDelete(m_router_thread); @@ -139,9 +137,10 @@ std::pair, std::vector> CommunicationRouter::g } } - // todo: get orientation (temporary) - for (int i = 0; i < MAX_WIRED_CONNECTIONS; i++) { - connected_module_orientations[i] = Orientation_Deg0; + if (const auto id = connected_module_ids[0]; 0 == id) { + connected_module_orientations[0] = Orientation_Deg0; + } else { + connected_module_orientations[0] = OrientationDetection::get_orientation(0); } return { connected_module_ids, connected_module_orientations }; diff --git a/components/rpc/OrientationDetection.cpp b/components/rpc/OrientationDetection.cpp new file mode 100644 index 0000000..f7b26b4 --- /dev/null +++ b/components/rpc/OrientationDetection.cpp @@ -0,0 +1,46 @@ +// +// Created by Johnathon Slightham on 2025-07-26. +// + +#include +#include "OrientationDetection.h" + +#include +#include +#include + +void OrientationDetection::init() { + for (int i = 0; i < MODULE_TO_NUM_CHANNELS_MAP[ConfigManager::get_module_type()]; i++) { + setup_gpio(static_cast(CHANNEL_TO_0_DEG_MAP[i])); + setup_gpio(static_cast(CHANNEL_TO_90_DEG_MAP[i])); + setup_gpio(static_cast(CHANNEL_TO_180_DEG_MAP[i])); + setup_gpio(static_cast(CHANNEL_TO_270_DEG_MAP[i])); + } +} + +Orientation OrientationDetection::get_orientation(const uint8_t channel) { + if (gpio_get_level(static_cast(CHANNEL_TO_90_DEG_MAP[channel]))) { + std::cout << "90deg" << std::endl; + return Orientation_Deg90; + } else if (gpio_get_level(static_cast(CHANNEL_TO_180_DEG_MAP[channel]))) { + std::cout << "180deg" << std::endl; + return Orientation_Deg180; + } else if (gpio_get_level(static_cast(CHANNEL_TO_270_DEG_MAP[channel]))) { + std::cout << "270deg" << std::endl; + return Orientation_Deg270; + } else { + std::cout << "No orientation detected" << std::endl; + return Orientation_Deg0; + } +} + +void OrientationDetection::setup_gpio(const gpio_num_t pin) { + const gpio_config_t io_conf = { + .pin_bit_mask = (1ULL << pin), + .mode = GPIO_MODE_INPUT, + .pull_up_en = GPIO_PULLUP_DISABLE, + .pull_down_en = GPIO_PULLDOWN_ENABLE, + .intr_type = GPIO_INTR_DISABLE + }; + gpio_config(&io_conf); +} diff --git a/components/rpc/include/CommunicationRouter.h b/components/rpc/include/CommunicationRouter.h index 9127e56..2142cc0 100644 --- a/components/rpc/include/CommunicationRouter.h +++ b/components/rpc/include/CommunicationRouter.h @@ -9,6 +9,7 @@ #include #include #include +#include #include #include "freertos/FreeRTOS.h" @@ -40,6 +41,7 @@ public: m_pc_connection(std::move(pc_connection)), m_module_id(ConfigManager::get_module_id()), m_last_leader_updated(std::chrono::system_clock::now()){ + OrientationDetection::init(); update_leader(); xTaskCreate(router_thread, "communication_router", 4096, this, 3, &this->m_router_thread); diff --git a/components/rpc/include/OrientationDetection.h b/components/rpc/include/OrientationDetection.h new file mode 100644 index 0000000..c4ac2fa --- /dev/null +++ b/components/rpc/include/OrientationDetection.h @@ -0,0 +1,20 @@ +// +// Created by Johnathon Slightham on 2025-07-26. +// + +#ifndef ORIENTATIONDETECTION_H +#define ORIENTATIONDETECTION_H +#include +#include "driver/gpio.h" + +class OrientationDetection { +public: + static void init(); + static Orientation get_orientation(uint8_t channel); + +private: + static void setup_gpio(gpio_num_t pin); + +}; + +#endif //ORIENTATIONDETECTION_H diff --git a/main/control/ActuatorFactory.cpp b/main/control/ActuatorFactory.cpp index 989fb6a..e4358db 100644 --- a/main/control/ActuatorFactory.cpp +++ b/main/control/ActuatorFactory.cpp @@ -15,6 +15,8 @@ std::unique_ptr ActuatorFactory::create_actuator(const ModuleType typ switch (type) { case ModuleType_SERVO_1: return std::make_unique(); + case ModuleType_SERVO_2: + return std::make_unique(); case ModuleType_DC_MOTOR: return std::make_unique(); default: diff --git a/main/main.cpp b/main/main.cpp index 0014a47..9e03187 100644 --- a/main/main.cpp +++ b/main/main.cpp @@ -18,6 +18,7 @@ extern "C" [[noreturn]] void app_main(void) { ConfigManager::init_config(); + const auto loop_manager = std::make_unique(); xTaskCreate(reinterpret_cast(LoopManager::metadata_tx_loop), "metadata_tx", 3096, loop_manager.get(), 3, nullptr); loop_manager->control_loop();