From 38f4e20a648cf655c5d8607fc3d1690db059d7b1 Mon Sep 17 00:00:00 2001 From: Darko Lukic Date: Sun, 6 Aug 2023 13:27:34 +0200 Subject: [PATCH 1/4] add serial hardware interface --- firmware/arm/README.md | 17 +++ firmware/arm/arm.ino | 92 ++++++++++++++ spesbot_hardware/CMakeLists.txt | 31 +++++ .../serial_hardware_interface.hpp | 45 +++++++ .../serial_hardware_interface.xml | 3 + .../src/serial_hardware_interface.cpp | 120 ++++++++++++++++++ 6 files changed, 308 insertions(+) create mode 100644 firmware/arm/README.md create mode 100644 firmware/arm/arm.ino create mode 100644 spesbot_hardware/include/spesbot_hardware/serial_hardware_interface.hpp create mode 100644 spesbot_hardware/serial_hardware_interface.xml create mode 100644 spesbot_hardware/src/serial_hardware_interface.cpp diff --git a/firmware/arm/README.md b/firmware/arm/README.md new file mode 100644 index 0000000..2547aa0 --- /dev/null +++ b/firmware/arm/README.md @@ -0,0 +1,17 @@ +# Arm Firmware + +## Getting Started + +#### Install Arduino CLI and dependencies +```bash +curl -fsSL https://raw.githubusercontent.com/arduino/arduino-cli/master/install.sh | BINDIR=~/.local/bin sh +echo "~/.local/bin" >> ~/.bashrc && source ~/.bashrc +arduino-cli core update-index +arduino-cli core install arduino:avr +arduino-cli lib install circularbuffer servo +``` + +#### Compile and upload +```bash +arduino-cli compile --upload --port /dev/ttyUSB0 -b arduino:avr:mega +``` \ No newline at end of file diff --git a/firmware/arm/arm.ino b/firmware/arm/arm.ino new file mode 100644 index 0000000..352cf1e --- /dev/null +++ b/firmware/arm/arm.ino @@ -0,0 +1,92 @@ +#include +#include + +const int N_SERVOS = 6; +const int START_BYTE = 0x29; +const int MAX_MESSAGE_SIZE = 10; +const int CMD_WRITE_POSITION = 0x01; + +Servo servos[N_SERVOS]; + +CircularBuffer incomming_buffer; +enum State +{ + STATE_SEARCH_START_BYTE = 0, + STATE_WAIT_FOR_MESSAGE +}; +State state = STATE_SEARCH_START_BYTE; + +void setup() +{ + Serial.begin(115200); + + servos[0].attach(2); + servos[1].attach(3); + servos[2].attach(4); + servos[3].attach(5); + servos[4].attach(6); + servos[5].attach(7); +} + +void loop() +{ + update_communication(); +} + +int16_t unpack_int16(byte *buffer) +{ + int16_t value = 0; + value |= ((uint32_t)buffer[2] << 8); + value |= ((uint32_t)buffer[3] << 0); + return value; +} + +void update_communication() +{ + // Add data to the buffer + while (Serial.available() > 0) + incomming_buffer.push(Serial.read()); + + // Find the start byte + switch (state) + { + case STATE_SEARCH_START_BYTE: + if (!incomming_buffer.isEmpty() && incomming_buffer.first() != START_BYTE) + incomming_buffer.shift(); + else + state = STATE_WAIT_FOR_MESSAGE; + break; + case STATE_WAIT_FOR_MESSAGE: + if (incomming_buffer.size() >= 2 && incomming_buffer.size() >= incomming_buffer[1]) + { + on_full_message_received(); + state = STATE_SEARCH_START_BYTE; + } + if (incomming_buffer.size() >= 2 && (uint8_t)incomming_buffer[1] > MAX_MESSAGE_SIZE) + { + // Message too big, something is wrong. + incomming_buffer.shift(); + incomming_buffer.shift(); + state = STATE_SEARCH_START_BYTE; + } + break; + } +} + +void on_full_message_received() +{ + incomming_buffer.shift(); // start byte + incomming_buffer.shift(); // message size + incomming_buffer.shift(); // command type + + for (uint8_t i = 0; i < N_SERVOS; i++) + { + byte buffer[2]; + buffer[0] = incomming_buffer.shift(); + buffer[1] = incomming_buffer.shift(); + + const int16_t target_position = unpack_int16(buffer); + + servos[i].writeMicroseconds(target_position); + } +} \ No newline at end of file diff --git a/spesbot_hardware/CMakeLists.txt b/spesbot_hardware/CMakeLists.txt index 34736b0..e573b89 100644 --- a/spesbot_hardware/CMakeLists.txt +++ b/spesbot_hardware/CMakeLists.txt @@ -3,6 +3,37 @@ project(spesbot_hardware) # find dependencies find_package(ament_cmake REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(Boost REQUIRED) + +# Serial Hardware Interface +add_library( + serial_hardware_interface + SHARED + src/serial_hardware_interface.cpp +) +target_include_directories( + serial_hardware_interface + PRIVATE + include + ${Boost_INCLUDE_DIRS} +) +ament_target_dependencies( + serial_hardware_interface + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle +) +target_link_libraries(serial_hardware_interface ${Boost_LIBRARIES}) +pluginlib_export_plugin_description_file(hardware_interface serial_hardware_interface.xml) +install( + TARGETS serial_hardware_interface + DESTINATION lib +) # resources install( diff --git a/spesbot_hardware/include/spesbot_hardware/serial_hardware_interface.hpp b/spesbot_hardware/include/spesbot_hardware/serial_hardware_interface.hpp new file mode 100644 index 0000000..b9303c8 --- /dev/null +++ b/spesbot_hardware/include/spesbot_hardware/serial_hardware_interface.hpp @@ -0,0 +1,45 @@ +#ifndef SPESBOT_HARDWARE__SERIAL_HARDWARE_INTERFACE_HPP_ +#define SPESBOT_HARDWARE__SERIAL_HARDWARE_INTERFACE_HPP_ + +#include +#include + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" + +namespace spesbot_hardware +{ + class Joint + { + public: + double position_command; + std::string name; + uint8_t index; + double scale; + double offset; + }; + + class SerialHardwareInterface : public hardware_interface::SystemInterface + { + public: + SerialHardwareInterface(); + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_init(const hardware_interface::HardwareInfo &info) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate(const rclcpp_lifecycle::State &) override; + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State &) override; + std::vector export_state_interfaces() override; + std::vector export_command_interfaces() override; + hardware_interface::return_type read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override; + hardware_interface::return_type write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) override; + + private: + std::vector joints_; + boost::asio::io_service io_; + boost::asio::serial_port serial_; + + uint8_t max_position_command_index_{0}; + }; +} // namespace spesbot_hardware + +#endif // SPESBOT_HARDWARE__SERIAL_HARDWARE_INTERFACE_HPP_ \ No newline at end of file diff --git a/spesbot_hardware/serial_hardware_interface.xml b/spesbot_hardware/serial_hardware_interface.xml new file mode 100644 index 0000000..c81d53e --- /dev/null +++ b/spesbot_hardware/serial_hardware_interface.xml @@ -0,0 +1,3 @@ + + + \ No newline at end of file diff --git a/spesbot_hardware/src/serial_hardware_interface.cpp b/spesbot_hardware/src/serial_hardware_interface.cpp new file mode 100644 index 0000000..b670e0f --- /dev/null +++ b/spesbot_hardware/src/serial_hardware_interface.cpp @@ -0,0 +1,120 @@ +#include "spesbot_hardware/serial_hardware_interface.hpp" + +#include +#include + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace spesbot_hardware +{ + const uint8_t START_BYTE = 0x29; + const uint8_t HEADER_SIZE = 3; + const uint8_t POSITION_N_BYTES = 2; + const uint8_t CMD_WRITE_POSITION = 0x01; + + inline void pack_int16(uint8_t *buffer, int16_t value) + { + buffer[2] = (value >> 8) & 0xFF; + buffer[3] = value & 0xFF; + } + + SerialHardwareInterface::SerialHardwareInterface() : serial_(io_) + { + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn SerialHardwareInterface::on_init(const hardware_interface::HardwareInfo &info) + { + if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) + { + return CallbackReturn::ERROR; + } + + for (hardware_interface::ComponentInfo component : info.joints) + { + Joint joint; + joint.name = component.name; + joint.index = std::stoi(component.parameters.at("index")); + joint.scale = std::stof(component.parameters.at("scale")); + joint.offset = std::stof(component.parameters.at("offset")); + joints_.push_back(joint); + + if (joint.index > max_position_command_index_) + max_position_command_index_ = joint.index; + } + + return CallbackReturn::SUCCESS; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn SerialHardwareInterface::on_activate(const rclcpp_lifecycle::State &) + { + const std::string port = info_.hardware_parameters["port"]; + const int baudrate_value = std::stod(info_.hardware_parameters["baudrate"]); + + boost::system::error_code error_code; + serial_.open(port, error_code); + if (!error_code) + { + boost::asio::serial_port_base::baud_rate baudrate_option(baudrate_value); + serial_.set_option(baudrate_option); + + // Sleep for some time, so the Arduino has time to initialize + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + } + else + { + std::cerr << "ERROR: Cannot connect to " << port << " with error code " << error_code << "\n"; + return CallbackReturn::ERROR; + } + + return CallbackReturn::SUCCESS; + } + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn SerialHardwareInterface::on_deactivate(const rclcpp_lifecycle::State &) + { + return CallbackReturn::SUCCESS; + } + + std::vector SerialHardwareInterface::export_state_interfaces() + { + std::vector interfaces; + return interfaces; + } + + std::vector + SerialHardwareInterface::export_command_interfaces() + { + std::vector interfaces; + for (Joint &joint : joints_) + { + interfaces.emplace_back(hardware_interface::CommandInterface(joint.name, hardware_interface::HW_IF_POSITION, &joint.position_command)); + } + return interfaces; + } + + hardware_interface::return_type SerialHardwareInterface::read(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) + { + return hardware_interface::return_type::OK; + } + + hardware_interface::return_type SerialHardwareInterface::write(const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) + { + const int message_length = HEADER_SIZE + (max_position_command_index_ + 1) * POSITION_N_BYTES; + uint8_t position_command_buffer[message_length]; + + position_command_buffer[0] = START_BYTE; + position_command_buffer[1] = message_length; + position_command_buffer[2] = CMD_WRITE_POSITION; + + for (Joint &joint : joints_) + { + int16_t position = joint.position_command * joint.scale + joint.offset; + pack_int16(&position_command_buffer[HEADER_SIZE + joint.index * POSITION_N_BYTES], position); + boost::asio::write(serial_, boost::asio::buffer(position_command_buffer, message_length)); + } + return hardware_interface::return_type::OK; + } +} // namespace spesbot_hardware + +#include "pluginlib/class_list_macros.hpp" +PLUGINLIB_EXPORT_CLASS(spesbot_hardware::SerialHardwareInterface, hardware_interface::SystemInterface) From c77ae84ae7880a33cce8b522a674142b053e92a3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Darko=20Luki=C4=87?= Date: Mon, 7 Aug 2023 12:04:48 +0200 Subject: [PATCH 2/4] Update README.md --- firmware/arm/README.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/firmware/arm/README.md b/firmware/arm/README.md index 2547aa0..057bbb9 100644 --- a/firmware/arm/README.md +++ b/firmware/arm/README.md @@ -4,8 +4,8 @@ #### Install Arduino CLI and dependencies ```bash -curl -fsSL https://raw.githubusercontent.com/arduino/arduino-cli/master/install.sh | BINDIR=~/.local/bin sh -echo "~/.local/bin" >> ~/.bashrc && source ~/.bashrc +sudo snap install arduino-cli +sudo apt remove brltty arduino-cli core update-index arduino-cli core install arduino:avr arduino-cli lib install circularbuffer servo @@ -14,4 +14,4 @@ arduino-cli lib install circularbuffer servo #### Compile and upload ```bash arduino-cli compile --upload --port /dev/ttyUSB0 -b arduino:avr:mega -``` \ No newline at end of file +``` From 7a254b75670bf9eccda1b48da578fa12403a84b4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Darko=20Luki=C4=87?= Date: Mon, 7 Aug 2023 12:05:58 +0200 Subject: [PATCH 3/4] Update README.md --- firmware/arm/README.md | 1 + 1 file changed, 1 insertion(+) diff --git a/firmware/arm/README.md b/firmware/arm/README.md index 057bbb9..2a9db82 100644 --- a/firmware/arm/README.md +++ b/firmware/arm/README.md @@ -4,6 +4,7 @@ #### Install Arduino CLI and dependencies ```bash +sudo adduser $USER dialout sudo snap install arduino-cli sudo apt remove brltty arduino-cli core update-index From fd6314b0ca44db53ad703ce37f1efbaf1fdada35 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Darko=20Luki=C4=87?= Date: Mon, 7 Aug 2023 12:12:17 +0200 Subject: [PATCH 4/4] Update README.md --- firmware/arm/README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/firmware/arm/README.md b/firmware/arm/README.md index 2a9db82..0278427 100644 --- a/firmware/arm/README.md +++ b/firmware/arm/README.md @@ -5,7 +5,8 @@ #### Install Arduino CLI and dependencies ```bash sudo adduser $USER dialout -sudo snap install arduino-cli +curl -fsSL https://raw.githubusercontent.com/arduino/arduino-cli/master/install.sh | BINDIR=~/.local/bin sh +echo 'export PATH="$PATH:~/.local/bin"' >> ~/.bashrc && source ~/.bashrc sudo apt remove brltty arduino-cli core update-index arduino-cli core install arduino:avr