diff --git a/ff_drivers/CMakeLists.txt b/ff_drivers/CMakeLists.txt index ec08039..433c296 100644 --- a/ff_drivers/CMakeLists.txt +++ b/ff_drivers/CMakeLists.txt @@ -12,27 +12,27 @@ find_package(rclcpp REQUIRED) find_package(ff_msgs REQUIRED) find_package(ff_control REQUIRED) -add_library(pwm src/pwm.cpp) -target_include_directories(pwm PUBLIC +add_library(driver_lib src/pwm.cpp src/gpio.cpp) +target_include_directories(driver_lib PUBLIC $ $) -target_compile_features(pwm PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 -ament_target_dependencies(pwm rclcpp) +target_compile_features(driver_lib PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 +ament_target_dependencies(driver_lib rclcpp) -# nodes (TODO: needs to be updated) -# add_executable(thruster_node src/thruster_node.cpp) -# target_link_libraries(thruster_node pwm) -# ament_target_dependencies(thruster_node ff_msgs) +add_executable(thruster_node src/thruster_node.cpp) +target_link_libraries(thruster_node driver_lib) +ament_target_dependencies(thruster_node ff_msgs) # tests +# TODO(alvin): remove pwm stuff add_executable(test_single src/tests/test_single.cpp) -target_link_libraries(test_single pwm) +target_link_libraries(test_single driver_lib) add_executable(test_all_thrusters src/tests/test_all_thrusters.cpp) ament_target_dependencies(test_all_thrusters rclcpp ff_msgs ff_control) # install nodes -install(TARGETS test_single test_all_thrusters +install(TARGETS thruster_node test_single test_all_thrusters DESTINATION lib/${PROJECT_NAME}) # install launch files diff --git a/ff_drivers/include/ff_drivers/gpio.hpp b/ff_drivers/include/ff_drivers/gpio.hpp new file mode 100644 index 0000000..1330ee3 --- /dev/null +++ b/ff_drivers/include/ff_drivers/gpio.hpp @@ -0,0 +1,51 @@ +// MIT License +// +// Copyright (c) 2024 Stanford Autonomous Systems Lab +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + + +#pragma once + +#include +#include + +namespace ff +{ + +class GPIO +{ +public: + explicit GPIO(int pin); + ~GPIO(); + + void SetState(bool state); + void SetPolarity(bool normal = true); + bool GetState() const; + +private: + int pin_; + bool state_; + std::ofstream f_value_; + std::ofstream f_active_low_; + + std::string BasePath() const; +}; + +} // namespace ff diff --git a/ff_drivers/launch/hardware_bringup.launch.py b/ff_drivers/launch/hardware_bringup.launch.py index df549b8..f30f09f 100644 --- a/ff_drivers/launch/hardware_bringup.launch.py +++ b/ff_drivers/launch/hardware_bringup.launch.py @@ -53,7 +53,7 @@ def generate_launch_description(): # ASL optitrack IP and port parameters=[ { - "server": "192.168.1.8", + "server": "192.168.1.2", "port": 3883, } ], diff --git a/ff_drivers/src/gpio.cpp b/ff_drivers/src/gpio.cpp new file mode 100644 index 0000000..367b839 --- /dev/null +++ b/ff_drivers/src/gpio.cpp @@ -0,0 +1,87 @@ +// MIT License +// +// Copyright (c) 2024 Stanford Autonomous Systems Lab +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + + +#include "ff_drivers/gpio.hpp" + +namespace ff +{ + +GPIO::GPIO(int pin) +: pin_(pin) +{ + // request gpio device + { + std::ofstream f_export("/sys/class/gpio/export"); + f_export << pin; + } + + // configure GPIO to be an output + { + std::ofstream f_direction(BasePath() + "direction"); + f_direction << "out"; + } + + f_value_.open(BasePath() + "value"); + f_active_low_.open(BasePath() + "active_low"); + + SetPolarity(true); + SetState(false); +} + +GPIO::~GPIO() +{ + SetState(false); + + // close files + f_value_.close(); + f_active_low_.close(); + + // free gpio device + std::ofstream f_unexport("/sys/class/gpio/unexport"); + f_unexport << pin_; +} + +void GPIO::SetState(bool state) +{ + f_value_ << static_cast(state); + f_value_.flush(); + state_ = state; +} + +bool GPIO::GetState() const +{ + return state_; +} + +void GPIO::SetPolarity(bool normal) +{ + f_active_low_ << static_cast(!normal); + f_active_low_.flush(); +} + +std::string GPIO::BasePath() const +{ + return "/sys/class/gpio/gpio" + std::to_string(pin_) + "/"; +} + +} // namespace ff diff --git a/ff_drivers/src/thruster_node.cpp b/ff_drivers/src/thruster_node.cpp index 8e9fe7e..7a1e9a3 100644 --- a/ff_drivers/src/thruster_node.cpp +++ b/ff_drivers/src/thruster_node.cpp @@ -1,6 +1,6 @@ // MIT License // -// Copyright (c) 2023 Stanford Autonomous Systems Lab +// Copyright (c) 2024 Stanford Autonomous Systems Lab // // Permission is hereby granted, free of charge, to any person obtaining a copy // of this software and associated documentation files (the "Software"), to deal @@ -21,13 +21,12 @@ // SOFTWARE. -#include #include - +#include #include -#include "ff_drivers/pwm.hpp" -#include "ff_msgs/msg/thruster_pwm_command.hpp" +#include "ff_drivers/gpio.hpp" +#include "ff_msgs/msg/thruster_command.hpp" #define NUM_THRUSTERS 8 @@ -45,47 +44,36 @@ static constexpr int THRUSTER_PINS[NUM_THRUSTERS] = { }; -using namespace std::chrono_literals; -using ff_msgs::msg::ThrusterPWMCommand; +using ff_msgs::msg::ThrusterCommand; -class ThrusterNode : public ff::PWMManager +class ThrusterNode : public rclcpp::Node { public: ThrusterNode() - : ff::PWMManager("thruster_driver_node") + : rclcpp::Node("thruster_node") { - // add all PWMs + // initialize all GPIO for (size_t i = 0; i < NUM_THRUSTERS; ++i) { - this->AddSoftPWM(THRUSTER_PINS[i]); + gpios_.push_back(std::make_unique(THRUSTER_PINS[i])); } - // set period (default to 10Hz) - double period = this->declare_parameter("period", .1); - this->SetPeriodAll(period * 1s); - // update period on the fly - sub_params_ = std::make_shared(this); - cb_period_ = sub_params_->add_parameter_callback( - "period", - [this](const rclcpp::Parameter & p) {SetPeriodAll(p.as_double() * 1s);}); - - // start all PWMs - this->EnableAll(); - // listen to commands - sub_duty_cycle_ = this->create_subscription( - "commands/duty_cycle", - 10, [this](const ThrusterPWMCommand::SharedPtr msg) {DutyCycleCallback(msg);}); + sub_thruster_ = this->create_subscription( + "commands/binary_thrust", + 10, + [this](const ThrusterCommand::SharedPtr msg) {ThrusterCommandCallback(msg);}); } private: - std::shared_ptr sub_params_; - std::shared_ptr cb_period_; - rclcpp::Subscription::SharedPtr sub_duty_cycle_; + std::vector> gpios_; + rclcpp::Subscription::SharedPtr sub_thruster_; - void DutyCycleCallback(const ThrusterPWMCommand::SharedPtr msg) + void ThrusterCommandCallback(const ThrusterCommand::SharedPtr msg) { for (size_t i = 0; i < NUM_THRUSTERS; ++i) { - this->SetDutyCycle(i, msg->duty_cycles[i]); + if (gpios_[i]->GetState() != msg->switches[i]) { + gpios_[i]->SetState(msg->switches[i]); + } } } }; diff --git a/ff_drivers/src/thruster_pwm_node.cpp b/ff_drivers/src/thruster_pwm_node.cpp new file mode 100644 index 0000000..8e9fe7e --- /dev/null +++ b/ff_drivers/src/thruster_pwm_node.cpp @@ -0,0 +1,99 @@ +// MIT License +// +// Copyright (c) 2023 Stanford Autonomous Systems Lab +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. + + +#include +#include + +#include + +#include "ff_drivers/pwm.hpp" +#include "ff_msgs/msg/thruster_pwm_command.hpp" + +#define NUM_THRUSTERS 8 + +// thruster pin connection +// @see: https://wiki.odroid.com/odroid-n2l/application_note/gpio/pwm#tab__odroid-n2 +static constexpr int THRUSTER_PINS[NUM_THRUSTERS] = { + 476, // thruster pin 1 -> odroid pin 16 + 477, // thruster pin 2 -> odroid pin 18 + 484, // thruster pin 3 -> odroid pin 19 + 485, // thruster pin 4 -> odroid pin 21 + 478, // thruster pin 5 -> odroid pin 22 + 487, // thruster pin 6 -> odroid pin 23 + 486, // thruster pin 7 -> odroid pin 24 + 464, // thruster pin 8 -> odroid pin 26 +}; + + +using namespace std::chrono_literals; +using ff_msgs::msg::ThrusterPWMCommand; + +class ThrusterNode : public ff::PWMManager +{ +public: + ThrusterNode() + : ff::PWMManager("thruster_driver_node") + { + // add all PWMs + for (size_t i = 0; i < NUM_THRUSTERS; ++i) { + this->AddSoftPWM(THRUSTER_PINS[i]); + } + + // set period (default to 10Hz) + double period = this->declare_parameter("period", .1); + this->SetPeriodAll(period * 1s); + // update period on the fly + sub_params_ = std::make_shared(this); + cb_period_ = sub_params_->add_parameter_callback( + "period", + [this](const rclcpp::Parameter & p) {SetPeriodAll(p.as_double() * 1s);}); + + // start all PWMs + this->EnableAll(); + + // listen to commands + sub_duty_cycle_ = this->create_subscription( + "commands/duty_cycle", + 10, [this](const ThrusterPWMCommand::SharedPtr msg) {DutyCycleCallback(msg);}); + } + +private: + std::shared_ptr sub_params_; + std::shared_ptr cb_period_; + rclcpp::Subscription::SharedPtr sub_duty_cycle_; + + void DutyCycleCallback(const ThrusterPWMCommand::SharedPtr msg) + { + for (size_t i = 0; i < NUM_THRUSTERS; ++i) { + this->SetDutyCycle(i, msg->duty_cycles[i]); + } + } +}; + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::spin(std::make_shared()); + rclcpp::shutdown(); + return 0; +}