From 2fd85cac230279c284a29d99727da5526db6323c Mon Sep 17 00:00:00 2001 From: Tom Lonergan <43603330+TomLonergan03@users.noreply.github.com> Date: Tue, 2 Jul 2024 19:48:33 +0100 Subject: [PATCH] SNS - MQTT node for keyence (#125) --- config/pod.toml | 19 +++--- lib/navigation/control/navigator.cpp | 2 +- lib/sensors/CMakeLists.txt | 2 +- lib/sensors/accelerometer_node.cpp | 41 ++++++++++-- lib/sensors/accelerometer_node.hpp | 2 + lib/sensors/keyence_node.cpp | 93 ++++++++++++++++++++++++++++ lib/sensors/keyence_node.hpp | 31 ++++++++++ src/pod/CMakeLists.txt | 1 + src/pod/main.cpp | 8 +++ 9 files changed, 181 insertions(+), 18 deletions(-) create mode 100644 lib/sensors/keyence_node.cpp create mode 100644 lib/sensors/keyence_node.hpp diff --git a/config/pod.toml b/config/pod.toml index 8c5658bc..14d80207 100644 --- a/config/pod.toml +++ b/config/pod.toml @@ -9,23 +9,18 @@ host = "192.168.1.0" port = 4556 [raspberry] -nodes = ["state_machine", "mqtt_broker", "navigator"] +nodes = ["state_machine", "mqtt_broker", "navigator", "accelerometer", "keyence"] [state_machine] # Possible transition tables are "full_run", # "no_levitation", "levitation_only" transition_table = "full_run" -[some_sensors] -[some_sensors.accelerometer] -count = 5 -upper_limit = 100 -lower_limit = 0 -name = 'Acceleration' -topic = 'accelerometer' -format = 'float' -type = 'acceleration' -unit = 'm/s²' - [navigator] maxVelocity = 10 + +[accelerometer] +mux_channel = 3 + +[keyence] +pin = 4 diff --git a/lib/navigation/control/navigator.cpp b/lib/navigation/control/navigator.cpp index 92d94d81..d679be87 100644 --- a/lib/navigation/control/navigator.cpp +++ b/lib/navigation/control/navigator.cpp @@ -132,7 +132,7 @@ void Navigator::requestFailure() rapidjson::Value message_value; message_value.SetString(failure_message.c_str(), message_payload->GetAllocator()); - message_payload->AddMember("transition", message_value, message_payload->GetAllocator()); + message_payload->AddMember("state", message_value, message_payload->GetAllocator()); const core::MqttMessage::Header header{ .timestamp = static_cast(time_.now().time_since_epoch().count()), diff --git a/lib/sensors/CMakeLists.txt b/lib/sensors/CMakeLists.txt index ac30562d..6119846b 100644 --- a/lib/sensors/CMakeLists.txt +++ b/lib/sensors/CMakeLists.txt @@ -8,4 +8,4 @@ include_directories( "${CMAKE_SOURCE_DIR}/lib" "${CMAKE_CURRENT_SOURCE_DIR}" ) -target_link_libraries(${target} PUBLIC tomlplusplus::tomlplusplus) +target_link_libraries(${target} PUBLIC tomlplusplus::tomlplusplus hyped_io) diff --git a/lib/sensors/accelerometer_node.cpp b/lib/sensors/accelerometer_node.cpp index d4aa7e4c..864a392d 100644 --- a/lib/sensors/accelerometer_node.cpp +++ b/lib/sensors/accelerometer_node.cpp @@ -1,5 +1,5 @@ -#include "accelerometer.hpp" #include "accelerometer_node.hpp" +#include "i2c_mux.hpp" #include @@ -27,8 +27,20 @@ core::Result AccelerometerNode::startNode(toml::v3::node_view(); + if (!optional_mux_channel) { + logger.log(core::LogLevel::kFatal, "No mux channel specified"); + return core::Result::kFailure; + } + auto mux_channel = *optional_mux_channel; + auto optional_i2c_mux = I2cMux::create(logger, i2c, kDefaultMuxAddress, mux_channel); + if (!optional_i2c_mux) { + logger.log(core::LogLevel::kFatal, "Failed to create I2C mux"); + return core::Result::kFailure; + } + auto i2c_mux = *optional_i2c_mux; + auto optional_accelerometer = Accelerometer::create(logger, i2c_mux, accelerometerAddress::k1D); if (!optional_accelerometer) { logger.log(core::LogLevel::kFatal, "Failed to create accelerometer"); return core::Result::kFailure; @@ -50,11 +62,32 @@ AccelerometerNode::AccelerometerNode(core::Logger &logger, { } +void AccelerometerNode::requestFailure() +{ + std::string failure_message = "kFailure"; + const auto topic = core::MqttTopic::kState; + auto message_payload = std::make_shared(); + message_payload->SetObject(); + + rapidjson::Value message_value; + message_value.SetString(failure_message.c_str(), message_payload->GetAllocator()); + message_payload->AddMember("state", message_value, message_payload->GetAllocator()); + + const core::MqttMessage::Header header{ + .timestamp = static_cast(time_.now().time_since_epoch().count()), + .priority = core::MqttMessagePriority::kNormal}; + const core::MqttMessage message{topic, header, message_payload}; + mqtt_->publish(message, core::MqttMessageQos::kExactlyOnce); +} + void AccelerometerNode::run() { while (true) { auto result = accelerometer_->isValueReady(); - if (!result) { continue; } + if (!result) { + requestFailure(); + return; + } if (*result == core::Result::kFailure) { continue; } auto optional_acceration = accelerometer_->read(); if (!optional_acceration) { diff --git a/lib/sensors/accelerometer_node.hpp b/lib/sensors/accelerometer_node.hpp index 8135a8c9..83db83ea 100644 --- a/lib/sensors/accelerometer_node.hpp +++ b/lib/sensors/accelerometer_node.hpp @@ -20,6 +20,8 @@ class AccelerometerNode { void run(); private: + void requestFailure(); + core::Logger logger_; core::ITimeSource &time_; std::shared_ptr mqtt_; diff --git a/lib/sensors/keyence_node.cpp b/lib/sensors/keyence_node.cpp new file mode 100644 index 00000000..dc869c69 --- /dev/null +++ b/lib/sensors/keyence_node.cpp @@ -0,0 +1,93 @@ +#include "keyence.hpp" +#include "keyence_node.hpp" + +#include + +#include "core/logger.hpp" +#include "core/mqtt.hpp" +#include "core/wall_clock.hpp" +#include "io/hardware_gpio.hpp" + +namespace hyped::sensors { + +core::Result KeyenceNode::startNode(toml::v3::node_view config, + const std::string &mqtt_ip, + const std::uint32_t mqtt_port) +{ + auto time = core::WallClock(); + auto logger = core::Logger("Keyence", core::LogLevel::kDebug, time); + auto optional_mqtt = core::Mqtt::create(logger, "Keyence", mqtt_ip, mqtt_port); + if (!optional_mqtt) { + logger.log(core::LogLevel::kFatal, "Failed to create MQTT client"); + return core::Result::kFailure; + } + auto mqtt = *optional_mqtt; + auto gpio = std::make_shared(logger); + const auto pin = config["pin"].value(); + if (!pin) { + logger.log(core::LogLevel::kFatal, "No pin specified"); + return core::Result::kFailure; + } + auto optional_keyence = Keyence::create(logger, gpio, *pin); + if (!optional_keyence) { + logger.log(core::LogLevel::kFatal, "Failed to create keyence"); + return core::Result::kFailure; + } + auto keyence = *optional_keyence; + auto node = KeyenceNode(logger, time, mqtt, keyence); + node.run(); + return core::Result::kSuccess; +} + +KeyenceNode::KeyenceNode(core::Logger &logger, + core::ITimeSource &time, + std::shared_ptr mqtt, + std::shared_ptr keyence) + : logger_(logger), + time_(time), + mqtt_(std::move(mqtt)), + keyence_(std::move(keyence)) +{ +} + +void KeyenceNode::requestFailure() +{ + std::string failure_message = "kFailure"; + const auto topic = core::MqttTopic::kState; + auto message_payload = std::make_shared(); + message_payload->SetObject(); + + rapidjson::Value message_value; + message_value.SetString(failure_message.c_str(), message_payload->GetAllocator()); + message_payload->AddMember("state", message_value, message_payload->GetAllocator()); + + const core::MqttMessage::Header header{ + .timestamp = static_cast(time_.now().time_since_epoch().count()), + .priority = core::MqttMessagePriority::kNormal}; + const core::MqttMessage message{topic, header, message_payload}; + mqtt_->publish(message, core::MqttMessageQos::kExactlyOnce); +} + +void KeyenceNode::run() +{ + while (true) { + auto result = keyence_->updateStripeCount(); + if (result == core::Result::kFailure) { + requestFailure(); + return; + } + const auto stripe_count = keyence_->getStripeCount(); + const auto topic = core::MqttTopic::kKeyence; + auto message_payload = std::make_shared(); + message_payload->SetObject(); + rapidjson::Value stripe_count_json(stripe_count); + message_payload->AddMember("count", stripe_count_json, message_payload->GetAllocator()); + const core::MqttMessage::Header header{ + .timestamp = static_cast(time_.now().time_since_epoch().count()), + .priority = core::MqttMessagePriority::kNormal}; + const core::MqttMessage message{topic, header, message_payload}; + mqtt_->publish(message, core::MqttMessageQos::kExactlyOnce); + } +} + +} // namespace hyped::sensors diff --git a/lib/sensors/keyence_node.hpp b/lib/sensors/keyence_node.hpp new file mode 100644 index 00000000..9e9199b4 --- /dev/null +++ b/lib/sensors/keyence_node.hpp @@ -0,0 +1,31 @@ +#pragma once + +#include "keyence.hpp" + +#include "core/mqtt.hpp" +#include "core/types.hpp" +#include + +namespace hyped::sensors { + +class KeyenceNode { + public: + static core::Result startNode(toml::v3::node_view config, + const std::string &mqtt_ip, + const std::uint32_t mqtt_port); + KeyenceNode(core::Logger &logger, + core::ITimeSource &time, + std::shared_ptr mqtt, + std::shared_ptr keyence); + void run(); + + private: + void requestFailure(); + + core::Logger logger_; + core::ITimeSource &time_; + std::shared_ptr mqtt_; + std::shared_ptr keyence_; +}; + +} // namespace hyped::sensors diff --git a/src/pod/CMakeLists.txt b/src/pod/CMakeLists.txt index 02f5cc9d..1202a90d 100644 --- a/src/pod/CMakeLists.txt +++ b/src/pod/CMakeLists.txt @@ -14,5 +14,6 @@ target_link_libraries( hyped_navigation_control hyped_navigation_filters hyped_navigation_preprocessing + hyped_sensors tomlplusplus::tomlplusplus ) diff --git a/src/pod/main.cpp b/src/pod/main.cpp index 2359ec6c..4f780a9d 100644 --- a/src/pod/main.cpp +++ b/src/pod/main.cpp @@ -8,6 +8,8 @@ #include #include #include +#include +#include #include #include #include @@ -80,6 +82,12 @@ int main(int argc, char **argv) } else if (node_name == "navigation") { auto navigator_config = config["navigation"]; hyped::navigation::Navigator::startNode(navigator_config, mqtt_ip, mqtt_port); + } else if (node_name == "keyence") { + auto keyence_config = config["keyence"]; + hyped::sensors::KeyenceNode::startNode(keyence_config, mqtt_ip, mqtt_port); + } else if (node_name == "accelerometer") { + auto accelerometer_config = config["accelerometer"]; + hyped::sensors::AccelerometerNode::startNode(accelerometer_config, mqtt_ip, mqtt_port); } else { std::cerr << "Unknown node: " << node_name << "\n"; return 1;