Skip to content

Commit

Permalink
Merge pull request #22 from StanfordASL/alvin/binary_thrust_driver
Browse files Browse the repository at this point in the history
change driver to work with binary thrust message
  • Loading branch information
alvinsunyixiao authored Feb 14, 2024
2 parents a62d895 + fb468aa commit 35c3a55
Show file tree
Hide file tree
Showing 7 changed files with 268 additions and 45 deletions.
20 changes: 10 additions & 10 deletions ff_drivers/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -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
$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
$<INSTALL_INTERFACE:include>)
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
Expand Down
51 changes: 51 additions & 0 deletions ff_drivers/include/ff_drivers/gpio.hpp
Original file line number Diff line number Diff line change
@@ -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 <fstream>
#include <string>

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
2 changes: 1 addition & 1 deletion ff_drivers/launch/hardware_bringup.launch.py
Original file line number Diff line number Diff line change
Expand Up @@ -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,
}
],
Expand Down
87 changes: 87 additions & 0 deletions ff_drivers/src/gpio.cpp
Original file line number Diff line number Diff line change
@@ -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<int>(state);
f_value_.flush();
state_ = state;
}

bool GPIO::GetState() const
{
return state_;
}

void GPIO::SetPolarity(bool normal)
{
f_active_low_ << static_cast<int>(!normal);
f_active_low_.flush();
}

std::string GPIO::BasePath() const
{
return "/sys/class/gpio/gpio" + std::to_string(pin_) + "/";
}

} // namespace ff
50 changes: 19 additions & 31 deletions ff_drivers/src/thruster_node.cpp
Original file line number Diff line number Diff line change
@@ -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
Expand All @@ -21,13 +21,12 @@
// SOFTWARE.


#include <chrono>
#include <memory>

#include <vector>
#include <rclcpp/rclcpp.hpp>

#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

Expand All @@ -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<ff::GPIO>(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<rclcpp::ParameterEventHandler>(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<ThrusterPWMCommand>(
"commands/duty_cycle",
10, [this](const ThrusterPWMCommand::SharedPtr msg) {DutyCycleCallback(msg);});
sub_thruster_ = this->create_subscription<ThrusterCommand>(
"commands/binary_thrust",
10,
[this](const ThrusterCommand::SharedPtr msg) {ThrusterCommandCallback(msg);});
}

private:
std::shared_ptr<rclcpp::ParameterEventHandler> sub_params_;
std::shared_ptr<rclcpp::ParameterCallbackHandle> cb_period_;
rclcpp::Subscription<ThrusterPWMCommand>::SharedPtr sub_duty_cycle_;
std::vector<std::unique_ptr<ff::GPIO>> gpios_;
rclcpp::Subscription<ThrusterCommand>::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]);
}
}
}
};
Expand Down
99 changes: 99 additions & 0 deletions ff_drivers/src/thruster_pwm_node.cpp
Original file line number Diff line number Diff line change
@@ -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 <chrono>
#include <memory>

#include <rclcpp/rclcpp.hpp>

#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<rclcpp::ParameterEventHandler>(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<ThrusterPWMCommand>(
"commands/duty_cycle",
10, [this](const ThrusterPWMCommand::SharedPtr msg) {DutyCycleCallback(msg);});
}

private:
std::shared_ptr<rclcpp::ParameterEventHandler> sub_params_;
std::shared_ptr<rclcpp::ParameterCallbackHandle> cb_period_;
rclcpp::Subscription<ThrusterPWMCommand>::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<ThrusterNode>());
rclcpp::shutdown();
return 0;
}
4 changes: 1 addition & 3 deletions ff_sim/ff_sim/simulator_node.py
Original file line number Diff line number Diff line change
Expand Up @@ -374,9 +374,7 @@ def f_dynamics_continuous_time(self, x, u):
f[0:2] = v
f[2] = thetadot
thetaddot = (M - F[1] * p0[0] + F[0] * p0[1]) / J
f[3:5] = np.matmul(
R, (F / m - (thetaddot * np.array([-p0[1], p0[0]]) - thetadot**2 * p0))
)
f[3:5] = np.matmul(R, (F / m - (thetaddot * np.array([-p0[1], p0[0]]) - thetadot**2 * p0)))
f[5] = thetaddot

# add constant force due to table tilt
Expand Down

0 comments on commit 35c3a55

Please sign in to comment.