From 0c29a541ce06d1001a818042d7bab2a23e602eaf Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Fri, 29 Aug 2025 15:38:26 +0900 Subject: [PATCH 01/37] api update in cpp linux 64bit Signed-off-by: Hyungyu Kim --- c++/build/linux64/Makefile | 13 + c++/include/dynamixel_sdk/dynamixel_api.hpp | 25 ++ .../dynamixel_sdk/dynamixel_api/connector.hpp | 57 +++ .../dynamixel_api/control_table.hpp | 45 +++ .../dynamixel_api/dynamixel_error.hpp | 119 ++++++ .../dynamixel_sdk/dynamixel_api/motor.hpp | 85 +++++ .../dynamixel_sdk/dynamixel_api/connector.cpp | 202 ++++++++++ .../dynamixel_api/control_table.cpp | 126 ++++++ .../dynamixel_api/dynamixel_error.cpp | 61 +++ c++/src/dynamixel_sdk/dynamixel_api/motor.cpp | 360 ++++++++++++++++++ control_table/2xc430_w250.model | 70 ++++ control_table/2xl430_w250.model | 70 ++++ control_table/dynamixel.model | 63 +++ control_table/h42_20_s300_ra.model | 84 ++++ control_table/h54_100_s500_ra.model | 84 ++++ control_table/h54_200_s500_ra.model | 84 ++++ control_table/m42_10_s260_ra.model | 84 ++++ control_table/m54_40_s250_ra.model | 84 ++++ control_table/m54_60_s250_ra.model | 84 ++++ control_table/mx_106.model | 75 ++++ control_table/mx_28.model | 71 ++++ control_table/mx_64.model | 75 ++++ control_table/ph42_020_s300.model | 87 +++++ control_table/ph54_100_s500.model | 87 +++++ control_table/ph54_200_s500.model | 87 +++++ control_table/pm42_010_s260.model | 87 +++++ control_table/pm54_040_s250.model | 87 +++++ control_table/pm54_060_s250.model | 87 +++++ control_table/rh_p12_rn.model | 80 ++++ control_table/xc330_m181.model | 75 ++++ control_table/xc330_m181_fw52.model | 75 ++++ control_table/xc330_m288.model | 75 ++++ control_table/xc330_m288_fw52.model | 75 ++++ control_table/xc330_t181.model | 75 ++++ control_table/xc330_t181_fw52.model | 75 ++++ control_table/xc330_t288.model | 75 ++++ control_table/xc330_t288_fw52.model | 75 ++++ control_table/xc430_w150.model | 70 ++++ control_table/xc430_w240.model | 70 ++++ control_table/xd430_t210.model | 74 ++++ control_table/xd430_t350.model | 74 ++++ control_table/xd540_t150.model | 80 ++++ control_table/xd540_t270.model | 80 ++++ control_table/xh430_v210.model | 74 ++++ control_table/xh430_v350.model | 74 ++++ control_table/xh430_w210.model | 74 ++++ control_table/xh430_w350.model | 74 ++++ control_table/xh540_v150.model | 80 ++++ control_table/xh540_v270.model | 80 ++++ control_table/xh540_w150.model | 80 ++++ control_table/xh540_w270.model | 80 ++++ control_table/xl320.model | 46 +++ control_table/xl330_m077.model | 75 ++++ control_table/xl330_m077_fw52.model | 75 ++++ control_table/xl330_m288.model | 75 ++++ control_table/xl330_m288_fw52.model | 75 ++++ control_table/xl430_w250.model | 70 ++++ control_table/xm430_w210.model | 74 ++++ control_table/xm430_w350.model | 74 ++++ control_table/xm540_w150.model | 80 ++++ control_table/xm540_w270.model | 80 ++++ control_table/xw430_t200.model | 73 ++++ control_table/xw430_t333.model | 73 ++++ control_table/xw540_h260.model | 73 ++++ control_table/xw540_t140.model | 73 ++++ control_table/xw540_t260.model | 73 ++++ control_table/ym070_210_a051.model | 115 ++++++ control_table/ym070_210_a099.model | 115 ++++++ control_table/ym070_210_b001.model | 115 ++++++ control_table/ym070_210_m001.model | 115 ++++++ control_table/ym070_210_r051.model | 115 ++++++ control_table/ym070_210_r099.model | 115 ++++++ control_table/ym080_230_a051.model | 115 ++++++ control_table/ym080_230_a099.model | 115 ++++++ control_table/ym080_230_b001.model | 115 ++++++ control_table/ym080_230_m001.model | 115 ++++++ control_table/ym080_230_r051.model | 115 ++++++ control_table/ym080_230_r099.model | 115 ++++++ 78 files changed, 6756 insertions(+) create mode 100644 c++/include/dynamixel_sdk/dynamixel_api.hpp create mode 100644 c++/include/dynamixel_sdk/dynamixel_api/connector.hpp create mode 100644 c++/include/dynamixel_sdk/dynamixel_api/control_table.hpp create mode 100644 c++/include/dynamixel_sdk/dynamixel_api/dynamixel_error.hpp create mode 100644 c++/include/dynamixel_sdk/dynamixel_api/motor.hpp create mode 100644 c++/src/dynamixel_sdk/dynamixel_api/connector.cpp create mode 100644 c++/src/dynamixel_sdk/dynamixel_api/control_table.cpp create mode 100644 c++/src/dynamixel_sdk/dynamixel_api/dynamixel_error.cpp create mode 100644 c++/src/dynamixel_sdk/dynamixel_api/motor.cpp create mode 100644 control_table/2xc430_w250.model create mode 100644 control_table/2xl430_w250.model create mode 100644 control_table/dynamixel.model create mode 100644 control_table/h42_20_s300_ra.model create mode 100644 control_table/h54_100_s500_ra.model create mode 100644 control_table/h54_200_s500_ra.model create mode 100644 control_table/m42_10_s260_ra.model create mode 100644 control_table/m54_40_s250_ra.model create mode 100644 control_table/m54_60_s250_ra.model create mode 100644 control_table/mx_106.model create mode 100644 control_table/mx_28.model create mode 100644 control_table/mx_64.model create mode 100644 control_table/ph42_020_s300.model create mode 100644 control_table/ph54_100_s500.model create mode 100644 control_table/ph54_200_s500.model create mode 100644 control_table/pm42_010_s260.model create mode 100644 control_table/pm54_040_s250.model create mode 100644 control_table/pm54_060_s250.model create mode 100644 control_table/rh_p12_rn.model create mode 100644 control_table/xc330_m181.model create mode 100644 control_table/xc330_m181_fw52.model create mode 100644 control_table/xc330_m288.model create mode 100644 control_table/xc330_m288_fw52.model create mode 100644 control_table/xc330_t181.model create mode 100644 control_table/xc330_t181_fw52.model create mode 100644 control_table/xc330_t288.model create mode 100644 control_table/xc330_t288_fw52.model create mode 100644 control_table/xc430_w150.model create mode 100644 control_table/xc430_w240.model create mode 100644 control_table/xd430_t210.model create mode 100644 control_table/xd430_t350.model create mode 100644 control_table/xd540_t150.model create mode 100644 control_table/xd540_t270.model create mode 100644 control_table/xh430_v210.model create mode 100644 control_table/xh430_v350.model create mode 100644 control_table/xh430_w210.model create mode 100644 control_table/xh430_w350.model create mode 100644 control_table/xh540_v150.model create mode 100644 control_table/xh540_v270.model create mode 100644 control_table/xh540_w150.model create mode 100644 control_table/xh540_w270.model create mode 100644 control_table/xl320.model create mode 100644 control_table/xl330_m077.model create mode 100644 control_table/xl330_m077_fw52.model create mode 100644 control_table/xl330_m288.model create mode 100644 control_table/xl330_m288_fw52.model create mode 100644 control_table/xl430_w250.model create mode 100644 control_table/xm430_w210.model create mode 100644 control_table/xm430_w350.model create mode 100644 control_table/xm540_w150.model create mode 100644 control_table/xm540_w270.model create mode 100644 control_table/xw430_t200.model create mode 100644 control_table/xw430_t333.model create mode 100644 control_table/xw540_h260.model create mode 100644 control_table/xw540_t140.model create mode 100644 control_table/xw540_t260.model create mode 100644 control_table/ym070_210_a051.model create mode 100644 control_table/ym070_210_a099.model create mode 100644 control_table/ym070_210_b001.model create mode 100644 control_table/ym070_210_m001.model create mode 100644 control_table/ym070_210_r051.model create mode 100644 control_table/ym070_210_r099.model create mode 100644 control_table/ym080_230_a051.model create mode 100644 control_table/ym080_230_a099.model create mode 100644 control_table/ym080_230_b001.model create mode 100644 control_table/ym080_230_m001.model create mode 100644 control_table/ym080_230_r051.model create mode 100644 control_table/ym080_230_r099.model diff --git a/c++/build/linux64/Makefile b/c++/build/linux64/Makefile index 12fe82a37..9df49c5a6 100644 --- a/c++/build/linux64/Makefile +++ b/c++/build/linux64/Makefile @@ -39,6 +39,7 @@ CCFLAGS = -O2 -O3 -DLINUX -D_GNU_SOURCE -Wall -c $(INCLUDES) $(FORMAT) -fPIC CXFLAGS = -O2 -O3 -DLINUX -D_GNU_SOURCE -Wall -c $(INCLUDES) $(FORMAT) -fPIC -g FORMAT = -m64 INCLUDES += -I$(DIR_DXL)/include/dynamixel_sdk +INCLUDES += -I$(DIR_DXL)/include #--------------------------------------------------------------------- # Required external libraries @@ -60,6 +61,10 @@ SOURCES = src/dynamixel_sdk/group_bulk_read.cpp \ src/dynamixel_sdk/protocol1_packet_handler.cpp \ src/dynamixel_sdk/protocol2_packet_handler.cpp \ src/dynamixel_sdk/port_handler_linux.cpp \ + src/dynamixel_sdk/dynamixel_api/connector.cpp \ + src/dynamixel_sdk/dynamixel_api/control_table.cpp \ + src/dynamixel_sdk/dynamixel_api/motor.cpp \ + src/dynamixel_sdk/dynamixel_api/dynamixel_error.cpp OBJECTS=$(addsuffix .o,$(addprefix $(DIR_OBJS)/,$(basename $(notdir $(SOURCES))))) @@ -89,6 +94,10 @@ install: $(TARGET) @$(CHK_DIR_EXISTS) $(INSTALL_ROOT)/include/dynamixel_sdk || $(MKDIR) $(INSTALL_ROOT)/include/dynamixel_sdk $(CP_ALL) $(DIR_DXL)/include/dynamixel_sdk/* $(INSTALL_ROOT)/include/dynamixel_sdk + # copy the control_table directory into the share directory + @$(CHK_DIR_EXISTS) $(INSTALL_ROOT)/share/dynamixel_sdk || $(MKDIR) -p $(INSTALL_ROOT)/share/dynamixel_sdk + $(CP_ALL) $(DIR_DXL)/../control_table $(INSTALL_ROOT)/share/dynamixel_sdk/ + $(LD_CONFIG) uninstall: @@ -99,6 +108,7 @@ uninstall: $(RM) $(INSTALL_ROOT)/include/dynamixel_sdk/dynamixel_sdk.h $(RM_ALL) $(INSTALL_ROOT)/include/dynamixel_sdk/* + $(RM_ALL) $(INSTALL_ROOT)/share/dynamixel_sdk/control_table reinstall: uninstall install @@ -119,6 +129,9 @@ $(DIR_OBJS)/%.o: $(DIR_DXL)/src/dynamixel_sdk/%.c $(DIR_OBJS)/%.o: $(DIR_DXL)/src/dynamixel_sdk/%.cpp $(CX) $(CXFLAGS) -c $? -o $@ +$(DIR_OBJS)/%.o: $(DIR_DXL)/src/dynamixel_sdk/dynamixel_api/%.cpp + $(CX) $(CXFLAGS) -c $? -o $@ + #--------------------------------------------------------------------- # END OF MAKEFILE #--------------------------------------------------------------------- diff --git a/c++/include/dynamixel_sdk/dynamixel_api.hpp b/c++/include/dynamixel_sdk/dynamixel_api.hpp new file mode 100644 index 000000000..c452733bb --- /dev/null +++ b/c++/include/dynamixel_sdk/dynamixel_api.hpp @@ -0,0 +1,25 @@ +// Copyright 2025 ROBOTIS CO., LTD. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Author: Hyungyu Kim + +#ifndef DYNAMIXEL_API_INCLUDE_DYNAMIXEL_API_HPP_ +#define DYNAMIXEL_API_INCLUDE_DYNAMIXEL_API_HPP_ + + +#include "dynamixel_sdk/dynamixel_api/connector.hpp" +#include "dynamixel_sdk/dynamixel_api/motor.hpp" +#include "dynamixel_sdk/dynamixel_api/dynamixel_error.hpp" + +#endif /* DYNAMIXEL_API_INCLUDE_DYNAMIXEL_API_HPP_ */ diff --git a/c++/include/dynamixel_sdk/dynamixel_api/connector.hpp b/c++/include/dynamixel_sdk/dynamixel_api/connector.hpp new file mode 100644 index 000000000..850982481 --- /dev/null +++ b/c++/include/dynamixel_sdk/dynamixel_api/connector.hpp @@ -0,0 +1,57 @@ +// Copyright 2025 ROBOTIS CO., LTD. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Author: Hyungyu Kim + +#ifndef DYNAMIXEL_SDK_DYNAMIXEL_API_CONNECTOR_HPP_ +#define DYNAMIXEL_SDK_DYNAMIXEL_API_CONNECTOR_HPP_ + +#include +#include +#include + +#include "dynamixel_sdk/dynamixel_sdk.h" +#include "dynamixel_sdk/dynamixel_api/dynamixel_error.hpp" +#include "dynamixel_sdk/dynamixel_api/motor.hpp" + + +namespace dynamixel +{ +class Connector +{ +public: + Connector(const std::string & port_name, float protocol_version, int baud_rate); + + virtual ~Connector(); + + std::shared_ptr getMotor(uint8_t id); + std::vector> getAllMotors(int start_id = 0, int end_id = 252); + Result write1ByteData(uint8_t id, uint16_t address, uint8_t value); + Result write2ByteData(uint8_t id, uint16_t address, uint16_t value); + Result write4ByteData(uint8_t id, uint16_t address, uint32_t value); + + Result read1ByteData(uint8_t id, uint16_t address); + Result read2ByteData(uint8_t id, uint16_t address); + Result read4ByteData(uint8_t id, uint16_t address); + + Result reboot(uint8_t id); + Result ping(uint8_t id); + Result factoryReset(uint8_t id, uint8_t option); + +private: + std::unique_ptr port_handler_; + std::unique_ptr packet_handler_; +}; +} // namespace dynamixel +#endif /* DYNAMIXEL_SDK_DYNAMIXEL_API_CONNECTOR_HPP_ */ diff --git a/c++/include/dynamixel_sdk/dynamixel_api/control_table.hpp b/c++/include/dynamixel_sdk/dynamixel_api/control_table.hpp new file mode 100644 index 000000000..3a1ecd837 --- /dev/null +++ b/c++/include/dynamixel_sdk/dynamixel_api/control_table.hpp @@ -0,0 +1,45 @@ +// Copyright 2025 ROBOTIS CO., LTD. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Author: Hyungyu Kim + +#ifndef DYNAMIXEL_SDK_DYNAMIXEL_API_CONTROL_TABLE_HPP_ +#define DYNAMIXEL_SDK_DYNAMIXEL_API_CONTROL_TABLE_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "dynamixel_sdk/dynamixel_api/dynamixel_error.hpp" + +namespace dynamixel +{ +struct ControlTableItem +{ + uint16_t address; + uint8_t size; +}; + +class ControlTable +{ +public: + static const std::map ParsingModelList(); + static const std::string getModelName(uint16_t model_number); + static const std::map & getControlTable(uint16_t model_number); +}; +} // namespace dynamixel +#endif /* DYNAMIXEL_SDK_DYNAMIXEL_API_CONTROL_TABLE_HPP_ */ diff --git a/c++/include/dynamixel_sdk/dynamixel_api/dynamixel_error.hpp b/c++/include/dynamixel_sdk/dynamixel_api/dynamixel_error.hpp new file mode 100644 index 000000000..86cf1481a --- /dev/null +++ b/c++/include/dynamixel_sdk/dynamixel_api/dynamixel_error.hpp @@ -0,0 +1,119 @@ +// Copyright 2025 ROBOTIS CO., LTD. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Author: Hyungyu Kim + +#ifndef DYNAMIXEL_SDK_DYNAMIXEL_API_DYNAMIXEL_ERROR_HPP_ +#define DYNAMIXEL_SDK_DYNAMIXEL_API_DYNAMIXEL_ERROR_HPP_ + +#include +#include +#include + +namespace dynamixel +{ +enum class DxlError +{ + SDK_COMM_SUCCESS = 0, // tx or rx packet communication success + SDK_COMM_PORT_BUSY = -1000, // Port is busy (in use) + SDK_COMM_TX_FAIL = -1001, // Failed transmit instruction packet + SDK_COMM_RX_FAIL = -1002, // Failed get status packet + SDK_COMM_TX_ERROR = -2000, // Incorrect instruction packet + SDK_COMM_RX_WAITING = -3000, // Now receiving status packet + SDK_COMM_RX_TIMEOUT = -3001, // There is no status packet + SDK_COMM_RX_CORRUPT = -3002, // Incorrect status packet + SDK_COMM_NOT_AVAILABLE = -9000, + SDK_ERRNUM_RESULT_FAIL = 1, // Failed to process the instruction packet. + SDK_ERRNUM_INSTRUCTION = 2, // Instruction error + SDK_ERRNUM_CRC = 3, // CRC check error + SDK_ERRNUM_DATA_RANGE = 4, // Data range error + SDK_ERRNUM_DATA_LENGTH = 5, // Data length error + SDK_ERRNUM_DATA_LIMIT = 6, // Data limit error + SDK_ERRNUM_ACCESS = 7, // Access error + API_FUNCTION_NOT_SUPPORTED = 11 // API does not support this function +}; + +template +class Result +{ +private: + std::variant result; + +public: + Result(const T & return_value) + : result(return_value) + {} + + Result(const E & error) + : result(error) + {} + + bool isSuccess() const + { + return std::holds_alternative(result); + } + + T & value() + { + if (!isSuccess()) {throw std::logic_error("Result has no value.");} + return std::get(result); + } + + E & error() + { + if (isSuccess()) {throw std::logic_error("Result has no error.");} + return std::get(result); + } +}; + +template +class Result +{ +private: + std::variant result; + +public: + Result() + : result(std::monostate{}) + {} + + Result(const E & error) + : result(error) + {} + + bool isSuccess() const + { + return std::holds_alternative(result); + } + + E & error() + { + if (!std::holds_alternative(result)) { + throw std::logic_error("Result has no error."); + } + return std::get(result); + } +}; + +class DxlRuntimeError : public std::runtime_error +{ +public: + explicit DxlRuntimeError(const std::string & message) + : std::runtime_error(message) {} +}; + +std::string getErrorMessage(DxlError error_code); +} // namespace dynamixel + +#endif /* DYNAMIXEL_SDK_DYNAMIXEL_API_DYNAMIXEL_ERROR_HPP_ */ diff --git a/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp b/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp new file mode 100644 index 000000000..5451fb6a2 --- /dev/null +++ b/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp @@ -0,0 +1,85 @@ +// Copyright 2025 ROBOTIS CO., LTD. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Author: Hyungyu Kim + +#ifndef DYNAMIXEL_SDK_DYNAMIXEL_API_MOTOR_HPP_ +#define DYNAMIXEL_SDK_DYNAMIXEL_API_MOTOR_HPP_ + +#include +#include +#include +#include + +#include "dynamixel_sdk/dynamixel_api/control_table.hpp" +#include "dynamixel_sdk/dynamixel_sdk.h" +#include "dynamixel_sdk/dynamixel_api/dynamixel_error.hpp" + +namespace dynamixel +{ +class Connector; + +class Motor +{ +public: + Motor(uint8_t id, uint16_t model_number, Connector * connector); + + virtual ~Motor(); + + Result enableTorque(); + Result disableTorque(); + Result setGoalPosition(uint32_t position); + Result setGoalVelocity(uint32_t velocity); + Result LEDOn(); + Result LEDOff(); + + Result ping(); + Result isTorqueOn(); + Result isLEDOn(); + Result getPresentPosition(); + Result getPresentVelocity(); + Result getMaxPositionLimit(); + Result getMinPositionLimit(); + Result getVelocityLimit(); + + Result changeID(uint8_t new_id); + Result changeBaudRate(int new_baud_rate); + Result setPositionControlMode(); + Result setVelocityControlMode(); + Result setCurrentControlMode(); + Result setTimeBasedProfile(); + Result setVelocityBasedProfile(); + Result setNormalDirection(); + Result setReverseDirection(); + + Result reboot(); + Result factoryResetAll(); + Result factoryResetExceptID(); + Result factoryResetExceptIDAndBaudRate(); + + uint8_t getID() const {return id_;} + uint16_t getModelNumber() const {return model_number_;} + std::string getModelName() const {return model_name_;} + +private: + Result getControlTableItem(const std::string & item_name); + uint8_t id_; + uint16_t model_number_; + std::string model_name_; + + Connector * connector_; + const std::map & control_table_; +}; +} // namespace dynamixel +#endif /* DYNAMIXEL_SDK_DYNAMIXEL_API_MOTOR_HPP_ */ diff --git a/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp b/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp new file mode 100644 index 000000000..be8359450 --- /dev/null +++ b/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp @@ -0,0 +1,202 @@ +// Copyright 2025 ROBOTIS CO., LTD. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Author: Hyungyu Kim + +#include "dynamixel_sdk/dynamixel_api/connector.hpp" + +namespace dynamixel +{ +Connector::Connector(const std::string & port_name, float protocol_version, int baud_rate) +{ + if (protocol_version != 2.0f){ + throw DxlRuntimeError("Only Protocol 2.0 is supported in this Dynamixel API."); + } + port_handler_ = std::unique_ptr(PortHandler::getPortHandler(port_name.c_str())); + packet_handler_ = std::unique_ptr( + PacketHandler::getPacketHandler(protocol_version)); + + if (!port_handler_->openPort()) { + throw DxlRuntimeError("Failed to open the port!"); + } + + if (!port_handler_->setBaudRate(baud_rate)) { + throw DxlRuntimeError("Failed to set the baud rate!"); + } +} + +Connector::~Connector() +{ + port_handler_->closePort(); +} + +std::shared_ptr Connector::getMotor(uint8_t id) +{ + Result result = ping(id); + if (!result.isSuccess()) { + throw DxlRuntimeError(getErrorMessage(result.error())); + } + return std::make_shared(id, result.value(), this); +} + +std::vector> Connector::getAllMotors(int start_id, int end_id) +{ + std::vector> motors; + for (uint8_t id = start_id; id <= end_id; ++id) { + try { + std::shared_ptr motor = getMotor(id); + motors.push_back(motor); + } catch (const DxlRuntimeError & e) { + continue; + } + } + return motors; +} + +Result Connector::read1ByteData(uint8_t id, uint16_t address) +{ + uint8_t dxl_error = 0; + uint8_t data = 0; + int dxl_comm_result = packet_handler_->read1ByteTxRx( + port_handler_.get(), + id, address, + &data, + &dxl_error); + if (dxl_comm_result != COMM_SUCCESS) { + return static_cast(dxl_comm_result); + } + if (dxl_error != 0) { + return static_cast(dxl_error); + } + return data; +} + +Result Connector::read2ByteData(uint8_t id, uint16_t address) +{ + uint8_t dxl_error = 0; + uint16_t data = 0; + int dxl_comm_result = packet_handler_->read2ByteTxRx( + port_handler_.get(), + id, address, + &data, + &dxl_error); + if (dxl_comm_result != COMM_SUCCESS) { + return static_cast(dxl_comm_result); + } + if (dxl_error != 0) { + return static_cast(dxl_error); + } + return data; +} + +Result Connector::read4ByteData(uint8_t id, uint16_t address) +{ + uint8_t dxl_error = 0; + uint32_t data = 0; + int dxl_comm_result = packet_handler_->read4ByteTxRx( + port_handler_.get(), + id, address, + &data, + &dxl_error); + if (dxl_comm_result != COMM_SUCCESS) { + return static_cast(dxl_comm_result); + } + if (dxl_error != 0) { + return static_cast(dxl_error); + } + return data; +} + +Result Connector::write1ByteData(uint8_t id, uint16_t address, uint8_t value) +{ + uint8_t dxl_error = 0; + int dxl_comm_result = packet_handler_->write1ByteTxRx( + port_handler_.get(), + id, address, value, + &dxl_error); + if (dxl_comm_result != COMM_SUCCESS) { + return static_cast(dxl_comm_result); + } + if (dxl_error != 0) { + return static_cast(dxl_error); + } + return {}; +} + +Result Connector::write2ByteData(uint8_t id, uint16_t address, uint16_t value) +{ + uint8_t dxl_error = 0; + int dxl_comm_result = packet_handler_->write2ByteTxRx( + port_handler_.get(), + id, address, + value, + &dxl_error); + if (dxl_comm_result != COMM_SUCCESS) { + return static_cast(dxl_comm_result); + } + if (dxl_error != 0) { + return static_cast(dxl_error); + } + return {}; +} + +Result Connector::write4ByteData(uint8_t id, uint16_t address, uint32_t value) +{ + uint8_t dxl_error = 0; + int dxl_comm_result = packet_handler_->write4ByteTxRx( + port_handler_.get(), + id, address, + value, + &dxl_error); + if (dxl_comm_result != COMM_SUCCESS) { + return static_cast(dxl_comm_result); + } + if (dxl_error != 0) { + return static_cast(dxl_error); + } + return {}; +} + +Result Connector::reboot(uint8_t id) +{ + uint8_t dxl_error = 0; + int dxl_comm_result = packet_handler_->reboot(port_handler_.get(), id, &dxl_error); + if (dxl_comm_result != COMM_SUCCESS) { + return static_cast(dxl_comm_result); + } + if (dxl_error != 0) { + return static_cast(dxl_error); + } + return {}; +} + +Result Connector::ping(uint8_t id) +{ + Result result = read2ByteData(id, 0); + return result; +} + +Result Connector::factoryReset(uint8_t id, uint8_t option) +{ + uint8_t dxl_error = 0; + int dxl_comm_result = packet_handler_->factoryReset(port_handler_.get(), id, option, &dxl_error); + if (dxl_comm_result != COMM_SUCCESS) { + return static_cast(dxl_comm_result); + } + if (dxl_error != 0) { + return static_cast(dxl_error); + } + return {}; +} +} // namespace dynamixel diff --git a/c++/src/dynamixel_sdk/dynamixel_api/control_table.cpp b/c++/src/dynamixel_sdk/dynamixel_api/control_table.cpp new file mode 100644 index 000000000..e0de7e7dd --- /dev/null +++ b/c++/src/dynamixel_sdk/dynamixel_api/control_table.cpp @@ -0,0 +1,126 @@ +// Copyright 2025 ROBOTIS CO., LTD. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Author: Hyungyu Kim + +#include + +#include "dynamixel_api/control_table.hpp" + +namespace dynamixel +{ +const std::map ControlTable::ParsingModelList() +{ + std::map tmp_model_list; + std::string file_name = "/usr/local/share/dynamixel_sdk/control_table/dynamixel.model"; + std::ifstream infile(file_name); + if (!infile.is_open()) { + throw std::runtime_error("Error: Could not open file " + file_name); + } + + std::string line; + std::getline(infile, line); + while (std::getline(infile, line)) { + if (line.empty()) { + continue; + } + std::stringstream ss(line); + std::string name; + std::string number_str; + if (std::getline(ss, number_str, '\t')) { + if (std::getline(ss, name)) { + try { + uint16_t number = std::stoi(number_str); + tmp_model_list[number] = name; + } catch (const std::invalid_argument & e) { + throw std::runtime_error("Invalid model number in " + file_name + ": " + number_str); + } catch (const std::out_of_range & e) { + throw std::runtime_error("Model number out of range in " + file_name + ": " + number_str); + } catch (...) { + throw std::runtime_error("Unknown error while parsing model number in " + file_name); + } + } + } + } + return tmp_model_list; +} + +const std::string ControlTable::getModelName(uint16_t model_number) +{ + static const std::map model_name_list_ = ParsingModelList(); + auto it = model_name_list_.find(model_number); + if (it != model_name_list_.end()) { + return it->second; + } else { + throw DxlRuntimeError( + "Model number is not found in dynamixel.model: " + std::to_string(model_number)); + } +} + +const std::map & ControlTable::getControlTable(uint16_t model_number) +{ + static std::map> control_tables_cache_; + if (control_tables_cache_.count(model_number)) { + return control_tables_cache_.at(model_number); + } + + const std::string & model_filename = getModelName(model_number); + std::string base_path = "/usr/local/share/dynamixel_sdk/control_table/"; + std::string full_path = base_path + model_filename; + + std::map control_table; + std::ifstream infile(full_path); + + if (!infile.is_open()) { + throw std::runtime_error("Error: Could not open model file: " + full_path); + } + + std::string line; + bool in_section = false; + + while (std::getline(infile, line)) { + if (line.empty()) { + continue; + } + if (line == "[control table]") { + in_section = true; + if (!std::getline(infile, line)) { + break; + } + continue; + } + + if (in_section) { + std::stringstream ss(line); + std::string address_str, size_str, name; + + if (std::getline(ss, address_str, '\t') && + std::getline(ss, size_str, '\t') && + std::getline(ss, name)) + { + try { + ControlTableItem item; + item.address = std::stoi(address_str); + item.size = std::stoi(size_str); + control_table[name] = item; + } catch (const std::exception & e) { + throw std::runtime_error("Error parsing control table item: " + line + " - " + e.what()); + } + } + } + } + auto result = control_tables_cache_.emplace(model_number, std::move(control_table)); + return result.first->second; +} +} // namespace dynamixel diff --git a/c++/src/dynamixel_sdk/dynamixel_api/dynamixel_error.cpp b/c++/src/dynamixel_sdk/dynamixel_api/dynamixel_error.cpp new file mode 100644 index 000000000..2cd07b89b --- /dev/null +++ b/c++/src/dynamixel_sdk/dynamixel_api/dynamixel_error.cpp @@ -0,0 +1,61 @@ +// Copyright 2025 ROBOTIS CO., LTD. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Author: Hyungyu Kim + +#include "dynamixel_sdk/dynamixel_api/dynamixel_error.hpp" + +namespace dynamixel +{ +std::string getErrorMessage(DxlError error_code) +{ + switch (error_code) { + case DxlError::SDK_COMM_SUCCESS: + return "[TxRxResult] Communication success."; + case DxlError::SDK_COMM_PORT_BUSY: + return "[TxRxResult] Port is in use!"; + case DxlError::SDK_COMM_TX_FAIL: + return "[TxRxResult] Failed transmit instruction packet!"; + case DxlError::SDK_COMM_RX_FAIL: + return "[TxRxResult] Failed get status packet from device!"; + case DxlError::SDK_COMM_TX_ERROR: + return "[TxRxResult] Incorrect instruction packet!"; + case DxlError::SDK_COMM_RX_WAITING: + return "[TxRxResult] Now recieving status packet!"; + case DxlError::SDK_COMM_RX_TIMEOUT: + return "[TxRxResult] There is no status packet!"; + case DxlError::SDK_COMM_RX_CORRUPT: + return "[TxRxResult] Incorrect status packet!"; + case DxlError::SDK_COMM_NOT_AVAILABLE: + return "[TxRxResult] Protocol does not support This function!"; + case DxlError::SDK_ERRNUM_RESULT_FAIL: + return "[RxPacketError] Failed to process the instruction packet!"; + case DxlError::SDK_ERRNUM_INSTRUCTION: + return "[RxPacketError] Undefined instruction or incorrect instruction!"; + case DxlError::SDK_ERRNUM_CRC: + return "[RxPacketError] CRC doesn't match!"; + case DxlError::SDK_ERRNUM_DATA_RANGE: + return "[RxPacketError] The data value is out of range!"; + case DxlError::SDK_ERRNUM_DATA_LENGTH: + return "[RxPacketError] The data length does not match as expected!"; + case DxlError::SDK_ERRNUM_DATA_LIMIT: + return "[RxPacketError] The data value exceeds the limit value!"; + case DxlError::SDK_ERRNUM_ACCESS: + return "[RxPacketError] Writing or Reading is not available to target address!"; + case DxlError::API_FUNCTION_NOT_SUPPORTED: + return "[APIError] API function is not supported on this model!"; + default: return "Unknown Error"; + } +} +} // namespace dynamixel diff --git a/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp b/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp new file mode 100644 index 000000000..0febd16ba --- /dev/null +++ b/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp @@ -0,0 +1,360 @@ +// Copyright 2025 ROBOTIS CO., LTD. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Author: Hyungyu Kim + +#include "dynamixel_api/motor.hpp" +#include "dynamixel_api/connector.hpp" + +namespace dynamixel +{ +Motor::Motor(uint8_t id, uint16_t model_number, Connector * connector) +: id_(id), + model_number_(model_number), + model_name_(ControlTable::getModelName(model_number)), + connector_(connector), + control_table_(ControlTable::getControlTable(model_number)) +{} + +Motor::~Motor() {} + +Result Motor::enableTorque() +{ + Result item_result = getControlTableItem("Torque Enable"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + Result result = connector_->write1ByteData(id_, item.address, 1); + return result; +} + +Result Motor::disableTorque() +{ + Result item_result = getControlTableItem("Torque Enable"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + Result result = connector_->write1ByteData(id_, item.address, 0); + return result; +} + +Result Motor::setGoalPosition(uint32_t position) +{ + Result item_result = getControlTableItem("Goal Position"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + if (item.size != 4) { + return DxlError::API_FUNCTION_NOT_SUPPORTED; + } + Result result = connector_->write4ByteData(id_, item.address, position); + return result; +} + +Result Motor::setGoalVelocity(uint32_t velocity) +{ + Result item_result = getControlTableItem("Goal Velocity"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + if (item.size != 4) { + return DxlError::API_FUNCTION_NOT_SUPPORTED; + } + Result result = connector_->write4ByteData(id_, item.address, velocity); + return result; +} + +Result Motor::LEDOn() +{ + Result item_result = getControlTableItem("LED"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + Result result = connector_->write1ByteData(id_, item.address, 1); + return result; +} + +Result Motor::LEDOff() +{ + Result item_result = getControlTableItem("LED"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + Result result = connector_->write1ByteData(id_, item.address, 0); + return result; +} + +Result Motor::ping() +{ + Result result = connector_->read2ByteData(id_, 0); + return result; +} + +Result Motor::isTorqueOn() +{ + Result item_result = getControlTableItem("Torque Enable"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + Result result = connector_->read1ByteData(id_, item.address); + return result; +} + +Result Motor::isLEDOn() +{ + Result item_result = getControlTableItem("LED"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + Result result = connector_->read1ByteData(id_, item.address); + return result; +} + +Result Motor::getPresentPosition() +{ + Result item_result = getControlTableItem("Present Position"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + Result result = connector_->read4ByteData(id_, item.address); + if (!result.isSuccess()) { + return result.error(); + } + return static_cast(result.value()); +} + +Result Motor::getPresentVelocity() +{ + Result item_result = getControlTableItem("Present Velocity"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + Result result = connector_->read4ByteData(id_, item.address); + if (!result.isSuccess()) { + return result.error(); + } + return static_cast(result.value()); +} + +Result Motor::getMaxPositionLimit() +{ + Result item_result = getControlTableItem("Max Position Limit"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + if (item.size != 4) { + return DxlError::API_FUNCTION_NOT_SUPPORTED; + } + Result result = connector_->read4ByteData(id_, item.address); + return result; +} + +Result Motor::getMinPositionLimit() +{ + Result item_result = getControlTableItem("Min Position Limit"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + if (item.size != 4) { + return DxlError::API_FUNCTION_NOT_SUPPORTED; + } + Result result = connector_->read4ByteData(id_, item.address); + return result; +} + +Result Motor::getVelocityLimit() +{ + Result item_result = getControlTableItem("Velocity Limit"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + if (item.size != 4) { + return DxlError::API_FUNCTION_NOT_SUPPORTED; + } + Result result = connector_->read4ByteData(id_, item.address); + return result; +} + +Result Motor::changeID(uint8_t new_id) +{ + Result item_result = getControlTableItem("ID"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + Result result = connector_->write1ByteData(id_, item.address, new_id); + if (result.isSuccess()) { + id_ = new_id; + } + return result; +} + +Result Motor::changeBaudRate(int new_baud_rate) +{ + Result item_result = getControlTableItem("Baud Rate"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + uint8_t baud_value = static_cast(2000000 / new_baud_rate - 1); + Result result = connector_->write1ByteData(id_, item.address, baud_value); + return result; +} + +Result Motor::setPositionControlMode() +{ + Result item_result = getControlTableItem("Operating Mode"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + Result result = connector_->write1ByteData(id_, item.address, 3); + return result; +} + +Result Motor::setVelocityControlMode() +{ + Result item_result = getControlTableItem("Operating Mode"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + Result result = connector_->write1ByteData(id_, item.address, 1); + return result; +} + +Result Motor::setCurrentControlMode() +{ + Result item_result = getControlTableItem("Operating Mode"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + Result result = connector_->write1ByteData(id_, item.address, 0); + return result; +} + +Result Motor::setTimeBasedProfile() +{ + Result item_result = getControlTableItem("Drive Mode"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + auto read_result = connector_->read1ByteData(id_, item.address); + if (!read_result.isSuccess()) { + return read_result.error(); + } + uint8_t drive_mode = read_result.value(); + drive_mode |= 0b00000100; + Result result = connector_->write1ByteData(id_, item.address, drive_mode); + return result; +} + +Result Motor::setVelocityBasedProfile() +{ + Result item_result = getControlTableItem("Drive Mode"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + auto read_result = connector_->read1ByteData(id_, item.address); + if (!read_result.isSuccess()) { + return read_result.error(); + } + uint8_t drive_mode = read_result.value(); + drive_mode &= 0b11111011; + Result result = connector_->write1ByteData(id_, item.address, drive_mode); + return result; +} + +Result Motor::setNormalDirection() +{ + Result item_result = getControlTableItem("Drive Mode"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + auto read_result = connector_->read1ByteData(id_, item.address); + if (!read_result.isSuccess()) { + return read_result.error(); + } + uint8_t drive_mode = read_result.value(); + drive_mode &= 0b11111110; + Result result = connector_->write1ByteData(id_, item.address, drive_mode); + return result; +} + +Result Motor::setReverseDirection() +{ + Result item_result = getControlTableItem("Drive Mode"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + auto read_result = connector_->read1ByteData(id_, item.address); + if (!read_result.isSuccess()) { + return read_result.error(); + } + uint8_t drive_mode = read_result.value(); + drive_mode |= 0b00000001; + Result result = connector_->write1ByteData(id_, item.address, drive_mode); + return result; +} + +Result Motor::reboot() +{ + Result result = connector_->reboot(id_); + return result; +} + +Result Motor::factoryResetAll() +{ + Result result = connector_->factoryReset(id_, 0xFF); + return result; +} + +Result Motor::factoryResetExceptID() +{ + Result result = connector_->factoryReset(id_, 0x01); + return result; +} + +Result Motor::factoryResetExceptIDAndBaudRate() +{ + Result result = connector_->factoryReset(id_, 0x02); + return result; +} + +Result Motor::getControlTableItem(const std::string & name) +{ + auto it = control_table_.find(name); + if (it == control_table_.end()) + return DxlError::API_FUNCTION_NOT_SUPPORTED; + return it->second; +} +} // namespace dynamixel diff --git a/control_table/2xc430_w250.model b/control_table/2xc430_w250.model new file mode 100644 index 000000000..deb48f6fb --- /dev/null +++ b/control_table/2xc430_w250.model @@ -0,0 +1,70 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Load +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/control_table/2xl430_w250.model b/control_table/2xl430_w250.model new file mode 100644 index 000000000..deb48f6fb --- /dev/null +++ b/control_table/2xl430_w250.model @@ -0,0 +1,70 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Load +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/control_table/dynamixel.model b/control_table/dynamixel.model new file mode 100644 index 000000000..b21f5dd9d --- /dev/null +++ b/control_table/dynamixel.model @@ -0,0 +1,63 @@ +Number Name +30 mx_28.model +311 mx_64.model +321 mx_106.model +350 xl320.model +1000 xh430_w350.model +1001 xd430_t350.model +1010 xh430_w210.model +1011 xd430_t210.model +1020 xm430_w350.model +1030 xm430_w210.model +1040 xh430_v350.model +1050 xh430_v210.model +1060 xl430_w250.model +1070 xc430_w150.model +1070 xc430_w150.model +1080 xc430_w240.model +1080 xc430_w240.model +1090 2xl430_w250.model +1100 xh540_w270.model +1101 xd540_t270.model +1110 xh540_w150.model +1111 xd540_t150.model +1120 xm540_w270.model +1130 xm540_w150.model +1140 xh540_v270.model +1150 xh540_v150.model +1160 2xc430_w250.model +1170 xw540_t260.model +1180 xw540_t140.model +1190 xl330_m077.model +1200 xl330_m288.model +1210 xc330_t181.model +1220 xc330_t288.model +1230 xc330_m181.model +1240 xc330_m288.model +1270 xw430_t333.model +1310 xw540_h260.model +2000 ph42_020_s300.model +2010 ph54_100_s500.model +2020 ph54_200_s500.model +2100 pm42_010_s260.model +2110 pm54_040_s250.model +2120 pm54_060_s250.model +4000 ym070_210_m001.model +4010 ym070_210_b001.model +4020 ym070_210_r051.model +4030 ym070_210_r099.model +4040 ym070_210_a051.model +4050 ym070_210_a099.model +4120 ym080_230_m001.model +4130 ym080_230_b001.model +4140 ym080_230_r051.model +4150 ym080_230_r099.model +4160 ym080_230_a051.model +4170 ym080_230_a099.model +35074 rh_p12_rn.model +43289 m42_10_s260_ra.model +46097 m54_40_s250_ra.model +46353 m54_60_s250_ra.model +51201 h42_20_s300_ra.model +53769 h54_100_s500_ra.model +54025 h54_200_s500_ra.model diff --git a/control_table/h42_20_s300_ra.model b/control_table/h42_20_s300_ra.model new file mode 100644 index 000000000..8dff9987a --- /dev/null +++ b/control_table/h42_20_s300_ra.model @@ -0,0 +1,84 @@ +[type info] +name value +value_of_zero_radian_position 0 +value_of_max_radian_position 303750 +value_of_min_radian_position -303750 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary ID +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +40 4 Acceleration Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +56 1 External Port Mode 1 +57 1 External Port Mode 2 +58 1 External Port Mode 3 +59 1 External Port Mode 4 +63 1 Shutdown +512 1 Torque Enable +513 1 LED Red +514 1 LED Green +515 1 LED Blue +516 1 Status Return Level +517 1 Registered Instruction +518 1 Hardware Error Status +524 2 Velocity I Gain +526 2 Velocity P Gain +528 2 Position D Gain +530 2 Position I Gain +532 2 Position P Gain +536 2 Feedforward 2nd Gain +538 2 Feedforward 1st Gain +546 1 Bus Watchdog +548 2 Goal PWM +550 2 Goal Current +552 4 Goal Velocity +556 4 Profile Acceleration +560 4 Profile Velocity +564 4 Goal Position +568 2 Realtime Tick +570 1 Moving +571 1 Moving Status +572 2 Present PWM +574 2 Present Current +576 4 Present Velocity +580 4 Present Position +584 4 Velocity Trajectory +588 4 Position Trajectory +592 2 Present Input Voltage +594 1 Present Temperature +600 2 External Port Data 1 +602 2 External Port Data 2 +604 2 External Port Data 3 +606 2 External Port Data 4 +168 2 Indirect Address 1 +634 1 Indirect Data 1 +168 2 Indirect Address Write +634 1 Indirect Data Write +296 2 Indirect Address Read +698 1 Indirect Data Read diff --git a/control_table/h54_100_s500_ra.model b/control_table/h54_100_s500_ra.model new file mode 100644 index 000000000..d75df80b8 --- /dev/null +++ b/control_table/h54_100_s500_ra.model @@ -0,0 +1,84 @@ +[type info] +name value +value_of_zero_radian_position 0 +value_of_max_radian_position 501923 +value_of_min_radian_position -501923 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary ID +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +40 4 Acceleration Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +56 1 External Port Mode 1 +57 1 External Port Mode 2 +58 1 External Port Mode 3 +59 1 External Port Mode 4 +63 1 Shutdown +512 1 Torque Enable +513 1 LED Red +514 1 LED Green +515 1 LED Blue +516 1 Status Return Level +517 1 Registered Instruction +518 1 Hardware Error Status +524 2 Velocity I Gain +526 2 Velocity P Gain +528 2 Position D Gain +530 2 Position I Gain +532 2 Position P Gain +536 2 Feedforward 2nd Gain +538 2 Feedforward 1st Gain +546 1 Bus Watchdog +548 2 Goal PWM +550 2 Goal Current +552 4 Goal Velocity +556 4 Profile Acceleration +560 4 Profile Velocity +564 4 Goal Position +568 2 Realtime Tick +570 1 Moving +571 1 Moving Status +572 2 Present PWM +574 2 Present Current +576 4 Present Velocity +580 4 Present Position +584 4 Velocity Trajectory +588 4 Position Trajectory +592 2 Present Input Voltage +594 1 Present Temperature +600 2 External Port Data 1 +602 2 External Port Data 2 +604 2 External Port Data 3 +606 2 External Port Data 4 +168 2 Indirect Address 1 +634 1 Indirect Data 1 +168 2 Indirect Address Write +634 1 Indirect Data Write +296 2 Indirect Address Read +698 1 Indirect Data Read diff --git a/control_table/h54_200_s500_ra.model b/control_table/h54_200_s500_ra.model new file mode 100644 index 000000000..d75df80b8 --- /dev/null +++ b/control_table/h54_200_s500_ra.model @@ -0,0 +1,84 @@ +[type info] +name value +value_of_zero_radian_position 0 +value_of_max_radian_position 501923 +value_of_min_radian_position -501923 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary ID +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +40 4 Acceleration Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +56 1 External Port Mode 1 +57 1 External Port Mode 2 +58 1 External Port Mode 3 +59 1 External Port Mode 4 +63 1 Shutdown +512 1 Torque Enable +513 1 LED Red +514 1 LED Green +515 1 LED Blue +516 1 Status Return Level +517 1 Registered Instruction +518 1 Hardware Error Status +524 2 Velocity I Gain +526 2 Velocity P Gain +528 2 Position D Gain +530 2 Position I Gain +532 2 Position P Gain +536 2 Feedforward 2nd Gain +538 2 Feedforward 1st Gain +546 1 Bus Watchdog +548 2 Goal PWM +550 2 Goal Current +552 4 Goal Velocity +556 4 Profile Acceleration +560 4 Profile Velocity +564 4 Goal Position +568 2 Realtime Tick +570 1 Moving +571 1 Moving Status +572 2 Present PWM +574 2 Present Current +576 4 Present Velocity +580 4 Present Position +584 4 Velocity Trajectory +588 4 Position Trajectory +592 2 Present Input Voltage +594 1 Present Temperature +600 2 External Port Data 1 +602 2 External Port Data 2 +604 2 External Port Data 3 +606 2 External Port Data 4 +168 2 Indirect Address 1 +634 1 Indirect Data 1 +168 2 Indirect Address Write +634 1 Indirect Data Write +296 2 Indirect Address Read +698 1 Indirect Data Read diff --git a/control_table/m42_10_s260_ra.model b/control_table/m42_10_s260_ra.model new file mode 100644 index 000000000..54a22a351 --- /dev/null +++ b/control_table/m42_10_s260_ra.model @@ -0,0 +1,84 @@ +[type info] +name value +value_of_zero_radian_position 0 +value_of_max_radian_position 263187 +value_of_min_radian_position -263187 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary ID +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +40 4 Acceleration Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +56 1 External Port Mode 1 +57 1 External Port Mode 2 +58 1 External Port Mode 3 +59 1 External Port Mode 4 +63 1 Shutdown +512 1 Torque Enable +513 1 LED Red +514 1 LED Green +515 1 LED Blue +516 1 Status Return Level +517 1 Registered Instruction +518 1 Hardware Error Status +524 2 Velocity I Gain +526 2 Velocity P Gain +528 2 Position D Gain +530 2 Position I Gain +532 2 Position P Gain +536 2 Feedforward 2nd Gain +538 2 Feedforward 1st Gain +546 1 Bus Watchdog +548 2 Goal PWM +550 2 Goal Current +552 4 Goal Velocity +556 4 Profile Acceleration +560 4 Profile Velocity +564 4 Goal Position +568 2 Realtime Tick +570 1 Moving +571 1 Moving Status +572 2 Present PWM +574 2 Present Current +576 4 Present Velocity +580 4 Present Position +584 4 Velocity Trajectory +588 4 Position Trajectory +592 2 Present Input Voltage +594 1 Present Temperature +600 2 External Port Data 1 +602 2 External Port Data 2 +604 2 External Port Data 3 +606 2 External Port Data 4 +168 2 Indirect Address 1 +634 1 Indirect Data 1 +168 2 Indirect Address Write +634 1 Indirect Data Write +296 2 Indirect Address Read +698 1 Indirect Data Read diff --git a/control_table/m54_40_s250_ra.model b/control_table/m54_40_s250_ra.model new file mode 100644 index 000000000..a076cb27c --- /dev/null +++ b/control_table/m54_40_s250_ra.model @@ -0,0 +1,84 @@ +[type info] +name value +value_of_zero_radian_position 0 +value_of_max_radian_position 251417 +value_of_min_radian_position -251417 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary ID +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +40 4 Acceleration Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +56 1 External Port Mode 1 +57 1 External Port Mode 2 +58 1 External Port Mode 3 +59 1 External Port Mode 4 +63 1 Shutdown +512 1 Torque Enable +513 1 LED Red +514 1 LED Green +515 1 LED Blue +516 1 Status Return Level +517 1 Registered Instruction +518 1 Hardware Error Status +524 2 Velocity I Gain +526 2 Velocity P Gain +528 2 Position D Gain +530 2 Position I Gain +532 2 Position P Gain +536 2 Feedforward 2nd Gain +538 2 Feedforward 1st Gain +546 1 Bus Watchdog +548 2 Goal PWM +550 2 Goal Current +552 4 Goal Velocity +556 4 Profile Acceleration +560 4 Profile Velocity +564 4 Goal Position +568 2 Realtime Tick +570 1 Moving +571 1 Moving Status +572 2 Present PWM +574 2 Present Current +576 4 Present Velocity +580 4 Present Position +584 4 Velocity Trajectory +588 4 Position Trajectory +592 2 Present Input Voltage +594 1 Present Temperature +600 2 External Port Data 1 +602 2 External Port Data 2 +604 2 External Port Data 3 +606 2 External Port Data 4 +168 2 Indirect Address 1 +634 1 Indirect Data 1 +168 2 Indirect Address Write +634 1 Indirect Data Write +296 2 Indirect Address Read +698 1 Indirect Data Read diff --git a/control_table/m54_60_s250_ra.model b/control_table/m54_60_s250_ra.model new file mode 100644 index 000000000..a076cb27c --- /dev/null +++ b/control_table/m54_60_s250_ra.model @@ -0,0 +1,84 @@ +[type info] +name value +value_of_zero_radian_position 0 +value_of_max_radian_position 251417 +value_of_min_radian_position -251417 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary ID +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +40 4 Acceleration Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +56 1 External Port Mode 1 +57 1 External Port Mode 2 +58 1 External Port Mode 3 +59 1 External Port Mode 4 +63 1 Shutdown +512 1 Torque Enable +513 1 LED Red +514 1 LED Green +515 1 LED Blue +516 1 Status Return Level +517 1 Registered Instruction +518 1 Hardware Error Status +524 2 Velocity I Gain +526 2 Velocity P Gain +528 2 Position D Gain +530 2 Position I Gain +532 2 Position P Gain +536 2 Feedforward 2nd Gain +538 2 Feedforward 1st Gain +546 1 Bus Watchdog +548 2 Goal PWM +550 2 Goal Current +552 4 Goal Velocity +556 4 Profile Acceleration +560 4 Profile Velocity +564 4 Goal Position +568 2 Realtime Tick +570 1 Moving +571 1 Moving Status +572 2 Present PWM +574 2 Present Current +576 4 Present Velocity +580 4 Present Position +584 4 Velocity Trajectory +588 4 Position Trajectory +592 2 Present Input Voltage +594 1 Present Temperature +600 2 External Port Data 1 +602 2 External Port Data 2 +604 2 External Port Data 3 +606 2 External Port Data 4 +168 2 Indirect Address 1 +634 1 Indirect Data 1 +168 2 Indirect Address Write +634 1 Indirect Data Write +296 2 Indirect Address Read +698 1 Indirect Data Read diff --git a/control_table/mx_106.model b/control_table/mx_106.model new file mode 100644 index 000000000..6bdd26fc0 --- /dev/null +++ b/control_table/mx_106.model @@ -0,0 +1,75 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +40 4 Acceleration Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 BUS Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/control_table/mx_28.model b/control_table/mx_28.model new file mode 100644 index 000000000..05ec3f10a --- /dev/null +++ b/control_table/mx_28.model @@ -0,0 +1,71 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +40 4 Acceleration Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 BUS Watchdog +100 2 Goal PWM +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Load +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/control_table/mx_64.model b/control_table/mx_64.model new file mode 100644 index 000000000..6bdd26fc0 --- /dev/null +++ b/control_table/mx_64.model @@ -0,0 +1,75 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +40 4 Acceleration Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 BUS Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/control_table/ph42_020_s300.model b/control_table/ph42_020_s300.model new file mode 100644 index 000000000..d9dd95273 --- /dev/null +++ b/control_table/ph42_020_s300.model @@ -0,0 +1,87 @@ +[type info] +name value +value_of_zero_radian_position 0 +value_of_max_radian_position 303750 +value_of_min_radian_position -303750 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +40 4 Acceleration Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +56 1 External Port Mode 1 +57 1 External Port Mode 2 +58 1 External Port Mode 3 +59 1 External Port Mode 4 +60 1 Startup Configuration +63 1 Shutdown +512 1 Torque Enable +513 1 LED Red +514 1 LED Green +515 1 LED Blue +516 1 Status Return Level +517 1 Registered Instruction +518 1 Hardware Error Status +524 2 Velocity I Gain +526 2 Velocity P Gain +528 2 Position D Gain +530 2 Position I Gain +532 2 Position P Gain +536 2 Feedforward 2nd Gain +538 2 Feedforward 1st Gain +546 1 Bus Watchdog +548 2 Goal PWM +550 2 Goal Current +552 4 Goal Velocity +556 4 Profile Acceleration +560 4 Profile Velocity +564 4 Goal Position +568 2 Realtime Tick +570 1 Moving +571 1 Moving Status +572 2 Present PWM +574 2 Present Current +576 4 Present Velocity +580 4 Present Position +584 4 Velocity Trajectory +588 4 Position Trajectory +592 2 Present Input Voltage +594 1 Present Temperature +600 2 External Port Data 1 +602 2 External Port Data 2 +604 2 External Port Data 3 +606 2 External Port Data 4 +878 1 Backup Ready +168 2 Indirect Address 1 +634 1 Indirect Data 1 +168 2 Indirect Address Write +634 1 Indirect Data Write +296 2 Indirect Address Read +698 1 Indirect Data Read diff --git a/control_table/ph54_100_s500.model b/control_table/ph54_100_s500.model new file mode 100644 index 000000000..e99523f92 --- /dev/null +++ b/control_table/ph54_100_s500.model @@ -0,0 +1,87 @@ +[type info] +name value +value_of_zero_radian_position 0 +value_of_max_radian_position 501923 +value_of_min_radian_position -501923 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +40 4 Acceleration Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +56 1 External Port Mode 1 +57 1 External Port Mode 2 +58 1 External Port Mode 3 +59 1 External Port Mode 4 +60 1 Startup Configuration +63 1 Shutdown +512 1 Torque Enable +513 1 LED Red +514 1 LED Green +515 1 LED Blue +516 1 Status Return Level +517 1 Registered Instruction +518 1 Hardware Error Status +524 2 Velocity I Gain +526 2 Velocity P Gain +528 2 Position D Gain +530 2 Position I Gain +532 2 Position P Gain +536 2 Feedforward 2nd Gain +538 2 Feedforward 1st Gain +546 1 Bus Watchdog +548 2 Goal PWM +550 2 Goal Current +552 4 Goal Velocity +556 4 Profile Acceleration +560 4 Profile Velocity +564 4 Goal Position +568 2 Realtime Tick +570 1 Moving +571 1 Moving Status +572 2 Present PWM +574 2 Present Current +576 4 Present Velocity +580 4 Present Position +584 4 Velocity Trajectory +588 4 Position Trajectory +592 2 Present Input Voltage +594 1 Present Temperature +600 2 External Port Data 1 +602 2 External Port Data 2 +604 2 External Port Data 3 +606 2 External Port Data 4 +878 1 Backup Ready +168 2 Indirect Address 1 +634 1 Indirect Data 1 +168 2 Indirect Address Write +634 1 Indirect Data Write +296 2 Indirect Address Read +698 1 Indirect Data Read diff --git a/control_table/ph54_200_s500.model b/control_table/ph54_200_s500.model new file mode 100644 index 000000000..e99523f92 --- /dev/null +++ b/control_table/ph54_200_s500.model @@ -0,0 +1,87 @@ +[type info] +name value +value_of_zero_radian_position 0 +value_of_max_radian_position 501923 +value_of_min_radian_position -501923 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +40 4 Acceleration Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +56 1 External Port Mode 1 +57 1 External Port Mode 2 +58 1 External Port Mode 3 +59 1 External Port Mode 4 +60 1 Startup Configuration +63 1 Shutdown +512 1 Torque Enable +513 1 LED Red +514 1 LED Green +515 1 LED Blue +516 1 Status Return Level +517 1 Registered Instruction +518 1 Hardware Error Status +524 2 Velocity I Gain +526 2 Velocity P Gain +528 2 Position D Gain +530 2 Position I Gain +532 2 Position P Gain +536 2 Feedforward 2nd Gain +538 2 Feedforward 1st Gain +546 1 Bus Watchdog +548 2 Goal PWM +550 2 Goal Current +552 4 Goal Velocity +556 4 Profile Acceleration +560 4 Profile Velocity +564 4 Goal Position +568 2 Realtime Tick +570 1 Moving +571 1 Moving Status +572 2 Present PWM +574 2 Present Current +576 4 Present Velocity +580 4 Present Position +584 4 Velocity Trajectory +588 4 Position Trajectory +592 2 Present Input Voltage +594 1 Present Temperature +600 2 External Port Data 1 +602 2 External Port Data 2 +604 2 External Port Data 3 +606 2 External Port Data 4 +878 1 Backup Ready +168 2 Indirect Address 1 +634 1 Indirect Data 1 +168 2 Indirect Address Write +634 1 Indirect Data Write +296 2 Indirect Address Read +698 1 Indirect Data Read diff --git a/control_table/pm42_010_s260.model b/control_table/pm42_010_s260.model new file mode 100644 index 000000000..1585fb4e4 --- /dev/null +++ b/control_table/pm42_010_s260.model @@ -0,0 +1,87 @@ +[type info] +name value +value_of_zero_radian_position 0 +value_of_max_radian_position 263187 +value_of_min_radian_position -263187 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +40 4 Acceleration Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +56 1 External Port Mode 1 +57 1 External Port Mode 2 +58 1 External Port Mode 3 +59 1 External Port Mode 4 +60 1 Startup Configuration +63 1 Shutdown +512 1 Torque Enable +513 1 LED Red +514 1 LED Green +515 1 LED Blue +516 1 Status Return Level +517 1 Registered Instruction +518 1 Hardware Error Status +524 2 Velocity I Gain +526 2 Velocity P Gain +528 2 Position D Gain +530 2 Position I Gain +532 2 Position P Gain +536 2 Feedforward 2nd Gain +538 2 Feedforward 1st Gain +546 1 Bus Watchdog +548 2 Goal PWM +550 2 Goal Current +552 4 Goal Velocity +556 4 Profile Acceleration +560 4 Profile Velocity +564 4 Goal Position +568 2 Realtime Tick +570 1 Moving +571 1 Moving Status +572 2 Present PWM +574 2 Present Current +576 4 Present Velocity +580 4 Present Position +584 4 Velocity Trajectory +588 4 Position Trajectory +592 2 Present Input Voltage +594 1 Present Temperature +600 2 External Port Data 1 +602 2 External Port Data 2 +604 2 External Port Data 3 +606 2 External Port Data 4 +878 1 Backup Ready +168 2 Indirect Address 1 +634 1 Indirect Data 1 +168 2 Indirect Address Write +634 1 Indirect Data Write +296 2 Indirect Address Read +698 1 Indirect Data Read diff --git a/control_table/pm54_040_s250.model b/control_table/pm54_040_s250.model new file mode 100644 index 000000000..3f0313461 --- /dev/null +++ b/control_table/pm54_040_s250.model @@ -0,0 +1,87 @@ +[type info] +name value +value_of_zero_radian_position 0 +value_of_max_radian_position 251417 +value_of_min_radian_position -251417 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +40 4 Acceleration Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +56 1 External Port Mode 1 +57 1 External Port Mode 2 +58 1 External Port Mode 3 +59 1 External Port Mode 4 +60 1 Startup Configuration +63 1 Shutdown +512 1 Torque Enable +513 1 LED Red +514 1 LED Green +515 1 LED Blue +516 1 Status Return Level +517 1 Registered Instruction +518 1 Hardware Error Status +524 2 Velocity I Gain +526 2 Velocity P Gain +528 2 Position D Gain +530 2 Position I Gain +532 2 Position P Gain +536 2 Feedforward 2nd Gain +538 2 Feedforward 1st Gain +546 1 Bus Watchdog +548 2 Goal PWM +550 2 Goal Current +552 4 Goal Velocity +556 4 Profile Acceleration +560 4 Profile Velocity +564 4 Goal Position +568 2 Realtime Tick +570 1 Moving +571 1 Moving Status +572 2 Present PWM +574 2 Present Current +576 4 Present Velocity +580 4 Present Position +584 4 Velocity Trajectory +588 4 Position Trajectory +592 2 Present Input Voltage +594 1 Present Temperature +600 2 External Port Data 1 +602 2 External Port Data 2 +604 2 External Port Data 3 +606 2 External Port Data 4 +878 1 Backup Ready +168 2 Indirect Address 1 +634 1 Indirect Data 1 +168 2 Indirect Address Write +634 1 Indirect Data Write +296 2 Indirect Address Read +698 1 Indirect Data Read diff --git a/control_table/pm54_060_s250.model b/control_table/pm54_060_s250.model new file mode 100644 index 000000000..3f0313461 --- /dev/null +++ b/control_table/pm54_060_s250.model @@ -0,0 +1,87 @@ +[type info] +name value +value_of_zero_radian_position 0 +value_of_max_radian_position 251417 +value_of_min_radian_position -251417 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +40 4 Acceleration Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +56 1 External Port Mode 1 +57 1 External Port Mode 2 +58 1 External Port Mode 3 +59 1 External Port Mode 4 +60 1 Startup Configuration +63 1 Shutdown +512 1 Torque Enable +513 1 LED Red +514 1 LED Green +515 1 LED Blue +516 1 Status Return Level +517 1 Registered Instruction +518 1 Hardware Error Status +524 2 Velocity I Gain +526 2 Velocity P Gain +528 2 Position D Gain +530 2 Position I Gain +532 2 Position P Gain +536 2 Feedforward 2nd Gain +538 2 Feedforward 1st Gain +546 1 Bus Watchdog +548 2 Goal PWM +550 2 Goal Current +552 4 Goal Velocity +556 4 Profile Acceleration +560 4 Profile Velocity +564 4 Goal Position +568 2 Realtime Tick +570 1 Moving +571 1 Moving Status +572 2 Present PWM +574 2 Present Current +576 4 Present Velocity +580 4 Present Position +584 4 Velocity Trajectory +588 4 Position Trajectory +592 2 Present Input Voltage +594 1 Present Temperature +600 2 External Port Data 1 +602 2 External Port Data 2 +604 2 External Port Data 3 +606 2 External Port Data 4 +878 1 Backup Ready +168 2 Indirect Address 1 +634 1 Indirect Data 1 +168 2 Indirect Address Write +634 1 Indirect Data Write +296 2 Indirect Address Read +698 1 Indirect Data Read diff --git a/control_table/rh_p12_rn.model b/control_table/rh_p12_rn.model new file mode 100644 index 000000000..b081fab49 --- /dev/null +++ b/control_table/rh_p12_rn.model @@ -0,0 +1,80 @@ +[type info] +name value +value_of_zero_radian_position 0 +value_of_max_radian_position 740 +value_of_min_radian_position -740 +min_radian -1.099 +max_radian 1.099 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0119380521 rad/s signed +Goal Velocity 0.0119380521 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +11 1 Operating Mode +12 1 Secondary ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +40 4 Acceleration Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +56 1 External Port Mode 1 +57 1 External Port Mode 2 +58 1 External Port Mode 3 +59 1 External Port Mode 4 +63 1 Shutdown +168 2 Indirect Address 1 +512 1 Torque Enable +513 1 LED Red +514 1 LED Green +515 1 LED Blue +516 1 Status Return Level +517 1 Registered Instruction +518 1 Hardware Error Status +524 2 Velocity I Gain +526 2 Velocity P Gain +528 2 Position D Gain +532 2 Position P Gain +530 2 Position I Gain +536 2 Feedforward 2nd Gain +538 2 Feedforward 1st Gain +546 1 Bus Watchdog +548 2 Goal PWM +550 2 Goal Current +552 4 Goal Velocity +556 4 Profile Acceleration +560 4 Profile Velocity +564 4 Goal Position +568 2 Realtime Tick +570 1 Moving +571 1 Moving Status +572 2 Present PWM +574 2 Present Current +576 4 Present Velocity +580 4 Present Position +584 4 Velocity Trajectory +588 4 Position Trajectory +592 2 Present Input Voltage +594 1 Present Temperature +634 1 Indirect Data 1 +168 2 Indirect Address Write +634 1 Indirect Data Write +296 2 Indirect Address Read +698 1 Indirect Data Read diff --git a/control_table/xc330_m181.model b/control_table/xc330_m181.model new file mode 100644 index 000000000..0a0935085 --- /dev/null +++ b/control_table/xc330_m181.model @@ -0,0 +1,75 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +62 1 PWM Slope +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +180 2 Indirect Address Read +230 1 Indirect Data Read diff --git a/control_table/xc330_m181_fw52.model b/control_table/xc330_m181_fw52.model new file mode 100644 index 000000000..d8483c92c --- /dev/null +++ b/control_table/xc330_m181_fw52.model @@ -0,0 +1,75 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +62 1 PWM Slope +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +208 1 Indirect Data 1 +168 2 Indirect Address Write +208 1 Indirect Data Write +180 2 Indirect Address Read +214 1 Indirect Data Read diff --git a/control_table/xc330_m288.model b/control_table/xc330_m288.model new file mode 100644 index 000000000..0a0935085 --- /dev/null +++ b/control_table/xc330_m288.model @@ -0,0 +1,75 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +62 1 PWM Slope +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +180 2 Indirect Address Read +230 1 Indirect Data Read diff --git a/control_table/xc330_m288_fw52.model b/control_table/xc330_m288_fw52.model new file mode 100644 index 000000000..d8483c92c --- /dev/null +++ b/control_table/xc330_m288_fw52.model @@ -0,0 +1,75 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +62 1 PWM Slope +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +208 1 Indirect Data 1 +168 2 Indirect Address Write +208 1 Indirect Data Write +180 2 Indirect Address Read +214 1 Indirect Data Read diff --git a/control_table/xc330_t181.model b/control_table/xc330_t181.model new file mode 100644 index 000000000..80ca68db9 --- /dev/null +++ b/control_table/xc330_t181.model @@ -0,0 +1,75 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed +Present Current 0.0006709470296015791 N/m signed +Goal Current 0.0006709470296015791 N/m signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +62 1 PWM Slope +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +180 2 Indirect Address Read +230 1 Indirect Data Read diff --git a/control_table/xc330_t181_fw52.model b/control_table/xc330_t181_fw52.model new file mode 100644 index 000000000..7d397de1d --- /dev/null +++ b/control_table/xc330_t181_fw52.model @@ -0,0 +1,75 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed +Present Current 0.0006709470296015791 N/m signed +Goal Current 0.0006709470296015791 N/m signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +62 1 PWM Slope +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +208 1 Indirect Data 1 +168 2 Indirect Address Write +208 1 Indirect Data Write +180 2 Indirect Address Read +214 1 Indirect Data Read diff --git a/control_table/xc330_t288.model b/control_table/xc330_t288.model new file mode 100644 index 000000000..a3ba279df --- /dev/null +++ b/control_table/xc330_t288.model @@ -0,0 +1,75 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed +Present Current 0.0009674796739867748 N/m signed +Goal Current 0.0009674796739867748 N/m signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +62 1 PWM Slope +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +180 2 Indirect Address Read +230 1 Indirect Data Read diff --git a/control_table/xc330_t288_fw52.model b/control_table/xc330_t288_fw52.model new file mode 100644 index 000000000..f01d3fe77 --- /dev/null +++ b/control_table/xc330_t288_fw52.model @@ -0,0 +1,75 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed +Present Current 0.0009674796739867748 N/m signed +Goal Current 0.0009674796739867748 N/m signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +62 1 PWM Slope +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +208 1 Indirect Data 1 +168 2 Indirect Address Write +208 1 Indirect Data Write +180 2 Indirect Address Read +214 1 Indirect Data Read diff --git a/control_table/xc430_w150.model b/control_table/xc430_w150.model new file mode 100644 index 000000000..deb48f6fb --- /dev/null +++ b/control_table/xc430_w150.model @@ -0,0 +1,70 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Load +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/control_table/xc430_w240.model b/control_table/xc430_w240.model new file mode 100644 index 000000000..deb48f6fb --- /dev/null +++ b/control_table/xc430_w240.model @@ -0,0 +1,70 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Load +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/control_table/xd430_t210.model b/control_table/xd430_t210.model new file mode 100644 index 000000000..aa324bb6c --- /dev/null +++ b/control_table/xd430_t210.model @@ -0,0 +1,74 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/control_table/xd430_t350.model b/control_table/xd430_t350.model new file mode 100644 index 000000000..aa324bb6c --- /dev/null +++ b/control_table/xd430_t350.model @@ -0,0 +1,74 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/control_table/xd540_t150.model b/control_table/xd540_t150.model new file mode 100644 index 000000000..033c4c871 --- /dev/null +++ b/control_table/xd540_t150.model @@ -0,0 +1,80 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +56 1 External Port Mode 1 +57 1 External Port Mode 2 +58 1 External Port Mode 3 +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +152 2 External Port Data 1 +154 2 External Port Data 2 +156 2 External Port Data 3 +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/control_table/xd540_t270.model b/control_table/xd540_t270.model new file mode 100644 index 000000000..033c4c871 --- /dev/null +++ b/control_table/xd540_t270.model @@ -0,0 +1,80 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +56 1 External Port Mode 1 +57 1 External Port Mode 2 +58 1 External Port Mode 3 +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +152 2 External Port Data 1 +154 2 External Port Data 2 +156 2 External Port Data 3 +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/control_table/xh430_v210.model b/control_table/xh430_v210.model new file mode 100644 index 000000000..aa324bb6c --- /dev/null +++ b/control_table/xh430_v210.model @@ -0,0 +1,74 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/control_table/xh430_v350.model b/control_table/xh430_v350.model new file mode 100644 index 000000000..aa324bb6c --- /dev/null +++ b/control_table/xh430_v350.model @@ -0,0 +1,74 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/control_table/xh430_w210.model b/control_table/xh430_w210.model new file mode 100644 index 000000000..aa324bb6c --- /dev/null +++ b/control_table/xh430_w210.model @@ -0,0 +1,74 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/control_table/xh430_w350.model b/control_table/xh430_w350.model new file mode 100644 index 000000000..aa324bb6c --- /dev/null +++ b/control_table/xh430_w350.model @@ -0,0 +1,74 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/control_table/xh540_v150.model b/control_table/xh540_v150.model new file mode 100644 index 000000000..033c4c871 --- /dev/null +++ b/control_table/xh540_v150.model @@ -0,0 +1,80 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +56 1 External Port Mode 1 +57 1 External Port Mode 2 +58 1 External Port Mode 3 +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +152 2 External Port Data 1 +154 2 External Port Data 2 +156 2 External Port Data 3 +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/control_table/xh540_v270.model b/control_table/xh540_v270.model new file mode 100644 index 000000000..033c4c871 --- /dev/null +++ b/control_table/xh540_v270.model @@ -0,0 +1,80 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +56 1 External Port Mode 1 +57 1 External Port Mode 2 +58 1 External Port Mode 3 +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +152 2 External Port Data 1 +154 2 External Port Data 2 +156 2 External Port Data 3 +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/control_table/xh540_w150.model b/control_table/xh540_w150.model new file mode 100644 index 000000000..3487436ba --- /dev/null +++ b/control_table/xh540_w150.model @@ -0,0 +1,80 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed +Present Current 0.0045 N/m signed +Goal Current 0.0045 N/m signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +56 1 External Port Mode 1 +57 1 External Port Mode 2 +58 1 External Port Mode 3 +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +152 2 External Port Data 1 +154 2 External Port Data 2 +156 2 External Port Data 3 +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/control_table/xh540_w270.model b/control_table/xh540_w270.model new file mode 100644 index 000000000..033c4c871 --- /dev/null +++ b/control_table/xh540_w270.model @@ -0,0 +1,80 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +56 1 External Port Mode 1 +57 1 External Port Mode 2 +58 1 External Port Mode 3 +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +152 2 External Port Data 1 +154 2 External Port Data 2 +156 2 External Port Data 3 +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/control_table/xl320.model b/control_table/xl320.model new file mode 100644 index 000000000..df5914a74 --- /dev/null +++ b/control_table/xl320.model @@ -0,0 +1,46 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0116228475 rad/s signed +Goal Velocity 0.0116228475 rad/s signed + +[control table] +Address Size Data Name +0 2 Model Number +2 1 Firmware Version +3 1 ID +4 1 Baud Rate +5 1 Return Delay Time +6 2 CW Angle Limit +8 2 CCW Angle Limit +11 1 Control Mode +12 1 Temperature Limit +13 1 Min Voltage Limit +14 1 Max Voltage Limit +15 2 Max Torque +17 1 Status Return Level +18 1 Shutdown +24 1 Torque Enable +25 1 LED +27 1 D Gain +28 1 I Gain +29 1 P Gain +30 2 Goal Position +32 2 Moving Speed +35 2 Torque Limit +37 2 Present Position +39 2 Present Speed +41 2 Present Load +45 1 Present Voltage +46 1 Present Temperature +47 1 Registered Instruction +49 1 Moving +50 1 Hardware Error Status +51 2 Punch diff --git a/control_table/xl330_m077.model b/control_table/xl330_m077.model new file mode 100644 index 000000000..0a0935085 --- /dev/null +++ b/control_table/xl330_m077.model @@ -0,0 +1,75 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +62 1 PWM Slope +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +180 2 Indirect Address Read +230 1 Indirect Data Read diff --git a/control_table/xl330_m077_fw52.model b/control_table/xl330_m077_fw52.model new file mode 100644 index 000000000..d8483c92c --- /dev/null +++ b/control_table/xl330_m077_fw52.model @@ -0,0 +1,75 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +62 1 PWM Slope +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +208 1 Indirect Data 1 +168 2 Indirect Address Write +208 1 Indirect Data Write +180 2 Indirect Address Read +214 1 Indirect Data Read diff --git a/control_table/xl330_m288.model b/control_table/xl330_m288.model new file mode 100644 index 000000000..0a0935085 --- /dev/null +++ b/control_table/xl330_m288.model @@ -0,0 +1,75 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +62 1 PWM Slope +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +180 2 Indirect Address Read +230 1 Indirect Data Read diff --git a/control_table/xl330_m288_fw52.model b/control_table/xl330_m288_fw52.model new file mode 100644 index 000000000..d8483c92c --- /dev/null +++ b/control_table/xl330_m288_fw52.model @@ -0,0 +1,75 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +62 1 PWM Slope +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +208 1 Indirect Data 1 +168 2 Indirect Address Write +208 1 Indirect Data Write +180 2 Indirect Address Read +214 1 Indirect Data Read diff --git a/control_table/xl430_w250.model b/control_table/xl430_w250.model new file mode 100644 index 000000000..deb48f6fb --- /dev/null +++ b/control_table/xl430_w250.model @@ -0,0 +1,70 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Load +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/control_table/xm430_w210.model b/control_table/xm430_w210.model new file mode 100644 index 000000000..aa324bb6c --- /dev/null +++ b/control_table/xm430_w210.model @@ -0,0 +1,74 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/control_table/xm430_w350.model b/control_table/xm430_w350.model new file mode 100644 index 000000000..aa324bb6c --- /dev/null +++ b/control_table/xm430_w350.model @@ -0,0 +1,74 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/control_table/xm540_w150.model b/control_table/xm540_w150.model new file mode 100644 index 000000000..033c4c871 --- /dev/null +++ b/control_table/xm540_w150.model @@ -0,0 +1,80 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +56 1 External Port Mode 1 +57 1 External Port Mode 2 +58 1 External Port Mode 3 +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +152 2 External Port Data 1 +154 2 External Port Data 2 +156 2 External Port Data 3 +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/control_table/xm540_w270.model b/control_table/xm540_w270.model new file mode 100644 index 000000000..033c4c871 --- /dev/null +++ b/control_table/xm540_w270.model @@ -0,0 +1,80 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +56 1 External Port Mode 1 +57 1 External Port Mode 2 +58 1 External Port Mode 3 +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +152 2 External Port Data 1 +154 2 External Port Data 2 +156 2 External Port Data 3 +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/control_table/xw430_t200.model b/control_table/xw430_t200.model new file mode 100644 index 000000000..f9c057569 --- /dev/null +++ b/control_table/xw430_t200.model @@ -0,0 +1,73 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +63 1 Shutdown +64 1 Torque Enable +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/control_table/xw430_t333.model b/control_table/xw430_t333.model new file mode 100644 index 000000000..f9c057569 --- /dev/null +++ b/control_table/xw430_t333.model @@ -0,0 +1,73 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +63 1 Shutdown +64 1 Torque Enable +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/control_table/xw540_h260.model b/control_table/xw540_h260.model new file mode 100644 index 000000000..f9c057569 --- /dev/null +++ b/control_table/xw540_h260.model @@ -0,0 +1,73 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +63 1 Shutdown +64 1 Torque Enable +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/control_table/xw540_t140.model b/control_table/xw540_t140.model new file mode 100644 index 000000000..f9c057569 --- /dev/null +++ b/control_table/xw540_t140.model @@ -0,0 +1,73 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +63 1 Shutdown +64 1 Torque Enable +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/control_table/xw540_t260.model b/control_table/xw540_t260.model new file mode 100644 index 000000000..f9c057569 --- /dev/null +++ b/control_table/xw540_t260.model @@ -0,0 +1,73 @@ +[type info] +name value +value_of_zero_radian_position 2048 +value_of_max_radian_position 4095 +value_of_min_radian_position 0 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0239691227 rad/s signed +Goal Velocity 0.0239691227 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +63 1 Shutdown +64 1 Torque Enable +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +224 1 Indirect Data 1 +168 2 Indirect Address Write +224 1 Indirect Data Write +578 2 Indirect Address Read +634 1 Indirect Data Read diff --git a/control_table/ym070_210_a051.model b/control_table/ym070_210_a051.model new file mode 100644 index 000000000..72a20d37a --- /dev/null +++ b/control_table/ym070_210_a051.model @@ -0,0 +1,115 @@ +[type info] +name value +value_of_zero_radian_position 0 +value_of_max_radian_position 262144 +value_of_min_radian_position -262144 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 2 Bus Watchdog +10 1 Secondary(Shadow) ID +11 1 Protocol Type +12 1 Baud Rate (Bus) +13 1 Return Delay Time +15 1 Status Return Level +16 1 Registered Instruction +32 1 Drive Mode +33 1 Operating Mode +34 1 Startup Configuration +38 2 Position Limit Threshold +40 4 In-Position Threshold +44 4 Following Error Threshold +48 4 Moving Threshold +52 4 Homing Offset +56 1 Inverter Temperature Limit +57 1 Motor Temperature Limit +60 2 Max Voltage Limit +62 2 Min Voltage Limit +64 2 PWM Limit +66 2 Current Limit +68 4 Acceleration Limit +72 4 Velocity Limit +76 4 Max Position Limit +84 4 Min Position Limit +96 4 Electronic GearRatio Numerator +100 4 Electronic GearRatio Denominator +104 2 Safe Stop Time +106 2 Brake Delay +108 2 Goal Update Delay +110 1 Overexcitation Voltage +111 1 Normal Excitation Voltage +112 2 Overexcitation Time +132 2 Present Velocity LPF Frequency +134 2 Goal Current LPF Frequency +136 2 Position FF LPF Time +138 2 Velocity FF LPF Time +152 1 Controller State +153 1 Error Code +154 1 Error Code History 1 +155 1 Error Code History 2 +156 1 Error Code History 3 +157 1 Error Code History 4 +158 1 Error Code History 5 +159 1 Error Code History 6 +160 1 Error Code History 7 +161 1 Error Code History 8 +162 1 Error Code History 9 +163 1 Error Code History 10 +164 1 Error Code History 11 +165 1 Error Code History 12 +166 1 Error Code History 13 +167 1 Error Code History 14 +168 1 Error Code History 15 +169 1 Error Code History 16 +170 1 Hybrid Save +212 4 Velocity I Gain +216 4 Velocity P Gain +220 4 Velocity FF Gain +224 4 Position D Gain +228 4 Position I Gain +232 4 Position P Gain +236 4 Position FF Gain +240 4 Profile Acceleration +244 4 Profile Velocity +248 4 Profile Acceleration Time +252 4 Profile Time +256 2 Indirect Address 1 +512 1 Torque Enable +513 1 LED +516 2 PWM Offset +518 2 Current Offset +520 4 Velocity Offset +524 2 Goal PWM +526 2 Goal Current +528 4 Goal Velocity +532 4 Goal Position +541 1 Moving Status +542 2 Realtime Tick +544 2 Present PWM +546 2 Present Current +548 4 Present Velocity +552 4 Present Position +560 4 Position Trajectory +564 4 Velocity Trajectory +568 2 Present Input Voltage +570 1 Present Inverter Temperature +571 1 Present Motor Temperature +634 1 Indirect Data 1 +919 1 Backup Ready +256 2 Indirect Address Write +634 1 Indirect Data Write +384 2 Indirect Address Read +698 1 Indirect Data Read diff --git a/control_table/ym070_210_a099.model b/control_table/ym070_210_a099.model new file mode 100644 index 000000000..72a20d37a --- /dev/null +++ b/control_table/ym070_210_a099.model @@ -0,0 +1,115 @@ +[type info] +name value +value_of_zero_radian_position 0 +value_of_max_radian_position 262144 +value_of_min_radian_position -262144 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 2 Bus Watchdog +10 1 Secondary(Shadow) ID +11 1 Protocol Type +12 1 Baud Rate (Bus) +13 1 Return Delay Time +15 1 Status Return Level +16 1 Registered Instruction +32 1 Drive Mode +33 1 Operating Mode +34 1 Startup Configuration +38 2 Position Limit Threshold +40 4 In-Position Threshold +44 4 Following Error Threshold +48 4 Moving Threshold +52 4 Homing Offset +56 1 Inverter Temperature Limit +57 1 Motor Temperature Limit +60 2 Max Voltage Limit +62 2 Min Voltage Limit +64 2 PWM Limit +66 2 Current Limit +68 4 Acceleration Limit +72 4 Velocity Limit +76 4 Max Position Limit +84 4 Min Position Limit +96 4 Electronic GearRatio Numerator +100 4 Electronic GearRatio Denominator +104 2 Safe Stop Time +106 2 Brake Delay +108 2 Goal Update Delay +110 1 Overexcitation Voltage +111 1 Normal Excitation Voltage +112 2 Overexcitation Time +132 2 Present Velocity LPF Frequency +134 2 Goal Current LPF Frequency +136 2 Position FF LPF Time +138 2 Velocity FF LPF Time +152 1 Controller State +153 1 Error Code +154 1 Error Code History 1 +155 1 Error Code History 2 +156 1 Error Code History 3 +157 1 Error Code History 4 +158 1 Error Code History 5 +159 1 Error Code History 6 +160 1 Error Code History 7 +161 1 Error Code History 8 +162 1 Error Code History 9 +163 1 Error Code History 10 +164 1 Error Code History 11 +165 1 Error Code History 12 +166 1 Error Code History 13 +167 1 Error Code History 14 +168 1 Error Code History 15 +169 1 Error Code History 16 +170 1 Hybrid Save +212 4 Velocity I Gain +216 4 Velocity P Gain +220 4 Velocity FF Gain +224 4 Position D Gain +228 4 Position I Gain +232 4 Position P Gain +236 4 Position FF Gain +240 4 Profile Acceleration +244 4 Profile Velocity +248 4 Profile Acceleration Time +252 4 Profile Time +256 2 Indirect Address 1 +512 1 Torque Enable +513 1 LED +516 2 PWM Offset +518 2 Current Offset +520 4 Velocity Offset +524 2 Goal PWM +526 2 Goal Current +528 4 Goal Velocity +532 4 Goal Position +541 1 Moving Status +542 2 Realtime Tick +544 2 Present PWM +546 2 Present Current +548 4 Present Velocity +552 4 Present Position +560 4 Position Trajectory +564 4 Velocity Trajectory +568 2 Present Input Voltage +570 1 Present Inverter Temperature +571 1 Present Motor Temperature +634 1 Indirect Data 1 +919 1 Backup Ready +256 2 Indirect Address Write +634 1 Indirect Data Write +384 2 Indirect Address Read +698 1 Indirect Data Read diff --git a/control_table/ym070_210_b001.model b/control_table/ym070_210_b001.model new file mode 100644 index 000000000..72a20d37a --- /dev/null +++ b/control_table/ym070_210_b001.model @@ -0,0 +1,115 @@ +[type info] +name value +value_of_zero_radian_position 0 +value_of_max_radian_position 262144 +value_of_min_radian_position -262144 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 2 Bus Watchdog +10 1 Secondary(Shadow) ID +11 1 Protocol Type +12 1 Baud Rate (Bus) +13 1 Return Delay Time +15 1 Status Return Level +16 1 Registered Instruction +32 1 Drive Mode +33 1 Operating Mode +34 1 Startup Configuration +38 2 Position Limit Threshold +40 4 In-Position Threshold +44 4 Following Error Threshold +48 4 Moving Threshold +52 4 Homing Offset +56 1 Inverter Temperature Limit +57 1 Motor Temperature Limit +60 2 Max Voltage Limit +62 2 Min Voltage Limit +64 2 PWM Limit +66 2 Current Limit +68 4 Acceleration Limit +72 4 Velocity Limit +76 4 Max Position Limit +84 4 Min Position Limit +96 4 Electronic GearRatio Numerator +100 4 Electronic GearRatio Denominator +104 2 Safe Stop Time +106 2 Brake Delay +108 2 Goal Update Delay +110 1 Overexcitation Voltage +111 1 Normal Excitation Voltage +112 2 Overexcitation Time +132 2 Present Velocity LPF Frequency +134 2 Goal Current LPF Frequency +136 2 Position FF LPF Time +138 2 Velocity FF LPF Time +152 1 Controller State +153 1 Error Code +154 1 Error Code History 1 +155 1 Error Code History 2 +156 1 Error Code History 3 +157 1 Error Code History 4 +158 1 Error Code History 5 +159 1 Error Code History 6 +160 1 Error Code History 7 +161 1 Error Code History 8 +162 1 Error Code History 9 +163 1 Error Code History 10 +164 1 Error Code History 11 +165 1 Error Code History 12 +166 1 Error Code History 13 +167 1 Error Code History 14 +168 1 Error Code History 15 +169 1 Error Code History 16 +170 1 Hybrid Save +212 4 Velocity I Gain +216 4 Velocity P Gain +220 4 Velocity FF Gain +224 4 Position D Gain +228 4 Position I Gain +232 4 Position P Gain +236 4 Position FF Gain +240 4 Profile Acceleration +244 4 Profile Velocity +248 4 Profile Acceleration Time +252 4 Profile Time +256 2 Indirect Address 1 +512 1 Torque Enable +513 1 LED +516 2 PWM Offset +518 2 Current Offset +520 4 Velocity Offset +524 2 Goal PWM +526 2 Goal Current +528 4 Goal Velocity +532 4 Goal Position +541 1 Moving Status +542 2 Realtime Tick +544 2 Present PWM +546 2 Present Current +548 4 Present Velocity +552 4 Present Position +560 4 Position Trajectory +564 4 Velocity Trajectory +568 2 Present Input Voltage +570 1 Present Inverter Temperature +571 1 Present Motor Temperature +634 1 Indirect Data 1 +919 1 Backup Ready +256 2 Indirect Address Write +634 1 Indirect Data Write +384 2 Indirect Address Read +698 1 Indirect Data Read diff --git a/control_table/ym070_210_m001.model b/control_table/ym070_210_m001.model new file mode 100644 index 000000000..72a20d37a --- /dev/null +++ b/control_table/ym070_210_m001.model @@ -0,0 +1,115 @@ +[type info] +name value +value_of_zero_radian_position 0 +value_of_max_radian_position 262144 +value_of_min_radian_position -262144 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 2 Bus Watchdog +10 1 Secondary(Shadow) ID +11 1 Protocol Type +12 1 Baud Rate (Bus) +13 1 Return Delay Time +15 1 Status Return Level +16 1 Registered Instruction +32 1 Drive Mode +33 1 Operating Mode +34 1 Startup Configuration +38 2 Position Limit Threshold +40 4 In-Position Threshold +44 4 Following Error Threshold +48 4 Moving Threshold +52 4 Homing Offset +56 1 Inverter Temperature Limit +57 1 Motor Temperature Limit +60 2 Max Voltage Limit +62 2 Min Voltage Limit +64 2 PWM Limit +66 2 Current Limit +68 4 Acceleration Limit +72 4 Velocity Limit +76 4 Max Position Limit +84 4 Min Position Limit +96 4 Electronic GearRatio Numerator +100 4 Electronic GearRatio Denominator +104 2 Safe Stop Time +106 2 Brake Delay +108 2 Goal Update Delay +110 1 Overexcitation Voltage +111 1 Normal Excitation Voltage +112 2 Overexcitation Time +132 2 Present Velocity LPF Frequency +134 2 Goal Current LPF Frequency +136 2 Position FF LPF Time +138 2 Velocity FF LPF Time +152 1 Controller State +153 1 Error Code +154 1 Error Code History 1 +155 1 Error Code History 2 +156 1 Error Code History 3 +157 1 Error Code History 4 +158 1 Error Code History 5 +159 1 Error Code History 6 +160 1 Error Code History 7 +161 1 Error Code History 8 +162 1 Error Code History 9 +163 1 Error Code History 10 +164 1 Error Code History 11 +165 1 Error Code History 12 +166 1 Error Code History 13 +167 1 Error Code History 14 +168 1 Error Code History 15 +169 1 Error Code History 16 +170 1 Hybrid Save +212 4 Velocity I Gain +216 4 Velocity P Gain +220 4 Velocity FF Gain +224 4 Position D Gain +228 4 Position I Gain +232 4 Position P Gain +236 4 Position FF Gain +240 4 Profile Acceleration +244 4 Profile Velocity +248 4 Profile Acceleration Time +252 4 Profile Time +256 2 Indirect Address 1 +512 1 Torque Enable +513 1 LED +516 2 PWM Offset +518 2 Current Offset +520 4 Velocity Offset +524 2 Goal PWM +526 2 Goal Current +528 4 Goal Velocity +532 4 Goal Position +541 1 Moving Status +542 2 Realtime Tick +544 2 Present PWM +546 2 Present Current +548 4 Present Velocity +552 4 Present Position +560 4 Position Trajectory +564 4 Velocity Trajectory +568 2 Present Input Voltage +570 1 Present Inverter Temperature +571 1 Present Motor Temperature +634 1 Indirect Data 1 +919 1 Backup Ready +256 2 Indirect Address Write +634 1 Indirect Data Write +384 2 Indirect Address Read +698 1 Indirect Data Read diff --git a/control_table/ym070_210_r051.model b/control_table/ym070_210_r051.model new file mode 100644 index 000000000..72a20d37a --- /dev/null +++ b/control_table/ym070_210_r051.model @@ -0,0 +1,115 @@ +[type info] +name value +value_of_zero_radian_position 0 +value_of_max_radian_position 262144 +value_of_min_radian_position -262144 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 2 Bus Watchdog +10 1 Secondary(Shadow) ID +11 1 Protocol Type +12 1 Baud Rate (Bus) +13 1 Return Delay Time +15 1 Status Return Level +16 1 Registered Instruction +32 1 Drive Mode +33 1 Operating Mode +34 1 Startup Configuration +38 2 Position Limit Threshold +40 4 In-Position Threshold +44 4 Following Error Threshold +48 4 Moving Threshold +52 4 Homing Offset +56 1 Inverter Temperature Limit +57 1 Motor Temperature Limit +60 2 Max Voltage Limit +62 2 Min Voltage Limit +64 2 PWM Limit +66 2 Current Limit +68 4 Acceleration Limit +72 4 Velocity Limit +76 4 Max Position Limit +84 4 Min Position Limit +96 4 Electronic GearRatio Numerator +100 4 Electronic GearRatio Denominator +104 2 Safe Stop Time +106 2 Brake Delay +108 2 Goal Update Delay +110 1 Overexcitation Voltage +111 1 Normal Excitation Voltage +112 2 Overexcitation Time +132 2 Present Velocity LPF Frequency +134 2 Goal Current LPF Frequency +136 2 Position FF LPF Time +138 2 Velocity FF LPF Time +152 1 Controller State +153 1 Error Code +154 1 Error Code History 1 +155 1 Error Code History 2 +156 1 Error Code History 3 +157 1 Error Code History 4 +158 1 Error Code History 5 +159 1 Error Code History 6 +160 1 Error Code History 7 +161 1 Error Code History 8 +162 1 Error Code History 9 +163 1 Error Code History 10 +164 1 Error Code History 11 +165 1 Error Code History 12 +166 1 Error Code History 13 +167 1 Error Code History 14 +168 1 Error Code History 15 +169 1 Error Code History 16 +170 1 Hybrid Save +212 4 Velocity I Gain +216 4 Velocity P Gain +220 4 Velocity FF Gain +224 4 Position D Gain +228 4 Position I Gain +232 4 Position P Gain +236 4 Position FF Gain +240 4 Profile Acceleration +244 4 Profile Velocity +248 4 Profile Acceleration Time +252 4 Profile Time +256 2 Indirect Address 1 +512 1 Torque Enable +513 1 LED +516 2 PWM Offset +518 2 Current Offset +520 4 Velocity Offset +524 2 Goal PWM +526 2 Goal Current +528 4 Goal Velocity +532 4 Goal Position +541 1 Moving Status +542 2 Realtime Tick +544 2 Present PWM +546 2 Present Current +548 4 Present Velocity +552 4 Present Position +560 4 Position Trajectory +564 4 Velocity Trajectory +568 2 Present Input Voltage +570 1 Present Inverter Temperature +571 1 Present Motor Temperature +634 1 Indirect Data 1 +919 1 Backup Ready +256 2 Indirect Address Write +634 1 Indirect Data Write +384 2 Indirect Address Read +698 1 Indirect Data Read diff --git a/control_table/ym070_210_r099.model b/control_table/ym070_210_r099.model new file mode 100644 index 000000000..72a20d37a --- /dev/null +++ b/control_table/ym070_210_r099.model @@ -0,0 +1,115 @@ +[type info] +name value +value_of_zero_radian_position 0 +value_of_max_radian_position 262144 +value_of_min_radian_position -262144 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 2 Bus Watchdog +10 1 Secondary(Shadow) ID +11 1 Protocol Type +12 1 Baud Rate (Bus) +13 1 Return Delay Time +15 1 Status Return Level +16 1 Registered Instruction +32 1 Drive Mode +33 1 Operating Mode +34 1 Startup Configuration +38 2 Position Limit Threshold +40 4 In-Position Threshold +44 4 Following Error Threshold +48 4 Moving Threshold +52 4 Homing Offset +56 1 Inverter Temperature Limit +57 1 Motor Temperature Limit +60 2 Max Voltage Limit +62 2 Min Voltage Limit +64 2 PWM Limit +66 2 Current Limit +68 4 Acceleration Limit +72 4 Velocity Limit +76 4 Max Position Limit +84 4 Min Position Limit +96 4 Electronic GearRatio Numerator +100 4 Electronic GearRatio Denominator +104 2 Safe Stop Time +106 2 Brake Delay +108 2 Goal Update Delay +110 1 Overexcitation Voltage +111 1 Normal Excitation Voltage +112 2 Overexcitation Time +132 2 Present Velocity LPF Frequency +134 2 Goal Current LPF Frequency +136 2 Position FF LPF Time +138 2 Velocity FF LPF Time +152 1 Controller State +153 1 Error Code +154 1 Error Code History 1 +155 1 Error Code History 2 +156 1 Error Code History 3 +157 1 Error Code History 4 +158 1 Error Code History 5 +159 1 Error Code History 6 +160 1 Error Code History 7 +161 1 Error Code History 8 +162 1 Error Code History 9 +163 1 Error Code History 10 +164 1 Error Code History 11 +165 1 Error Code History 12 +166 1 Error Code History 13 +167 1 Error Code History 14 +168 1 Error Code History 15 +169 1 Error Code History 16 +170 1 Hybrid Save +212 4 Velocity I Gain +216 4 Velocity P Gain +220 4 Velocity FF Gain +224 4 Position D Gain +228 4 Position I Gain +232 4 Position P Gain +236 4 Position FF Gain +240 4 Profile Acceleration +244 4 Profile Velocity +248 4 Profile Acceleration Time +252 4 Profile Time +256 2 Indirect Address 1 +512 1 Torque Enable +513 1 LED +516 2 PWM Offset +518 2 Current Offset +520 4 Velocity Offset +524 2 Goal PWM +526 2 Goal Current +528 4 Goal Velocity +532 4 Goal Position +541 1 Moving Status +542 2 Realtime Tick +544 2 Present PWM +546 2 Present Current +548 4 Present Velocity +552 4 Present Position +560 4 Position Trajectory +564 4 Velocity Trajectory +568 2 Present Input Voltage +570 1 Present Inverter Temperature +571 1 Present Motor Temperature +634 1 Indirect Data 1 +919 1 Backup Ready +256 2 Indirect Address Write +634 1 Indirect Data Write +384 2 Indirect Address Read +698 1 Indirect Data Read diff --git a/control_table/ym080_230_a051.model b/control_table/ym080_230_a051.model new file mode 100644 index 000000000..72a20d37a --- /dev/null +++ b/control_table/ym080_230_a051.model @@ -0,0 +1,115 @@ +[type info] +name value +value_of_zero_radian_position 0 +value_of_max_radian_position 262144 +value_of_min_radian_position -262144 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 2 Bus Watchdog +10 1 Secondary(Shadow) ID +11 1 Protocol Type +12 1 Baud Rate (Bus) +13 1 Return Delay Time +15 1 Status Return Level +16 1 Registered Instruction +32 1 Drive Mode +33 1 Operating Mode +34 1 Startup Configuration +38 2 Position Limit Threshold +40 4 In-Position Threshold +44 4 Following Error Threshold +48 4 Moving Threshold +52 4 Homing Offset +56 1 Inverter Temperature Limit +57 1 Motor Temperature Limit +60 2 Max Voltage Limit +62 2 Min Voltage Limit +64 2 PWM Limit +66 2 Current Limit +68 4 Acceleration Limit +72 4 Velocity Limit +76 4 Max Position Limit +84 4 Min Position Limit +96 4 Electronic GearRatio Numerator +100 4 Electronic GearRatio Denominator +104 2 Safe Stop Time +106 2 Brake Delay +108 2 Goal Update Delay +110 1 Overexcitation Voltage +111 1 Normal Excitation Voltage +112 2 Overexcitation Time +132 2 Present Velocity LPF Frequency +134 2 Goal Current LPF Frequency +136 2 Position FF LPF Time +138 2 Velocity FF LPF Time +152 1 Controller State +153 1 Error Code +154 1 Error Code History 1 +155 1 Error Code History 2 +156 1 Error Code History 3 +157 1 Error Code History 4 +158 1 Error Code History 5 +159 1 Error Code History 6 +160 1 Error Code History 7 +161 1 Error Code History 8 +162 1 Error Code History 9 +163 1 Error Code History 10 +164 1 Error Code History 11 +165 1 Error Code History 12 +166 1 Error Code History 13 +167 1 Error Code History 14 +168 1 Error Code History 15 +169 1 Error Code History 16 +170 1 Hybrid Save +212 4 Velocity I Gain +216 4 Velocity P Gain +220 4 Velocity FF Gain +224 4 Position D Gain +228 4 Position I Gain +232 4 Position P Gain +236 4 Position FF Gain +240 4 Profile Acceleration +244 4 Profile Velocity +248 4 Profile Acceleration Time +252 4 Profile Time +256 2 Indirect Address 1 +512 1 Torque Enable +513 1 LED +516 2 PWM Offset +518 2 Current Offset +520 4 Velocity Offset +524 2 Goal PWM +526 2 Goal Current +528 4 Goal Velocity +532 4 Goal Position +541 1 Moving Status +542 2 Realtime Tick +544 2 Present PWM +546 2 Present Current +548 4 Present Velocity +552 4 Present Position +560 4 Position Trajectory +564 4 Velocity Trajectory +568 2 Present Input Voltage +570 1 Present Inverter Temperature +571 1 Present Motor Temperature +634 1 Indirect Data 1 +919 1 Backup Ready +256 2 Indirect Address Write +634 1 Indirect Data Write +384 2 Indirect Address Read +698 1 Indirect Data Read diff --git a/control_table/ym080_230_a099.model b/control_table/ym080_230_a099.model new file mode 100644 index 000000000..72a20d37a --- /dev/null +++ b/control_table/ym080_230_a099.model @@ -0,0 +1,115 @@ +[type info] +name value +value_of_zero_radian_position 0 +value_of_max_radian_position 262144 +value_of_min_radian_position -262144 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 2 Bus Watchdog +10 1 Secondary(Shadow) ID +11 1 Protocol Type +12 1 Baud Rate (Bus) +13 1 Return Delay Time +15 1 Status Return Level +16 1 Registered Instruction +32 1 Drive Mode +33 1 Operating Mode +34 1 Startup Configuration +38 2 Position Limit Threshold +40 4 In-Position Threshold +44 4 Following Error Threshold +48 4 Moving Threshold +52 4 Homing Offset +56 1 Inverter Temperature Limit +57 1 Motor Temperature Limit +60 2 Max Voltage Limit +62 2 Min Voltage Limit +64 2 PWM Limit +66 2 Current Limit +68 4 Acceleration Limit +72 4 Velocity Limit +76 4 Max Position Limit +84 4 Min Position Limit +96 4 Electronic GearRatio Numerator +100 4 Electronic GearRatio Denominator +104 2 Safe Stop Time +106 2 Brake Delay +108 2 Goal Update Delay +110 1 Overexcitation Voltage +111 1 Normal Excitation Voltage +112 2 Overexcitation Time +132 2 Present Velocity LPF Frequency +134 2 Goal Current LPF Frequency +136 2 Position FF LPF Time +138 2 Velocity FF LPF Time +152 1 Controller State +153 1 Error Code +154 1 Error Code History 1 +155 1 Error Code History 2 +156 1 Error Code History 3 +157 1 Error Code History 4 +158 1 Error Code History 5 +159 1 Error Code History 6 +160 1 Error Code History 7 +161 1 Error Code History 8 +162 1 Error Code History 9 +163 1 Error Code History 10 +164 1 Error Code History 11 +165 1 Error Code History 12 +166 1 Error Code History 13 +167 1 Error Code History 14 +168 1 Error Code History 15 +169 1 Error Code History 16 +170 1 Hybrid Save +212 4 Velocity I Gain +216 4 Velocity P Gain +220 4 Velocity FF Gain +224 4 Position D Gain +228 4 Position I Gain +232 4 Position P Gain +236 4 Position FF Gain +240 4 Profile Acceleration +244 4 Profile Velocity +248 4 Profile Acceleration Time +252 4 Profile Time +256 2 Indirect Address 1 +512 1 Torque Enable +513 1 LED +516 2 PWM Offset +518 2 Current Offset +520 4 Velocity Offset +524 2 Goal PWM +526 2 Goal Current +528 4 Goal Velocity +532 4 Goal Position +541 1 Moving Status +542 2 Realtime Tick +544 2 Present PWM +546 2 Present Current +548 4 Present Velocity +552 4 Present Position +560 4 Position Trajectory +564 4 Velocity Trajectory +568 2 Present Input Voltage +570 1 Present Inverter Temperature +571 1 Present Motor Temperature +634 1 Indirect Data 1 +919 1 Backup Ready +256 2 Indirect Address Write +634 1 Indirect Data Write +384 2 Indirect Address Read +698 1 Indirect Data Read diff --git a/control_table/ym080_230_b001.model b/control_table/ym080_230_b001.model new file mode 100644 index 000000000..72a20d37a --- /dev/null +++ b/control_table/ym080_230_b001.model @@ -0,0 +1,115 @@ +[type info] +name value +value_of_zero_radian_position 0 +value_of_max_radian_position 262144 +value_of_min_radian_position -262144 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 2 Bus Watchdog +10 1 Secondary(Shadow) ID +11 1 Protocol Type +12 1 Baud Rate (Bus) +13 1 Return Delay Time +15 1 Status Return Level +16 1 Registered Instruction +32 1 Drive Mode +33 1 Operating Mode +34 1 Startup Configuration +38 2 Position Limit Threshold +40 4 In-Position Threshold +44 4 Following Error Threshold +48 4 Moving Threshold +52 4 Homing Offset +56 1 Inverter Temperature Limit +57 1 Motor Temperature Limit +60 2 Max Voltage Limit +62 2 Min Voltage Limit +64 2 PWM Limit +66 2 Current Limit +68 4 Acceleration Limit +72 4 Velocity Limit +76 4 Max Position Limit +84 4 Min Position Limit +96 4 Electronic GearRatio Numerator +100 4 Electronic GearRatio Denominator +104 2 Safe Stop Time +106 2 Brake Delay +108 2 Goal Update Delay +110 1 Overexcitation Voltage +111 1 Normal Excitation Voltage +112 2 Overexcitation Time +132 2 Present Velocity LPF Frequency +134 2 Goal Current LPF Frequency +136 2 Position FF LPF Time +138 2 Velocity FF LPF Time +152 1 Controller State +153 1 Error Code +154 1 Error Code History 1 +155 1 Error Code History 2 +156 1 Error Code History 3 +157 1 Error Code History 4 +158 1 Error Code History 5 +159 1 Error Code History 6 +160 1 Error Code History 7 +161 1 Error Code History 8 +162 1 Error Code History 9 +163 1 Error Code History 10 +164 1 Error Code History 11 +165 1 Error Code History 12 +166 1 Error Code History 13 +167 1 Error Code History 14 +168 1 Error Code History 15 +169 1 Error Code History 16 +170 1 Hybrid Save +212 4 Velocity I Gain +216 4 Velocity P Gain +220 4 Velocity FF Gain +224 4 Position D Gain +228 4 Position I Gain +232 4 Position P Gain +236 4 Position FF Gain +240 4 Profile Acceleration +244 4 Profile Velocity +248 4 Profile Acceleration Time +252 4 Profile Time +256 2 Indirect Address 1 +512 1 Torque Enable +513 1 LED +516 2 PWM Offset +518 2 Current Offset +520 4 Velocity Offset +524 2 Goal PWM +526 2 Goal Current +528 4 Goal Velocity +532 4 Goal Position +541 1 Moving Status +542 2 Realtime Tick +544 2 Present PWM +546 2 Present Current +548 4 Present Velocity +552 4 Present Position +560 4 Position Trajectory +564 4 Velocity Trajectory +568 2 Present Input Voltage +570 1 Present Inverter Temperature +571 1 Present Motor Temperature +634 1 Indirect Data 1 +919 1 Backup Ready +256 2 Indirect Address Write +634 1 Indirect Data Write +384 2 Indirect Address Read +698 1 Indirect Data Read diff --git a/control_table/ym080_230_m001.model b/control_table/ym080_230_m001.model new file mode 100644 index 000000000..72a20d37a --- /dev/null +++ b/control_table/ym080_230_m001.model @@ -0,0 +1,115 @@ +[type info] +name value +value_of_zero_radian_position 0 +value_of_max_radian_position 262144 +value_of_min_radian_position -262144 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 2 Bus Watchdog +10 1 Secondary(Shadow) ID +11 1 Protocol Type +12 1 Baud Rate (Bus) +13 1 Return Delay Time +15 1 Status Return Level +16 1 Registered Instruction +32 1 Drive Mode +33 1 Operating Mode +34 1 Startup Configuration +38 2 Position Limit Threshold +40 4 In-Position Threshold +44 4 Following Error Threshold +48 4 Moving Threshold +52 4 Homing Offset +56 1 Inverter Temperature Limit +57 1 Motor Temperature Limit +60 2 Max Voltage Limit +62 2 Min Voltage Limit +64 2 PWM Limit +66 2 Current Limit +68 4 Acceleration Limit +72 4 Velocity Limit +76 4 Max Position Limit +84 4 Min Position Limit +96 4 Electronic GearRatio Numerator +100 4 Electronic GearRatio Denominator +104 2 Safe Stop Time +106 2 Brake Delay +108 2 Goal Update Delay +110 1 Overexcitation Voltage +111 1 Normal Excitation Voltage +112 2 Overexcitation Time +132 2 Present Velocity LPF Frequency +134 2 Goal Current LPF Frequency +136 2 Position FF LPF Time +138 2 Velocity FF LPF Time +152 1 Controller State +153 1 Error Code +154 1 Error Code History 1 +155 1 Error Code History 2 +156 1 Error Code History 3 +157 1 Error Code History 4 +158 1 Error Code History 5 +159 1 Error Code History 6 +160 1 Error Code History 7 +161 1 Error Code History 8 +162 1 Error Code History 9 +163 1 Error Code History 10 +164 1 Error Code History 11 +165 1 Error Code History 12 +166 1 Error Code History 13 +167 1 Error Code History 14 +168 1 Error Code History 15 +169 1 Error Code History 16 +170 1 Hybrid Save +212 4 Velocity I Gain +216 4 Velocity P Gain +220 4 Velocity FF Gain +224 4 Position D Gain +228 4 Position I Gain +232 4 Position P Gain +236 4 Position FF Gain +240 4 Profile Acceleration +244 4 Profile Velocity +248 4 Profile Acceleration Time +252 4 Profile Time +256 2 Indirect Address 1 +512 1 Torque Enable +513 1 LED +516 2 PWM Offset +518 2 Current Offset +520 4 Velocity Offset +524 2 Goal PWM +526 2 Goal Current +528 4 Goal Velocity +532 4 Goal Position +541 1 Moving Status +542 2 Realtime Tick +544 2 Present PWM +546 2 Present Current +548 4 Present Velocity +552 4 Present Position +560 4 Position Trajectory +564 4 Velocity Trajectory +568 2 Present Input Voltage +570 1 Present Inverter Temperature +571 1 Present Motor Temperature +634 1 Indirect Data 1 +919 1 Backup Ready +256 2 Indirect Address Write +634 1 Indirect Data Write +384 2 Indirect Address Read +698 1 Indirect Data Read diff --git a/control_table/ym080_230_r051.model b/control_table/ym080_230_r051.model new file mode 100644 index 000000000..72a20d37a --- /dev/null +++ b/control_table/ym080_230_r051.model @@ -0,0 +1,115 @@ +[type info] +name value +value_of_zero_radian_position 0 +value_of_max_radian_position 262144 +value_of_min_radian_position -262144 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 2 Bus Watchdog +10 1 Secondary(Shadow) ID +11 1 Protocol Type +12 1 Baud Rate (Bus) +13 1 Return Delay Time +15 1 Status Return Level +16 1 Registered Instruction +32 1 Drive Mode +33 1 Operating Mode +34 1 Startup Configuration +38 2 Position Limit Threshold +40 4 In-Position Threshold +44 4 Following Error Threshold +48 4 Moving Threshold +52 4 Homing Offset +56 1 Inverter Temperature Limit +57 1 Motor Temperature Limit +60 2 Max Voltage Limit +62 2 Min Voltage Limit +64 2 PWM Limit +66 2 Current Limit +68 4 Acceleration Limit +72 4 Velocity Limit +76 4 Max Position Limit +84 4 Min Position Limit +96 4 Electronic GearRatio Numerator +100 4 Electronic GearRatio Denominator +104 2 Safe Stop Time +106 2 Brake Delay +108 2 Goal Update Delay +110 1 Overexcitation Voltage +111 1 Normal Excitation Voltage +112 2 Overexcitation Time +132 2 Present Velocity LPF Frequency +134 2 Goal Current LPF Frequency +136 2 Position FF LPF Time +138 2 Velocity FF LPF Time +152 1 Controller State +153 1 Error Code +154 1 Error Code History 1 +155 1 Error Code History 2 +156 1 Error Code History 3 +157 1 Error Code History 4 +158 1 Error Code History 5 +159 1 Error Code History 6 +160 1 Error Code History 7 +161 1 Error Code History 8 +162 1 Error Code History 9 +163 1 Error Code History 10 +164 1 Error Code History 11 +165 1 Error Code History 12 +166 1 Error Code History 13 +167 1 Error Code History 14 +168 1 Error Code History 15 +169 1 Error Code History 16 +170 1 Hybrid Save +212 4 Velocity I Gain +216 4 Velocity P Gain +220 4 Velocity FF Gain +224 4 Position D Gain +228 4 Position I Gain +232 4 Position P Gain +236 4 Position FF Gain +240 4 Profile Acceleration +244 4 Profile Velocity +248 4 Profile Acceleration Time +252 4 Profile Time +256 2 Indirect Address 1 +512 1 Torque Enable +513 1 LED +516 2 PWM Offset +518 2 Current Offset +520 4 Velocity Offset +524 2 Goal PWM +526 2 Goal Current +528 4 Goal Velocity +532 4 Goal Position +541 1 Moving Status +542 2 Realtime Tick +544 2 Present PWM +546 2 Present Current +548 4 Present Velocity +552 4 Present Position +560 4 Position Trajectory +564 4 Velocity Trajectory +568 2 Present Input Voltage +570 1 Present Inverter Temperature +571 1 Present Motor Temperature +634 1 Indirect Data 1 +919 1 Backup Ready +256 2 Indirect Address Write +634 1 Indirect Data Write +384 2 Indirect Address Read +698 1 Indirect Data Read diff --git a/control_table/ym080_230_r099.model b/control_table/ym080_230_r099.model new file mode 100644 index 000000000..72a20d37a --- /dev/null +++ b/control_table/ym080_230_r099.model @@ -0,0 +1,115 @@ +[type info] +name value +value_of_zero_radian_position 0 +value_of_max_radian_position 262144 +value_of_min_radian_position -262144 +min_radian -3.14159265 +max_radian 3.14159265 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed +Present Current 1.0 raw signed +Goal Current 1.0 raw signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 2 Bus Watchdog +10 1 Secondary(Shadow) ID +11 1 Protocol Type +12 1 Baud Rate (Bus) +13 1 Return Delay Time +15 1 Status Return Level +16 1 Registered Instruction +32 1 Drive Mode +33 1 Operating Mode +34 1 Startup Configuration +38 2 Position Limit Threshold +40 4 In-Position Threshold +44 4 Following Error Threshold +48 4 Moving Threshold +52 4 Homing Offset +56 1 Inverter Temperature Limit +57 1 Motor Temperature Limit +60 2 Max Voltage Limit +62 2 Min Voltage Limit +64 2 PWM Limit +66 2 Current Limit +68 4 Acceleration Limit +72 4 Velocity Limit +76 4 Max Position Limit +84 4 Min Position Limit +96 4 Electronic GearRatio Numerator +100 4 Electronic GearRatio Denominator +104 2 Safe Stop Time +106 2 Brake Delay +108 2 Goal Update Delay +110 1 Overexcitation Voltage +111 1 Normal Excitation Voltage +112 2 Overexcitation Time +132 2 Present Velocity LPF Frequency +134 2 Goal Current LPF Frequency +136 2 Position FF LPF Time +138 2 Velocity FF LPF Time +152 1 Controller State +153 1 Error Code +154 1 Error Code History 1 +155 1 Error Code History 2 +156 1 Error Code History 3 +157 1 Error Code History 4 +158 1 Error Code History 5 +159 1 Error Code History 6 +160 1 Error Code History 7 +161 1 Error Code History 8 +162 1 Error Code History 9 +163 1 Error Code History 10 +164 1 Error Code History 11 +165 1 Error Code History 12 +166 1 Error Code History 13 +167 1 Error Code History 14 +168 1 Error Code History 15 +169 1 Error Code History 16 +170 1 Hybrid Save +212 4 Velocity I Gain +216 4 Velocity P Gain +220 4 Velocity FF Gain +224 4 Position D Gain +228 4 Position I Gain +232 4 Position P Gain +236 4 Position FF Gain +240 4 Profile Acceleration +244 4 Profile Velocity +248 4 Profile Acceleration Time +252 4 Profile Time +256 2 Indirect Address 1 +512 1 Torque Enable +513 1 LED +516 2 PWM Offset +518 2 Current Offset +520 4 Velocity Offset +524 2 Goal PWM +526 2 Goal Current +528 4 Goal Velocity +532 4 Goal Position +541 1 Moving Status +542 2 Realtime Tick +544 2 Present PWM +546 2 Present Current +548 4 Present Velocity +552 4 Present Position +560 4 Position Trajectory +564 4 Velocity Trajectory +568 2 Present Input Voltage +570 1 Present Inverter Temperature +571 1 Present Motor Temperature +634 1 Indirect Data 1 +919 1 Backup Ready +256 2 Indirect Address Write +634 1 Indirect Data Write +384 2 Indirect Address Read +698 1 Indirect Data Read From ed28160192c397158da776bb01668c95a62971a5 Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Mon, 1 Sep 2025 07:43:27 +0900 Subject: [PATCH 02/37] Add exception in input param of getAllMotors Signed-off-by: Hyungyu Kim --- c++/src/dynamixel_sdk/dynamixel_api/connector.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp b/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp index be8359450..38d6b0920 100644 --- a/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp +++ b/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp @@ -52,6 +52,9 @@ std::shared_ptr Connector::getMotor(uint8_t id) std::vector> Connector::getAllMotors(int start_id, int end_id) { + if (start_id < 0 || start_id > 252 || end_id < 0 || end_id > 252 || start_id > end_id) { + throw DxlRuntimeError("Invalid ID range. ID should be in the range of 0 to 252."); + } std::vector> motors; for (uint8_t id = start_id; id <= end_id; ++id) { try { From 73c37dc547d26d5bc3df088c7913a08791a1387b Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Mon, 1 Sep 2025 09:01:50 +0900 Subject: [PATCH 03/37] Use Makefile macro for control table path instead of hardcoding Signed-off-by: Hyungyu Kim --- c++/build/linux64/Makefile | 5 +++-- c++/src/dynamixel_sdk/dynamixel_api/control_table.cpp | 5 ++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/c++/build/linux64/Makefile b/c++/build/linux64/Makefile index 9df49c5a6..d2a0f87a6 100644 --- a/c++/build/linux64/Makefile +++ b/c++/build/linux64/Makefile @@ -35,8 +35,9 @@ SYMLINK = ln -s MKDIR = mkdir CC = gcc CX = g++ -CCFLAGS = -O2 -O3 -DLINUX -D_GNU_SOURCE -Wall -c $(INCLUDES) $(FORMAT) -fPIC -g -CXFLAGS = -O2 -O3 -DLINUX -D_GNU_SOURCE -Wall -c $(INCLUDES) $(FORMAT) -fPIC -g +CONTROL_TABLE_PATH_FLAG = -DCONTROL_TABLE_PATH=\"$(INSTALL_ROOT)/share/dynamixel_sdk/control_table\" +CCFLAGS = -O2 -O3 -DLINUX -D_GNU_SOURCE -Wall -c $(INCLUDES) $(FORMAT) -fPIC -g $(CONTROL_TABLE_PATH_FLAG) +CXFLAGS = -O2 -O3 -DLINUX -D_GNU_SOURCE -Wall -c $(INCLUDES) $(FORMAT) -fPIC -g $(CONTROL_TABLE_PATH_FLAG) FORMAT = -m64 INCLUDES += -I$(DIR_DXL)/include/dynamixel_sdk INCLUDES += -I$(DIR_DXL)/include diff --git a/c++/src/dynamixel_sdk/dynamixel_api/control_table.cpp b/c++/src/dynamixel_sdk/dynamixel_api/control_table.cpp index e0de7e7dd..04248d8fd 100644 --- a/c++/src/dynamixel_sdk/dynamixel_api/control_table.cpp +++ b/c++/src/dynamixel_sdk/dynamixel_api/control_table.cpp @@ -23,7 +23,7 @@ namespace dynamixel const std::map ControlTable::ParsingModelList() { std::map tmp_model_list; - std::string file_name = "/usr/local/share/dynamixel_sdk/control_table/dynamixel.model"; + std::string file_name = std::string(CONTROL_TABLE_PATH)+"/dynamixel.model"; std::ifstream infile(file_name); if (!infile.is_open()) { throw std::runtime_error("Error: Could not open file " + file_name); @@ -76,8 +76,7 @@ const std::map & ControlTable::getControlTable(ui } const std::string & model_filename = getModelName(model_number); - std::string base_path = "/usr/local/share/dynamixel_sdk/control_table/"; - std::string full_path = base_path + model_filename; + std::string full_path = std::string(CONTROL_TABLE_PATH) + "/" + model_filename; std::map control_table; std::ifstream infile(full_path); From be4e0a7f77c7eee9205907dd3776c17979b696a9 Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Mon, 1 Sep 2025 09:24:11 +0900 Subject: [PATCH 04/37] remove duplicate model_list Signed-off-by: Hyungyu Kim --- control_table/dynamixel.model | 2 -- 1 file changed, 2 deletions(-) diff --git a/control_table/dynamixel.model b/control_table/dynamixel.model index b21f5dd9d..2814bee74 100644 --- a/control_table/dynamixel.model +++ b/control_table/dynamixel.model @@ -13,8 +13,6 @@ Number Name 1050 xh430_v210.model 1060 xl430_w250.model 1070 xc430_w150.model -1070 xc430_w150.model -1080 xc430_w240.model 1080 xc430_w240.model 1090 2xl430_w250.model 1100 xh540_w270.model From c5309475b1a1d24f95552fd99815b324e838cd1b Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Mon, 1 Sep 2025 10:40:15 +0900 Subject: [PATCH 05/37] fix indentation Signed-off-by: Hyungyu Kim --- c++/src/dynamixel_sdk/dynamixel_api/connector.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp b/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp index 38d6b0920..073699b3e 100644 --- a/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp +++ b/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp @@ -25,7 +25,7 @@ Connector::Connector(const std::string & port_name, float protocol_version, int } port_handler_ = std::unique_ptr(PortHandler::getPortHandler(port_name.c_str())); packet_handler_ = std::unique_ptr( - PacketHandler::getPacketHandler(protocol_version)); + PacketHandler::getPacketHandler(protocol_version)); if (!port_handler_->openPort()) { throw DxlRuntimeError("Failed to open the port!"); From 2ecc213f970298b220348af133c438a164a994ff Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Mon, 1 Sep 2025 16:19:03 +0900 Subject: [PATCH 06/37] make getmotor function return unique_ptr Signed-off-by: Hyungyu Kim --- .../dynamixel_sdk/dynamixel_api/connector.hpp | 4 ++-- c++/src/dynamixel_sdk/dynamixel_api/connector.cpp | 12 ++++++------ 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/c++/include/dynamixel_sdk/dynamixel_api/connector.hpp b/c++/include/dynamixel_sdk/dynamixel_api/connector.hpp index 850982481..001b491ef 100644 --- a/c++/include/dynamixel_sdk/dynamixel_api/connector.hpp +++ b/c++/include/dynamixel_sdk/dynamixel_api/connector.hpp @@ -35,8 +35,8 @@ class Connector virtual ~Connector(); - std::shared_ptr getMotor(uint8_t id); - std::vector> getAllMotors(int start_id = 0, int end_id = 252); + std::unique_ptr getMotor(uint8_t id); + std::vector> getAllMotors(int start_id = 0, int end_id = 252); Result write1ByteData(uint8_t id, uint16_t address, uint8_t value); Result write2ByteData(uint8_t id, uint16_t address, uint16_t value); Result write4ByteData(uint8_t id, uint16_t address, uint32_t value); diff --git a/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp b/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp index 073699b3e..8b940f090 100644 --- a/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp +++ b/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp @@ -41,25 +41,25 @@ Connector::~Connector() port_handler_->closePort(); } -std::shared_ptr Connector::getMotor(uint8_t id) +std::unique_ptr Connector::getMotor(uint8_t id) { Result result = ping(id); if (!result.isSuccess()) { throw DxlRuntimeError(getErrorMessage(result.error())); } - return std::make_shared(id, result.value(), this); + return std::make_unique(id, result.value(), this); } -std::vector> Connector::getAllMotors(int start_id, int end_id) +std::vector> Connector::getAllMotors(int start_id, int end_id) { if (start_id < 0 || start_id > 252 || end_id < 0 || end_id > 252 || start_id > end_id) { throw DxlRuntimeError("Invalid ID range. ID should be in the range of 0 to 252."); } - std::vector> motors; + std::vector> motors; for (uint8_t id = start_id; id <= end_id; ++id) { try { - std::shared_ptr motor = getMotor(id); - motors.push_back(motor); + std::unique_ptr motor = getMotor(id); + motors.push_back(std::move(motor)); } catch (const DxlRuntimeError & e) { continue; } From c469d8bf1e8490ec4b82b2a782e4db69f0e035c4 Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Mon, 1 Sep 2025 16:41:16 +0900 Subject: [PATCH 07/37] apply guard clause to improve readability Signed-off-by: Hyungyu Kim --- c++/src/dynamixel_sdk/dynamixel_api/control_table.cpp | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/c++/src/dynamixel_sdk/dynamixel_api/control_table.cpp b/c++/src/dynamixel_sdk/dynamixel_api/control_table.cpp index 04248d8fd..0439bcdf7 100644 --- a/c++/src/dynamixel_sdk/dynamixel_api/control_table.cpp +++ b/c++/src/dynamixel_sdk/dynamixel_api/control_table.cpp @@ -23,7 +23,7 @@ namespace dynamixel const std::map ControlTable::ParsingModelList() { std::map tmp_model_list; - std::string file_name = std::string(CONTROL_TABLE_PATH)+"/dynamixel.model"; + std::string file_name = std::string(CONTROL_TABLE_PATH) + "/dynamixel.model"; std::ifstream infile(file_name); if (!infile.is_open()) { throw std::runtime_error("Error: Could not open file " + file_name); @@ -48,7 +48,7 @@ const std::map ControlTable::ParsingModelList() } catch (const std::out_of_range & e) { throw std::runtime_error("Model number out of range in " + file_name + ": " + number_str); } catch (...) { - throw std::runtime_error("Unknown error while parsing model number in " + file_name); + throw std::runtime_error("Unknown error while parsing model list in " + file_name); } } } @@ -60,12 +60,11 @@ const std::string ControlTable::getModelName(uint16_t model_number) { static const std::map model_name_list_ = ParsingModelList(); auto it = model_name_list_.find(model_number); - if (it != model_name_list_.end()) { - return it->second; - } else { + if (it == model_name_list_.end()) { throw DxlRuntimeError( "Model number is not found in dynamixel.model: " + std::to_string(model_number)); } + return it->second; } const std::map & ControlTable::getControlTable(uint16_t model_number) From 986575369f038a856b5f585ddc0f790e3b7edae0 Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Tue, 9 Sep 2025 08:48:11 +0900 Subject: [PATCH 08/37] delete tab Signed-off-by: Hyungyu Kim --- c++/build/linux64/Makefile | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/c++/build/linux64/Makefile b/c++/build/linux64/Makefile index d2a0f87a6..27b569c03 100644 --- a/c++/build/linux64/Makefile +++ b/c++/build/linux64/Makefile @@ -62,10 +62,10 @@ SOURCES = src/dynamixel_sdk/group_bulk_read.cpp \ src/dynamixel_sdk/protocol1_packet_handler.cpp \ src/dynamixel_sdk/protocol2_packet_handler.cpp \ src/dynamixel_sdk/port_handler_linux.cpp \ - src/dynamixel_sdk/dynamixel_api/connector.cpp \ - src/dynamixel_sdk/dynamixel_api/control_table.cpp \ - src/dynamixel_sdk/dynamixel_api/motor.cpp \ - src/dynamixel_sdk/dynamixel_api/dynamixel_error.cpp + src/dynamixel_sdk/dynamixel_api/connector.cpp \ + src/dynamixel_sdk/dynamixel_api/control_table.cpp \ + src/dynamixel_sdk/dynamixel_api/motor.cpp \ + src/dynamixel_sdk/dynamixel_api/dynamixel_error.cpp OBJECTS=$(addsuffix .o,$(addprefix $(DIR_OBJS)/,$(basename $(notdir $(SOURCES))))) From f28c6209d174a80179cbf1a719e5edc74e729a42 Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Tue, 9 Sep 2025 09:49:48 +0900 Subject: [PATCH 09/37] apply broadcastPing to getAllmotors function Signed-off-by: Hyungyu Kim --- .../dynamixel_sdk/dynamixel_api/connector.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp b/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp index 8b940f090..852c71638 100644 --- a/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp +++ b/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp @@ -55,14 +55,16 @@ std::vector> Connector::getAllMotors(int start_id, int en if (start_id < 0 || start_id > 252 || end_id < 0 || end_id > 252 || start_id > end_id) { throw DxlRuntimeError("Invalid ID range. ID should be in the range of 0 to 252."); } + std::vector ids; std::vector> motors; - for (uint8_t id = start_id; id <= end_id; ++id) { - try { - std::unique_ptr motor = getMotor(id); - motors.push_back(std::move(motor)); - } catch (const DxlRuntimeError & e) { - continue; - } + int dxl_comm_result = packet_handler_->broadcastPing(port_handler_.get(), ids); + if (dxl_comm_result != COMM_SUCCESS) { + throw DxlRuntimeError(getErrorMessage(static_cast(dxl_comm_result))); + } + for (auto &id : ids) { + if (id >= start_id && id <= end_id) { + motors.push_back(getMotor(id)); + } } return motors; } From f67d1d93b1b071961fb427772cbd894fae60bf1f Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Tue, 9 Sep 2025 10:02:23 +0900 Subject: [PATCH 10/37] makefile typo error Signed-off-by: Hyungyu Kim --- c++/build/linux64/Makefile | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/c++/build/linux64/Makefile b/c++/build/linux64/Makefile index 27b569c03..3f8ccff44 100644 --- a/c++/build/linux64/Makefile +++ b/c++/build/linux64/Makefile @@ -65,7 +65,7 @@ SOURCES = src/dynamixel_sdk/group_bulk_read.cpp \ src/dynamixel_sdk/dynamixel_api/connector.cpp \ src/dynamixel_sdk/dynamixel_api/control_table.cpp \ src/dynamixel_sdk/dynamixel_api/motor.cpp \ - src/dynamixel_sdk/dynamixel_api/dynamixel_error.cpp + src/dynamixel_sdk/dynamixel_api/dynamixel_error.cpp OBJECTS=$(addsuffix .o,$(addprefix $(DIR_OBJS)/,$(basename $(notdir $(SOURCES))))) From f0485ca540d2e780a05e0730ef9a9e6ec48249de Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Tue, 9 Sep 2025 13:37:16 +0900 Subject: [PATCH 11/37] add example Signed-off-by: Hyungyu Kim --- c++/example/dynamixel_api/basic_test.cpp | 336 +++++++++++++++++++++++ 1 file changed, 336 insertions(+) create mode 100644 c++/example/dynamixel_api/basic_test.cpp diff --git a/c++/example/dynamixel_api/basic_test.cpp b/c++/example/dynamixel_api/basic_test.cpp new file mode 100644 index 000000000..1ec401eac --- /dev/null +++ b/c++/example/dynamixel_api/basic_test.cpp @@ -0,0 +1,336 @@ +#include +#include +#include +#include +#include +#include + +#include "dynamixel_sdk/dynamixel_api.hpp" + + +int getch() +{ + struct termios oldt, newt; + int ch; + tcgetattr(STDIN_FILENO, &oldt); + newt = oldt; + newt.c_lflag &= ~(ICANON | ECHO); + tcsetattr(STDIN_FILENO, TCSANOW, &newt); + ch = getchar(); + tcsetattr(STDIN_FILENO, TCSANOW, &oldt); + return ch; +} + +int kbhit(void) +{ + struct termios oldt, newt; + int ch; + int oldf; + + tcgetattr(STDIN_FILENO, &oldt); + newt = oldt; + newt.c_lflag &= ~(ICANON | ECHO); + tcsetattr(STDIN_FILENO, TCSANOW, &newt); + oldf = fcntl(STDIN_FILENO, F_GETFL, 0); + fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK); + + ch = getchar(); + + tcsetattr(STDIN_FILENO, TCSANOW, &oldt); + fcntl(STDIN_FILENO, F_SETFL, oldf); + + if (ch != EOF) + { + ungetc(ch, stdin); + return 1; + } + + return 0; +} + +int main() { + int baudrate = 57600; + std::cout << "Dynamixel API Source Test Code" << std::endl; + std::cout << "┌─────[Test Process]────┐" << std::endl; + std::cout << "│ 1. Ping Test │" << std::endl; + std::cout << "│ 2. Torque ON/OFF Test │" << std::endl; + std::cout << "│ 3. Position Test │" << std::endl; + std::cout << "│ 4. Velocity Test │" << std::endl; + std::cout << "│ 5. LED Test │" << std::endl; + std::cout << "│ 6. Reverse Mode Test │" << std::endl; + std::cout << "└───────────────────────┘" << std::endl; + std::cout << "Baudrate set to: " << baudrate << std::endl; + std::cout << "Scanning all motors..." << std::endl; + + dynamixel::Connector connector("/dev/ttyUSB0",2.0 , baudrate); + std::vector> motors = connector.getAllMotors(); + + if (motors.size() == 0) { + std::cerr << "No motors found!" << std::endl; + return 1; + } + std::cout << "┌───────────[Motor List]──────────┐" << std::endl; + for (auto & motor : motors) { + std::cout << "│ ID: " << static_cast(motor->getID()) + << ", Model: " << motor->getModelName() <<" │"<< std::endl; + } + std::cout << "└─────────────────────────────────┘" << std::endl; + std::unique_ptr motor1 = std::move(motors[0]); + std::cout << "The test is conducted only on the motor with ID " << static_cast(motor1->getID()) << std::endl; + + std::cout << "Press any key to continue! (or press ESC to quit!)" << std::endl; + if (getch() == 0x1B) + return 0; + auto result_uint16_t = motor1->ping(); + if(!result_uint16_t.isSuccess()){ + std::cerr << dynamixel::getErrorMessage(result_uint16_t.error()) << std::endl; + return 1; + } + std::cout << "┌──────[Ping Test]─────┐" << std::endl; + std::cout << "│Ping result: " << result_uint16_t.value() << " │"<< std::endl; + std::cout << "└──────────────────────┘" << std::endl; + + std::cout << "Press any key to continue! (or press ESC to quit!)" << std::endl; + if (getch() == 0x1B) + return 0; + auto result_void = motor1->enableTorque(); + if(!result_void.isSuccess()){ + std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; + return 1; + } + + auto result_uint8_t = motor1->isTorqueOn(); + if(!result_uint8_t.isSuccess()){ + std::cerr << dynamixel::getErrorMessage(result_uint8_t.error()) << std::endl; + return 1; + } + std::cout << "┌──[Torque ON/OFF Test]──┐" << std::endl; + std::cout << "│Torque ON result: " << static_cast(result_uint8_t.value())<< " │" << std::endl; + + result_void = motor1->disableTorque(); + if(!result_void.isSuccess()){ + std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; + return 1; + } + + result_uint8_t = motor1->isTorqueOn(); + if(!result_uint8_t.isSuccess()){ + std::cerr << dynamixel::getErrorMessage(result_uint8_t.error()) << std::endl; + return 1; + } + + std::cout << "│Torque OFF result: " << static_cast(result_uint8_t.value()) << " │" << std::endl; + std::cout << "└────────────────────────┘" << std::endl; + + std::cout << "Press any key to continue! (or press ESC to quit!)" << std::endl; + if (getch() == 0x1B) + return 0; + result_void = motor1->setPositionControlMode(); + if(!result_void.isSuccess()){ + std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; + return 1; + } + std::cout << "───────────[Position Control Test]─────────────" << std::endl; + std::cout << "Position Control Mode set." << std::endl; + + + result_void = motor1->enableTorque(); + if(!result_void.isSuccess()){ + std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; + return 1; + } else { + std::cout << "Torque ON." << std::endl; + } + + int32_t current_pos; + int target_position = motor1 ->getMaxPositionLimit().value(); + + result_void = motor1->setGoalPosition(target_position); + if(!result_void.isSuccess()){ + std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; + return 1; + } + std::cout << "Target Position:" << target_position << "." << std::endl; + + auto result_int32_t = motor1->getPresentPosition(); + if(!result_int32_t.isSuccess()){ + std::cerr << dynamixel::getErrorMessage(result_int32_t.error()) << std::endl; + return 1; + } + current_pos = result_int32_t.value(); + + while (abs(target_position - current_pos) > 10) { + std::cout << "\rCurrent position: " << current_pos << std::flush;; + result_int32_t = motor1->getPresentPosition(); + if(!result_int32_t.isSuccess()){ + std::cerr << dynamixel::getErrorMessage(result_int32_t.error()) << std::endl; + return 1; + } + current_pos = result_int32_t.value(); + } + std::cout << "\rTarget position reached: " << current_pos << std::endl; + + target_position = 0; + usleep(1000000); + + result_void = motor1->setGoalPosition(target_position); + if(!result_void.isSuccess()){ + std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; + return 1; + } else { + std::cout << "Target Position: " << target_position << "." << std::endl; + } + + result_int32_t = motor1->getPresentPosition(); + if(!result_int32_t.isSuccess()){ + std::cerr << dynamixel::getErrorMessage(result_int32_t.error()) << std::endl; + return 1; + } else { + current_pos = result_int32_t.value(); + } + + while (abs(target_position - current_pos) > 5) { + std::cout << "\rCurrent position: " << current_pos <<" "<< std::flush; + result_int32_t = motor1->getPresentPosition(); + if(!result_int32_t.isSuccess()){ + std::cerr << dynamixel::getErrorMessage(result_int32_t.error()) << std::endl; + return 1; + } + current_pos = result_int32_t.value(); + } + std::cout << "\rTarget position reached: " << current_pos << std::endl; + + result_void = motor1->disableTorque(); + if(!result_void.isSuccess()){ + std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; + return 1; + } + std::cout << "Torque OFF." << std::endl; + std::cout << "───────────────────────────────────────────────" << std::endl; + std::cout << "Press any key to continue! (or press ESC to quit!)" << std::endl; + if (getch() == 0x1B) + return 0; + + std::cout << "───────────[Velocity Control Test]─────────────" << std::endl; + result_void = motor1->setVelocityControlMode(); + if(!result_void.isSuccess()){ + std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; + return 1; + } + std::cout << "Velocity Control Mode set." << std::endl; + + result_void = motor1->enableTorque(); + if(!result_void.isSuccess()){ + std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; + return 1; + } + std::cout << "Torque ON." << std::endl; + + int32_t current_vel = motor1 ->getPresentVelocity().value(); + int target_velocity = motor1 ->getVelocityLimit().value(); + std::cout << "Target Velocity: " << target_velocity << std::endl; + result_void = motor1->setGoalVelocity(target_velocity); + if(!result_void.isSuccess()){ + std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; + return 1; + } + while (abs(target_velocity - current_vel) > 5) { + std::cout << "\rCurrent velocity: " << current_vel << std::flush; + result_int32_t = motor1->getPresentVelocity(); + if(!result_int32_t.isSuccess()){ + std::cerr << dynamixel::getErrorMessage(result_int32_t.error()) << std::endl; + return 1; + } + current_vel = result_int32_t.value(); + } + std::cout << "\rTarget velocity reached." << current_vel << std::endl; + usleep(3000000); + target_velocity = -target_velocity; + result_void = motor1->setGoalVelocity(target_velocity); + if(!result_void.isSuccess()){ + std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; + return 1; + } + std::cout << "Goal velocity set to " << target_velocity << "." << std::endl; + + while (abs(target_velocity - current_vel) > 5) { + std::cout << "\rCurrent velocity: " << current_vel << std::flush; + result_int32_t = motor1->getPresentVelocity(); + if(!result_int32_t.isSuccess()){ + std::cerr << dynamixel::getErrorMessage(result_int32_t.error()) << std::endl; + return 1; + } + current_vel = result_int32_t.value(); + } + + std::cout << "\rTarget velocity reached." << current_vel << std::endl; + usleep(3000000); + result_void = motor1->disableTorque(); + if(!result_void.isSuccess()){ + std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; + return 1; + } + std::cout << "Torque OFF." << std::endl; + std::cout << "───────────────────────────────────────────────" << std::endl; + std::cout << "Press any key to continue! (or press ESC to quit!)" << std::endl; + if (getch() == 0x1B) + return 0; + + std::cout << "───────────[Direction Test]─────────────" << std::endl; + result_void = motor1->setReverseDirection(); + if(!result_void.isSuccess()){ + std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; + return 1; + } + std::cout << "Reverse Direction set." << std::endl; + + target_velocity = motor1 ->getVelocityLimit().value(); + motor1->enableTorque(); + motor1->setGoalVelocity(target_velocity); + std::cout << "Set goal velocity :" << target_velocity << std::endl; + usleep(3000000); + motor1->setGoalVelocity(0); + motor1->disableTorque(); + result_void = motor1->setNormalDirection(); + if(!result_void.isSuccess()){ + std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; + return 1; + } + std::cout << "Normal Direction set." << std::endl; + + std::cout << "Press any key to continue! (or press ESC to quit!)" << std::endl; + if (getch() == 0x1B) + return 0; + + std::cout << "───────────[LED Test]─────────────" << std::endl; + result_void = motor1->LEDOn(); + if(!result_void.isSuccess()){ + std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; + return 1; + } + std::cout << "LED ON." << std::endl; + + result_uint8_t = motor1->isLEDOn(); + if(!result_uint8_t.isSuccess()){ + std::cerr << dynamixel::getErrorMessage(result_uint8_t.error()) << std::endl; + return 1; + } + std::cout << "LED ON result: " << static_cast(result_uint8_t.value()) << std::endl; + sleep(2); + result_void = motor1->LEDOff(); + if(!result_void.isSuccess()){ + std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; + return 1; + } else { + std::cout << "LED OFF." << std::endl; + } + + result_uint8_t = motor1->isLEDOn(); + if(!result_uint8_t.isSuccess()){ + std::cerr << dynamixel::getErrorMessage(result_uint8_t.error()) << std::endl; + return 1; + } + std::cout << "LED OFF result: " << static_cast(result_uint8_t.value()) << std::endl; + + return 0; +} From 6ed01569bd7253951db0169dd48c2f0da1f7a504 Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Tue, 9 Sep 2025 14:13:52 +0900 Subject: [PATCH 12/37] lint Signed-off-by: Hyungyu Kim --- c++/example/dynamixel_api/basic_test | Bin 0 -> 72120 bytes c++/example/dynamixel_api/basic_test.cpp | 128 +++++++++++------- .../dynamixel_sdk/dynamixel_api/connector.cpp | 12 +- c++/src/dynamixel_sdk/dynamixel_api/motor.cpp | 3 +- 4 files changed, 85 insertions(+), 58 deletions(-) create mode 100755 c++/example/dynamixel_api/basic_test diff --git a/c++/example/dynamixel_api/basic_test b/c++/example/dynamixel_api/basic_test new file mode 100755 index 0000000000000000000000000000000000000000..402e85515f53548030bb226b0f3b2145e73494c4 GIT binary patch literal 72120 zcmeHw3w#vS_5TEjq9O@M2q5aBsHhOLdGJycNEQ+Z7)d}B>9A}zWO1{*ZZ;wKi~>p; z(A4^%Ra?|*)ndig2h~>NbG4$4{aH(W;;Lor{22{r z@AsU0?z!il`<&U?D?Jr6`ebBi^6IOdrLojRj!YGd=0T>+RBfVmq&8I>rMa|2!0(4Y zDQBhI`hbZ`(*8WD^#!GT!{~zz9m!!$Qdv4t%I9=VJ%Mv1)v|e-@@3M;e4e(_m9sf6 zshW?K*CO&+=trYDE~%Q2e3Q7J^AnFJ(|l%(rga|5ahZ?YMfukLisNgaNTx2~Pf4Xe z$wvB{DEw)q)@obx)rovoTFQ&pB&9Ows`@xq)0^@tMZOJdxgIMWE&L*>Ebm<8Bfs?Y z63wp76XiLz!!S{vq%zJ<4+fTwFFHL~GcFhiH8ziHo;ZHo_@caqa9#m3f$VbOkFw94 zJ6}VybPy)osP$>#X{nERLj<4XFTeHOvwrhd$Nm2M?tSgra|Zq@I)1`)Btv#n9+IJp z^ob^C=|T8+mcS@<2KR06m#?||W*;^!d-ar-5hO%>3g-&^&B0$K{^WHoKFq^kwEz}? zF2vsj_**2f;v1%{{#VZK-IGS_-raoc?%jz;Z~yCw^#e-EhgL4gxbyy9!C!Bgao%fR zuD*GnZ`q2_4I?`JuhuV{@68^vVL-{H{crvDu+_JG?YUyMN2s$UZ9oUxaNaz^9={#| zYbQU$LC>{toSl3ooN0$|g){B&+Z@XM9|t=hLy+3(>2}b6E*jfT{x%2wuQ}9f%VG9< zjyT*N|Ca;4)S=uv9q^kS@XZeTFLS8xkI|9r{P2Q<{!<4(&C> z!Jdr{dOkp>wyW1%2YZSf{4mr(&u#~M9(KUzIkZc{p}rd(+UryY{O=Cs{?4JFUXOBV zOtW66I>hs<4*mB@hjMRnz(+ag+2nxlaj@rp2Yj#tzR|(|!yMw_0f)F+<Z!pb?O*_e*-;)M+81h;MXADju)R*@;>lESnW^k;3&e6T{ok@5Z=UTJ+y0{9~=}V&EjNS?tvG^6EKyjX&yN7HEk1qt$atg5i+A+PgI9*K~bZ zeK@2y#Jtg%u5(d)!3*Y2s#zKG)(4vX!QwgLSUBn~Zt%xuhP}ZB{$SV_h^_Q2*Ys4e z34w+RPw6=!Pm>+Dpa4oL!wrF0ARH$^?-axFO+~=wC;vbKvHb4e|J#neGMsM;) z1Kv;PO^ZZTzXoJ5r5Jiw6l$ckltL(jER}KqA8Odn^ zk+)D$1hMAOz!PE4+;Fts8%(w)wF8ZZhh>pw$W+A&#_M{lE*f5;r$*{hJLBCGp{X+J zZ}5kV&cr=X-g6I(r+}!fb*hzFf;5?zu)wi|^c6v)^ZvNM9l4|+UDUXXVW=k>4M*po zUwfDNrx&wf!ZWu@pIF))WGoLa*>)tz=MLEaXm%IsRk|-6S*g?T)8MJ}p^8-#D5|UB zq)Gl}pFcve*}G~_&@qa8V_xX(odUNT8hqY}myOl+WEP?$$*iha5CbL})OG}&2Sy#{gHw ziu|FPpq)J)XH&+rQKT2GS3uqghfuLbU#uKW=kYXE>s1qVgmHdtIJ&|ctZSWVBhXS#rN>t96R~4?(QEALnUf!dTsd~X&cRriEyh{U3?gD&n z2BGhc1QZ~>tMN2W0htQHSUmbUo&mWC)YuxT&co`-`0O2S% zhGp>vF-3c0eq9MdeOYs}9>J&?4tax^zV)Vjri2;;lPpcw6d*||NdQyL{&ra-cG(($ z&BwexG#~G;GPVFfOu-fwuc_f`7aqQ9#A&f^j5^8fChA)iE3OSTHq`x>Vr~+J9Qskr z@0nMRKEawQ>SsegYl{8$jS+vz&4%h372jWXV_2J`V@lJ5{?M{morm|`{k9AtkMS$e z5D7Q%RS&wiFWiWZ&T5UGZbWo1VuBe-As*yoB*huxVv?Lkm0r<%M-=E-4uySsT>z_T za;aRkuo~1x{Yl+xerQDim73>oXsq{(TGj@rt6k#P8XJOse?+VGgSeP;VYzr28TfobVEYT)xvzqvPK3lw9V60hDr6%E$4UB_(=cUcOecaN)G+<+?ks zFmIAJv!Z-D05FR33fu}$q_BGmy$; zsY8Hf0wXTbzS^PqBOaH@(E2kD*V6}RKgR0=l=Nn@bU*PgNgXEgk}g>~$x)iyLjRhd z=@V&FrB4{Z&#_&oMde8=v9mi&I|CS9WWf-@r*r9*uRRF zM1FZ+M(#Jai~C+(;(m?9GnSBcP1|e1HwyXwVqYoc<-RxDg3EpWFbgiv1zZ+f?zfvQ zxZJn*?_g%3#)68pET4`6#^O)j4`~s42%nBWd2N1*e}c0#x+fs7EeejUxN&tT_)!L` zX)h}H$qL@7;PQ-v*xMBR3`Kssf|n?GLcvQFyi395nFz77JGc>MDKyVrN zLWm*H69_+_0CCB43Bu2lAgDZ-Abg>M%X1jQ&r$G>Kkx*OYY@BU^`e5K0~lAQg5yZV zxV9-cjy8;IyMj~M@=7Q;wXwXq6r605*IosevBh{#atV?}M>4Md3XTqKT-gdPx*X#T zQ*d-x<8mqZKm*mZF$$ig;Q0zJW1QF%6!F^c>K1wU57H!ApX z3VyGGyA*u0f{#-0Eed|Tf_EtR2@3wAf{#}4P6a+NeaGQ!N(|gLcvc_@Gb>E zRl)Zv_-P8RiE)PFKTpB?E4Vz{C3d!g=PUBV6x^-gE(I@8@G%NrsNneuUZmg?6`U<6 z1aGQ>k5}Z&6kJScj9aPTlN9-d3NFuxiM>R@&s5~=6nu(;M-==l1#edHvlYBW!KW(t zS_Pk`;Oz?TQSc24K10DbD)>wVzgNM_6#U@FgC02OfrB16=z)VCIOu_c9ysWMgC02O zfrB3Sf5iizXC3?RvQ@kKm$hZScD<&Rt?7vM>Fz9B^<4jFSrxjAf4E-L_H>`v29kB0 zM)GyU*|X#Q?(Xh(mZpQOJv&}A(sY2eXUEe118kp zNYlX*>Tjg!Kne9X(sYo7`WtCFKtlbEG#wnF{zjS(j8K0gO$SA&zmcW`BGlhV)4>qx zZ={c9=?o)%982&1kF39orQbKwbnt`v8)-W5LH&(19rU37Mw$+IP=6y$2Ro?0k){J3 z)Za*-%+ePdX*$?J{f#so=%D^anhtVMeKJ4C-&B=|BecH_~*lj`|yEI+#KIjWiv|p#5bUz5S*(>g}Z(>+h6qO-`@3 zq}N!|ms`?}mUO)(?YE>CThj9_={c6P$C936Nf%qv<1FbDE$L${=^RV?NJ~1?lK#)y zWPg2WNq=fde{4y=V@dzTl77XKe%_M)gC+flCH;UUeYYijyCr>-CB5E~USmmLZb>&< z()E_K-;!QzNzb>W=UCDnOL~eWU2I8@v!qY7q>r(rb1dm2E$K{4`ajoN+TW7?)RO+# zl77dM{);93iY5KLCH)6W`VmX|0X6OZw7m7TAC$FzP`2u$uF7hUyTkob+4?D8U>fhv zxfPOM)aGR!w;F>476|L7ybq*oede1)Chd*oVCirjea6z^uqTpYrcm0VaKzk9=iU?b-+p zHDzu6P8`EjdG@+H%GR%mK&Y(s1$NGxI1yuiSsTaNZfa&M&*rk$`CH1`JS}By^VgQO zdODDP5$SfMHz3`K^fsh7;`8=4&%LdlM4RXLtG0jH>iK=yddmI`yQ{#9=z}5>U+WvK@xWyjB-|bGH6x*4dmmA_KJysd zQ$Wz*Qz6PKLm9JC2NZoFx=UH>8-$!s$okB|hG>5w>f)kqQ#6l@<{F~!FkU?IB`k;5 zLrl@WT=Z~5^fyBEk6iSvRfYlE2w9)`pHgY-wLaEt%U(C`H|+pD27J71jDIieMrrqLSr^Cw_fArqD+_sQunc+)W;8 zeFhot5h~EA+rP5r9m4WLjC{;G-xbiplT6QgmWA}xnR+%5*p_o&3mM09-9rdd5>Nc` zOZqIPhj?NI)pb3e5+us<91F(kj;uA^*fv4`QASmVGsnggZeGlEvzW7aG5nr8HFuOb zGM>1OQJA@A3^|S)mBp%(d8(nfgK3H5WkC&hcSVE1hg%i{LL%6keNkINRjeyVbh<#U^ewNZPE<6cw#iTJkBr2ZWq}a zrjsG0;3q6cJn;}$@XQs&U`_TEA?q_QHAF)~^crF?k4hUmVPn=~_+JF%7gTmD1=3uR z8v%I^FJXyU!eU;+c%y^?q6D%R9xgUT3%Ka2Y0~cZ884pr`g10FxG8!lL=&gM!`8;Q z3Oj$LF)~jeUGc;+($Ko5j5h`&K(9|~42j-iX^fXCFY5~A%X(qHhuhS2x#7zOLoS}U zWshu(E*woL2E~r&2GvqsTh~-_gBXE9zfLkpqK{e(nrswA2E8qu^e~EqKR>?AFz5q9 zL?1rHFi2+xWqvu8XpN#;ixp;p!{0 zkwr(o$c^Z5^Ae_;C7jJl5PdwJSb*M2&GjPFZ*8vaXiT&3$mU7_Rr}6Jrc(BuCxyi~ z?`GbKUut;gIYNYY5?!*nrr~5sX|Cs8+@Qy}L9Djxd8@(9?j(bxD-Oi~pc>R>6hz)0 zCk)Eu28}ljDm3KciSrDDHsk0E*;buRhLD1-ECz)N z)f{N4=5(e~R`VlK^zTVyTh6D=Mm7IIh^Xd3qngWb#)@VhW7xim85>XB$`x!f72L-Z z7`AsD!)?FeQ|N77)6B;#M$mARF`~6;t!rEy9zz6h8_R?xDB%cPgkB%AKc$HeM;A+N z;=w@GCZ1u`iTXf7nBGAawdL$yVbuMfgowHi#%JEd7va1a*(Mlqdo8PMJn?f@r6eoQ zWL8qI87{#oEi9B$59k0b1JNlJW{vjzxvkZYHJ3i4^{b!v?Mv?=sW!g}GW-jVa zV!V~ktq_HjM zc(dr^47qrs1YBO#+i-h9sp?)*E~?7o?KEDWXa4nOR{1ll_8y*f{kMEgdf^$;18Z0z zSkJRsBza&mZ|qwe4G(O9P@F|F9H~7ub$XVTSe@*ZTSt&|QX~x~iIjLBfsTbBc6DW#FMS8U{+GR zpCZH&@7MBzHpC1U+{6kp;(gO_;=~g_+d&F>)S1Dz1*4r=M;QbiK-D@%nMxT1Lqr{S zk;b;1W6YvQ7;^E%S>W56gxe=QiNCE2vHxXiXf=$DSgZd|I>z~xdM*S^q9Azqv`imyoMH*3mv*;0q9O@6Q zs6Xz>sr4u2={iAX*7ZAhCpd4SbV~~>B%av7YLVoYVjclEM-31Bn1oCZ+@H%mFp{iG ziSiNraaRqN)Y}o!+Le^()G(@ivqCexJ51OKzxafb1q}D}3w1JDRHbt)jlDHd=vaWHy z!TnSe_q0)5uF!Eh*D=-9F@+Up^sDNjRBSwPh^<%OMbxEw^%J0~S4S|F(yPB0Mc?=d z^Xl^v!zs@YBF2tSK9*iRo7?U;Y+uBT#d!@^u-a5`6;oi?z9EO({%`0?jfmrIZ9mmw z`wtA8sM~77_HCrGE$1+^=tB&-c;ZyU_EosAjcldGWC$tvIm;1GJjNBg7&a>ZJRxQ@ zzdeN89wOhRtZ{CpxJBn0#;5f$BAkCIG7bO8F4HIsk(6mP%anQgcxlw(!l>iP1R9J< zf=|9raUZ)(JC8}WvxpU$l&9@ORyylr>-hp20rXo05(|~A)g>7y+2lM1jv6jI$$Dd| zSuc$96WW$@bI9=6j}1Ba6J_#Fn0*8(&-58CAIofyCrU}2YDZ4vwR@kp!Ud+zvzbmK zf)>yk2F53tXRNcz(xeEIqv~=?1dTMzqyTtQSaUNusVyg7Z&c?wLPYc@K9JR!h9@VK zQT29GE*hS<(`mdua%q84RV*zUjRE6SR=;>+I%}9D54=alwdG7T&6onAr#fC3rzHx( z``7KGS+p1)DIH7(C$;yj?~`M#JEdn47^y?U%Rtq(?nXt;w!T9cx{}bgocn`@bAD;a z#S<^1Jl@u$4K( ztrbtqBeX4N)kRbQTNJbyawu=Rbklzhg@CyjI-ivHEQZ_#+*Ru{KVgN$6W>Csr5D}G zow#qgVaC@G;xY7DvM&zFCYgAmnc$R=X-90OhRg<_YRGIQ3yk3*BwA($p=~)UOe123 z9M%)>Nnh+fgnW@XG+$QiNKvs9xq|VgfDC-DQv_BU;f1DJ3 zOo;x8i*64PgEi4RKoS{<1?wDr9=GgE!?FgUqmAph&D8M|Ld*!;Jdm`<6HCd86t7N3 z{H1zzI#AWCO-!YXuwlX}UGK7*jxvir){w(^0xs`}w`RhE^_e#qwm-&l#1ntw3bxmo z^(Vx%{fi^H?X~YP+nc)?ml4z`L~U?V1 zU}hh~%xq!i7^WyG%kxyxww#kp%TIt1kLuyXGwYC#0-vOf7&XFrUQ{I6rXz*LS8|I* zHKoO$4d8iy#GPQ7Iag-cHQ-f1)dqZo@*3UZJW;XJ32n<+vdr-RVnZ&TxC*xN27I$G z1lDKn%Og1&Nxxz_;)#J=!EvU7kxW5m{i#y)Wg+?@)x0g|i&~?s-9QpKh)Qd$U&M>M z&nWI%q2uRV$7WN<1B95qe(ng;9#7183vf!T&qYs2jr9dURagCxsg$ukmbGv^(T^8h zY!+Q$$l)Xy<#AU%*asG1sxfSTndOKlJ|vS+f4^COLQLEH3)>IpK~TvMGR@K{;TlbP9g-heBpG!@KY3XGwp<}hx1Emb$=e#p-Z+b=+U zWgFdNvHeuTCUWssQS>ih9c=%L&#?W^goqX2SKEZ`JLe%!Kf~}4voxN#hGdf(r~{>{ z4K#$j-Imj6>W`8?*tlfI;A*Phde27cL-b}ud~T8bsTGgP%ug84v}COwif+1N0REw; zT3G4SAb6et#_l}S1evzc%))KN{|Ovma#@z4$g<-mM2iLg_OHPGsrV;^&q>8!CA>Hl zf0XcJQt_V=POliYlzR=~pCU{wc!==7q~Z$*|3fN%7U6fN;wKTlJ{2EGcw;L5)f>PU zr{eDtPOq%A*!cqC<5KZo6P}Zb-%R*_5DXSQR}lVjDqc(YE2;Qg!XHV+#}j^gDt;{C zYf|w;fbWc0jM@DcfQu~P+XT+AfN=ud7VtL&jhgUVjb~hS`CdGaq%{_xE5ExG zoeWuvkrm04mQa#TH=k#W7s}c)|E+V8C`@fPwLVEYo~@8@YL~05_3{Kb2|V?-cMJ04DTvrW4#X4pL*vdPWh(D=Rwnz_0DBmZ zbd%Dysr{3bW3s8!aZX=>F~w!Si(*N&~!`;wu^^wP7YcVI!1AKIw4O!|FWA1&l~-j+WToFw@~ZSb64*r{!ZYk%R4lBSd~vIXZ(lUJ-?$YqZ1DUQrOc&n?Ue~ zr?;3k_RN;IKk9p|?B=4B>8PLRq(>v9Ax;w+f1nf(Xa1GSV|_NYEkzxlVKe_T$cf-D z!`E+!!Uk66W5g7KGx1q%X`{zficI~>T06^5d66EdO>RLKTn4j}pL4~Oi*}M>bQv!w zrGbW`0_M;F_uK12pIQ$yOnqY=^u2%-nPji<9wnT+8I5b&mE6}@-za-}J04T+Ox3F(NqrAJ zl<8)@^eU)lN$R+5gK6kgN=wQ|&2RMXJCggX#b1kQv}I1pBK>^ix`!fCz{e6Cr;e(% zCr=%uX$*C2h;Ab{e|{)wKnqYSApYU0J%zA`ZySV3t6z~O?Q{sn%-JJVX~=Zwcqidap0 z6xxLUc9NVlNw&9*MYEwJsYUx37G1)Y9n4ANxJBR2mlmx)*Km?ovFK>4MWaoN&bPD3 z+Jgr{LD~pRZqfOFlJHzv0>`?V<4H!si zbs7*4<(xtlZ)4w7L1s2FEuh(gt#>eytlFE^GzHJB(^S0eahf&6G95P$-gA)R_)Ee7RuSY-O1(&-yM+h$6+vVF8ChC zHEV8h%+9o#r?BD47}E}<)y>JOEoSXb=2F3{F5j!MU8-u15;G(t1Mf7?UbXL7l)nq* zNpZ;cH#|G_)m4{wX|Y^B1W?zVgD%WAX20b*4GW}VRxYjajSl`qY8gqrbsP9ta67l# zUm&24FtR9CL_9&iSL2+Senp2y($Pt?cG9>9?)zi|=%WZ=XI=O8NNUVJ(Fty%Zzm0v z1LbUFrZ4ft6LdxiS z4>pqRt>31T+6&F!?b1fFibjlNvwmur)%GW}VohWZN8qe9&3wmVLQRL~=xZH~y8r;>}|6!U%r=ZRS{tJ1? zS$$BjvlQLX`|j>?+1GXfu+*?>mSUB~S!n;%(`s{|r_(SR-PxeZM)(?NbvUHiIb4ov zF=6%MMb#?e9Qa@L+p^0{$eMTGU9Vi;!W$8UzxyS9wCYk`08I_QCe z9ysWM|4SaAUvHs{|Ep;~{3$mCKY%lNGJZg3vP=9Fq$|*XkHJQN5I-EJ(a-7#{)%Wg zw9JJc^YJ&k@Izoyo~zMH<*#D8rd5`^s=|#?pWjvOZ-}`{@H>{8_Q8$oKDe<({eKbT zRz}10%ae;$&IdQPL;h-)JI_^#9~k6nT=D{kaSQTX{1*{j=gd9*oEbB?nB*2BulPMp zCBNhr<+e)|H=H=ySr`PzKPLIV_&aaxDugwh$4qB-( zR5OYXbD8Tx5i6s{fe77#}JdgXnN@7cSwk0~9yX9q_;C zZ;MhpiD4YG$>6&T!(fv{(XR|xF&Fv|RGgzVVRdRWT98FXcP`<-kIOoqQ59%O_rI2s zoAq6EtUOItbuK=7qZkW~kz+D``s2uw#%L5jDC)vbwy~e*MZ+B_GM2|etg+w zz#|7p)IK>C^*Gci!#N0M zPR;Ji(iDK=gUOl4MkH6Ax4{((yV$R0=4r_wTM{64)nA#OJX!s*P*bH-OeX*xfx9^xVze z-J5XIebPhS-CIE~{SEHT;t7PK@DOAtzJ?e8r5Dj$4Z0cfKPMdYUBW@X0L?}|Hy*vr z2fYNe0`y_fAm|@K*MaT>-359Qo;@A_{gt3G&?e9uK|cb07_=WANPZpkMbLeq*>81s zyYK|eM9`_At3j85z7E;~I^^x{?v2C;?Esw%x*aqKN`U}t^d}VJdXf^0A(0f3$@tpV;(0tHOKZpxOB9%1=S*TP~YGD?#4|jevd$+73D#UlZL7 zIsvp3bOC4=Xc#mbUnKZ3Xg=r`&`QuZKqH`^fwqGl^>^e4b%S<-R)Thc(zjo;@kN() zp!uM8gI0pl7pfzme+O*`%}gLa=+U5^pr?R#ftG@1W6xg$nh$zAXeH>2pb^j=pzWaj zcOXCLNuZse6`)<9^o{szd{b%zXg=tNpp~G9;e0a!dOB!3=t|Jdpg#xg1brT~3p5*t z#@YA|(|FK)&n?bukJ3;&IL4MFtpxGFw3PJNh z&jqan^?^n}n?Tz^e*wA~lrFf9T^X0m(=wW~Gmai`Xn#8MBplCwXs-LayH6sC(nAqp znl>7LMfe+YC(6Xxyf!d<#=v2-vW{5MzePKH#N<;8M;{NA<-ZWL>u%UYfVfC+41WpW z^!&QKsH}DP`{Qff-FUo4(>w#Sujx~ASp{K0AX zlr7_u$_f?*hu%6VAH$Q31!{BpJNZ|bH}Rq0W9aO%eacq#?R)KL!_G<^1iTI(o=6I% z%!Q^M5%Avxzh33{sWI}mgFgm#;-NV+{{o($Y@iq*{!J1L@%V;}lb#s#xWH$27pLek?4}r43pr-@@Dw>_ zH`VVs$nBAG|G|ix><6QMv|hdN4QIOsz~2{u&+K|SMUP=u5#*kM9JA~06gi{3C0Or{ z#rn7yHX1esdHa&x6a&-2r!S55yGYT4IO#iq4R5Sn?uDKuSU=m13oht+4tj`xEqPlZ_H6<`iu@SOO#5b;1?LvDGr;$79CVR`6*LK0@W6Yw~x2KNftuF@*Zx0PI1D zKS|9WG4qcF-vj=sDqmS+Re)a!zSH;+1V5dB)`|R%?GOH3=x6J3*}oQ>_H6~9+Rur< z3;YYx@COXw@jY9uU!Uo$0P@dR@Y~YluK@o?Y4}0#ZvlT+lKyFi{&nC#ohJV#@Z)Lt zTfu(?d?)*NfxkCR{sA}x_%;oHEcpF!M&qQv0{qj#ck+J_{NgnHb>N=?zLS48fnN*0 zQ~%rwz7D=q|J((>FHQLa(6It(_+!BjgYRTt1^7QnlRpUl&(rYNfqysnPX65lerKBe zTfu)L4SyH-Z-eh-|9~t_I|OHLAm;co!|Y#U!T1vS3sio<Dt@FQu;AAo`{PLqEu_%wbv*;fI^f;9Xf_+{YR?FHES7yQL( z@^1qF9Ppjy@2%j^1>dQE?E=3d4SxU@*0a*^$AVAxJFUMfz&JAvKL~z)8vZ))>AfUQ z`Zs|wEDe7v_(y>6Sby*j10QZM$2VoaKLCew-(dc8%0CwTufcc9UjhEkH2fg=?}9&0 zEuSC#usz;7@OOcKy2@Af7n{I8;}6~43sim|e!xfZvlaY{!MB?qDNA>E#`S%tY{O3Z z(SDgXmuP?Mmw8o*)|Q$1S()~D<}H2REYsdPbnL?=+F$$U;`8Ri{(_ehXg42`yA{w8 zPxL|Ju7SB9m1rLfoQBU2XXWlI(Z0;uN>YCwlzU^T_M^eX_;7IUrc&(}N3~Oq=W}wO zE7h(UdJ{=)%FTVdRQojdeoEXsJok%IZO8B%D6#J7+;tx9*`x2G#J@)7{=%dE`k22_ zV#9H{zxQZ=JC2F%bLGD7(SAE>3vu=xpSuf*6CS6;x1)1c&(NMaaWxhC`pLPs&CqTd zvmBq>Psx3FhW6SicMxOM*xao%wCBcNj?aG{m;2EStu>F;^0U)(_s!5Y=kqETtQ+J!csdn*I2x1N|W;zFOMy^xW4O`nXhf}$R5O$8+JILD9XM8lSfUF^&apY{qq zt;guXkt@5t{I)wxA2wt4MHl8+eu;u`G|#RjILD{!a89&agkeXBG~K7Ai`^6BAKAY` zt#DI9~Zbg;Cv6ORJ0^akPBuSHsxBZ@m5~QP9nTb_m)j z=ypN71l0;e`GUFx%@=g4pp}9y5i}xbi=gd-ZWMI0pdEsC3c6j;E!paFl~Q_18Wa zt@177HsvFHi3KOO5pEsl%YkQLx4QWmd|;nBozFh!8si^FaeosZNROO9B)*o(4>iV9 ziT@HG?CSd{lOL%)WGVM~h7Z>Ix8MUVF??#f@Hs;ptPN-()SM3vheHWJvW3CMJTXS# z11&gxPm$!aEO=1h*%o|_zz13Ip8+R7{7To@#rE*{^s2x!mT+9oGfwUJccy=ccJG~B zUj5h3Z(l+lPwZ(M$l=P>zYo=GxMm#}2QhqvF^|YBCj&no_ODyQ5q8Flk23{c+r)8p z_K%Mn1b*u!9GCOOPJst&IetAS+Azd1>91aqEZ+)zu#i#7V?w@aB}b(Uz08C3WG_$F zqYFG9kSzV3fe=`CG!@7JAqjBY1xUK1#!^FXqoNLjDUOKgQ1iS;rsr=X##>a{N>wf2Y8I zCHyAi;XQ$WBjQBr$-_b*8;g-)9YTR1#lXoiYwMR|x+M74@QZg^Lzn9Rd*K zQpigiRL)(%2N9=*qX<`a{YL1&O|;7_fq&zmCl~8WvU6*Y3(9uCQsAG|a9q~wX@TDn z<+$|!V60k@%~DdK!KhqZSE-s$5&g}{q&kIJsT=K?=o$fzXfApbiD zobHj4okK!gi>%kMERIjLjAK6(_>H2yP8Ef}CGe>5v&2Vcb3L1ualspee7V3U3wb%+ z-Xw6Bh^tXTej9Ka2aY;}1N7V>UGFfQe^&n;J&2cEDdI3sFs}q|=g+%@{3g-9LcKN| z!gliXOEToQyyaZ+^+Nx}0=LH7;{qSCgbRvohW0A(L4A(a`gaHgg1jZS_3 zoJ|Zwt+&XS&AtTwDE9NOVFEu11aUI8(3 zWT8F&gwSIhHwKL0p2yba2wM#3J7=@Y96-($Aj&AB26x)DEs! z#@k-tcJ_}%0Ndd~;8bp@X!p~(I_+lyzaqea8wCEcgPxBZ@bfSslKx0DS0Kmdp9=i4 zr5u;@<_7{Fv4rEO4!gb*c&BK0bZ2(eySV;cbv!NY{HehEiTOhIyH5q)F5(1cv+F__ zMs|)A`YQ$gkiZLsf20rTN03Q=Uo97u@Q8U=o@z)u$VR)Omx9%c#r z^b_pc>&AS-v+*S+E-2&ee8^L|OT8R-3q5Nb^t>eGJ1zY>AC4kDA2)FY1wzm50{=#| zqx8d32uPB*u6q{(moh>HBlQmW4;}Eo3;nx-T+6va*)&v~%C*K%tH1|}xDwUVegoXj zo|hePdI*d3Th|-2(Qt(STEq{+fL+%Ke4?fQehA#I+`=(jPn#GwWV_!i@Hd42vxNRV z!0q&$bBaA4bil85z;7p<4u26}>U9@zs_#ZioE&p1Uxy6qP^Ix*G~gsZp@HLYBfHiJ zyu#w2j|H9(wie}Ygz7< z!0q(h>43)_@P9hs{l@Wf|70omY~Xg~-T<7&`IZh|&+H6N?`1gutp5A9(EpOvpLx98 z+eE*3R%BW%@CR0MK-%ySaQRu)|Fr`?8WRV#d#y#!4+Q?Mz@?vG68J|KbL}!trXv1G z LA;yU28pVc~4Yc%avA^#`Ky#Kktr&;Woje!xPytd_;WIdI@soZ~tI6hpk*9m-y zuv7N;Z315`;%c^#pHX1%w+n!i{!jc|uujO|D)1+Te(8q+h4y;rR~kuAev~Va4{qA{ab+#7Ijz2Q$pT4j}M-}^*p(p zOWzq$4|1a?-U1oF>tcyjTqN6T-bA+z`qdfBI9tkgPubti+VM21+#>nG{hQfYx8_sjX&yNhSx&*qk61f_XWcte*+|I!uqmcc&Rt2*Tlln2Ho4(tog$A zk)R(hU&_n3m(Xhip@8mQmc)6>-~C7V}1QfWKmK&Bx6aU{PiRBqNhAx zB%Utndl1R>-r0dHS7xuU=g3;nfTylhDihJ6?5k#I2JTj?RO@wy&q zhy^G4oBh7VSRfoKXmVE+Hux`2!;N^O&fH1h>W!ha8j1~mg?m!@+^TAQ#x(p>b^hEE zeV%7pDGWC-BZrE#fVuIm>R1?Enwk#CNhSlS%)M5E!ThptlJIo0{HK(}7qh^W#7h%KgMR;7}$n7y~m1H+1B zd)MIgbuoW=m0O3Z33yFSo!?vInK!$tn5i@i9nW8LQy!aJr7H#$vzOp_;4nm3p)`HJ zN}V^GTvZrjhQpkC1bg|yY8`eKGq8MCX#`b-fkm;E5kFZs$zmO^SwR6_EhJxHCEmV7 zuTK)@O~BjuR(PW|;H^RIG=>l&>D+!!wN7f>riaFbt;`fJhm zLq7O&f-VfDR-T>e!a%YMCj}a+8h!L~zZ5sdl*Ut>?qMZQ62YN+gBWi@2=XSs@S7Po z7SC0UXO6Qms_8q7>V@+hjQ4nY>^rpw)H$dtW^Y<#u5;*er3K(NhNx#xomo*ny+kj_ zE66MU?mp-yN9XJN6&L7_6cvGx9tnDV=!x_mNSy~XD$aUp@kLAP0x{|{bE~}NJ`Wkl zEnB!yFU%`qoynb#(E(G5mtqP-hKFib96(chF^A}}s5cO6C`UJO7uN)8MeymYgH#pj z^na{id=!ITxL)_N@Y2o9n6=mlWz!3c5eT*KiT~9R$edK%6LdYqUgu?$T3(3??LUjD9*!U*D57jP50K;YY7o!1&QVazX_}B-qH`>{_SL$ZrQ@sEx z>|VY_V_OmR@^W~j7gZ=AYtOMCEzp2`R6D0?BCknrN}y`M9p_XPkq$8uEB-YHk6CqW zte=PBOKn{-PVlHe=#^m_(6NZ{V-|7S%@~Su(RiMUrEYZ&VYc{dyfN>0$C_ofbL#7A ztj$)pYJcmaU zb?*Xw0UM@x&*_y@DWqtGL(6(heYBnvqtx8qH&#S6w{4slPQo5+R7ITHDl^v+bm;TqY=q* zLHvD}M#@yvn?9j7U-44x1zB*wH(s5kbe|Xdi>W0G7fzdAuDkOJv9};5_Ij*)rY=zS z*Ry<9EYj5lMP7j$FQ|?N{h?I-z3ljIH+mknFSLD#^3ItqtPb2lind0yg0$(^G}rWS zRe`aKhCwzIniD7bu-tO&&%TK%aN00g7S_s!iAId?w_#%=6E8SvI&|7CrkM`wy@3#$ zoQnLRnxI|BrRAWzkauqk<#?5EJ(i!eF~f39jW(v{KuoPP2=du=9!3o+N%R)H)jN!i z@*2|WRAC4Zb^*^3I-axdAYH+>KWn)l!ULH=h@^B*A+^k3Z(S|UsQDQ6gCY)GflW8|rET84n zjE-Z4y&`qXFbjt;%0QuR@X~0t-({(K9dxEUpTfZd->&zrL%9pl-?1swu@qYttMk;S z_Aa;svBL+QV0anMBH0luMjKUDE*qnMr%90R9*E9r9tS!t<}HV42YMefiElRb>Ifzl zHg)6hnw@_s-PW>CD8N02d9!_dbW?6u(1adn(CH=WxKR-ZVfk5((=pG|N?H@@!N5`< z-2jNjbXw9F^F|?^s?mx|9jtr1U{p4Q#%+`kPU)cjKpt>;5ZySIwOhjQg_oylCZDIw zSVtA@4gpe+ww*$u$H%kw{kCqF(>X?(uF8%MoQIAyL)-q(X{-xU4B}=S_6xQXTral) z6c&6nZrccAhwU`~Ir*(;^B?A9yLKPXZ)t>L=o8_ncNxy}um)44R(7#et6@&5uBj2yLk>iz=<&WM>U-rs&koHRdb?pYWg%g*j}x0v2>e8Gm>5x zz`c8U#Fn&A;x`9yBhMJ>?FQrnJ@(g#fJ$x7I;ZBO>CL!L(wmt`J-iAud+HHR#!X!e z8aCS|^gU~I@>|L57;b;=>55yr2e1Z(ILKg~(ztnJQ-L1bmT@}RvEB(cy43+~2X^z4 z@3{_G+ZHw8Mu2dQdC%Ty;m(@!d)#a`BUoj^j^<(_f4<9^Jj{e;Rbbx|8_`$h& z@@0B4J7N}wV}L^RSr(0UXH-6Vl%QBoSwO2d_4f1BkKOm_LFn+u6kz6fW7#VY!s(2^ zw|x}H=lL~$c?gC&kJl?vL*8F|u@rP_`5rF?={9z+YC}^2yZw!!rY2a~RIi-lsDdrDTKw0qYx>9Gsh zGkbAzh+S3{?(aob>U8%HJ=lj(#BFk$b)Ol7e0~^D_f%_ZX_%*tJf27N)m^05;xRd6 zky@n1dEEY8|wn&#xH~yCwYen>=1T(mLPKkVGkd^`av3=^v!4wWc<->ma7xq0+ zZtVePvpDP{EYGc>w!Jv~pb5adzfMo!n0F}DSn1_xpHG{sKm&e%63wIPGsgJuXd#E^ zznr!w&U<;pmOBa0XolDW6YQpSFNPUUEuC>8~ve2A5%Ui&{~l(@^W^zhIF!E6!2 z48Y{7duwWNE2|+~+q^_>gOEH#%=*(2C-(J+1`mDj19Jx5W;33Mrl(1kg`;>7G~JWd6H?^qHZXfu zKq*)Zj`@KGeln~)AgE5u)6a~e%) z2#lm>YV2pUK)8WDep*zHXMPc5Ny}d9AqVh?Rfdw;>EvF)`ZQD0;phQBSK;R&wB3@= zbHi8reHT7lX?%;tte&e?uMWopd1G56@j|=99R^S_G zI(>Pw-ai#DF~;}H=;N3UBREia1oufQ?DZh!qZqYDJ4y!Z-AKRJt z{0js02$2uZ9LMne&w883rv6tNAn7(sC3e$viv^wKKu=)onH1%Yj=Y;gMcH_;G@m(G z9f53i$jEzO@6BRpw%J{{9Ud`p8WWSx8GAJf^=Mg`W(zt|eTdIoyYP5sUm}))Z2b~3 z8pJ%GwE5a5HCWObjeS?erX^Ck5f+uoH~65(#??+w-Z(E*<& zVKa}JqDUaLjP*$hfCGHj(A3Ai1t!WBxq6b|Wtm4dZMxwo-wXHU@$<;{r7T!q>yb z(aS}cc%8SQPRm2kp$JZ6QO;??W7+sVYf?f7FX|6^DTDYF3C6TMc5^HbAM=)lk)#g| zexH_y2f&bI&kN^8!}Khgmglb%?-!}7fi{^!F+Mzx%)UZ`@8r?ahiT=n9^pFVXVODv zKqWKTTOUAKTst<$S{`12La%zTd1p;;prer~1Al!)W_tfTU6PLEN#kFVRj(6}I23;} z|3r~r(tK4IpZiH#wv&c{OlbzCj0c-2=fvPO?%iqnB zRQ_&`EMMlA^}j{rpDq;2-{Fz8Gm|qY{|Nk9^WOoC{3`Vu@4Xa(NAay**7m;*Of|nt z8>F>E&sKcqkRm9nt#)sTvF1B(lDXGTK|6mr*|&OW|F^)mAId0 zu+~?WFKJ&CPVbm&$s} pdL0H#+RieHe_UJnhvB6vYL+WedZ(RT{z>8mag(H>B7-HV{U1r@LD~QS literal 0 HcmV?d00001 diff --git a/c++/example/dynamixel_api/basic_test.cpp b/c++/example/dynamixel_api/basic_test.cpp index 1ec401eac..916b10dbd 100644 --- a/c++/example/dynamixel_api/basic_test.cpp +++ b/c++/example/dynamixel_api/basic_test.cpp @@ -1,8 +1,25 @@ +// Copyright 2025 ROBOTIS CO., LTD. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Author: Hyungyu Kim + #include -#include #include #include #include + +#include #include #include "dynamixel_sdk/dynamixel_api.hpp" @@ -39,8 +56,7 @@ int kbhit(void) tcsetattr(STDIN_FILENO, TCSANOW, &oldt); fcntl(STDIN_FILENO, F_SETFL, oldf); - if (ch != EOF) - { + if (ch != EOF) { ungetc(ch, stdin); return 1; } @@ -48,7 +64,8 @@ int kbhit(void) return 0; } -int main() { +int main() +{ int baudrate = 57600; std::cout << "Dynamixel API Source Test Code" << std::endl; std::cout << "┌─────[Test Process]────┐" << std::endl; @@ -62,7 +79,7 @@ int main() { std::cout << "Baudrate set to: " << baudrate << std::endl; std::cout << "Scanning all motors..." << std::endl; - dynamixel::Connector connector("/dev/ttyUSB0",2.0 , baudrate); + dynamixel::Connector connector("/dev/ttyUSB0", 2.0, baudrate); std::vector> motors = connector.getAllMotors(); if (motors.size() == 0) { @@ -72,61 +89,68 @@ int main() { std::cout << "┌───────────[Motor List]──────────┐" << std::endl; for (auto & motor : motors) { std::cout << "│ ID: " << static_cast(motor->getID()) - << ", Model: " << motor->getModelName() <<" │"<< std::endl; + << ", Model: " << motor->getModelName() << " │" << std::endl; } std::cout << "└─────────────────────────────────┘" << std::endl; std::unique_ptr motor1 = std::move(motors[0]); - std::cout << "The test is conducted only on the motor with ID " << static_cast(motor1->getID()) << std::endl; + std::cout << "The test is conducted only on the motor with ID " + << static_cast(motor1->getID()) << std::endl; std::cout << "Press any key to continue! (or press ESC to quit!)" << std::endl; - if (getch() == 0x1B) + if (getch() == 0x1B) { return 0; + } auto result_uint16_t = motor1->ping(); - if(!result_uint16_t.isSuccess()){ + if (!result_uint16_t.isSuccess()) { std::cerr << dynamixel::getErrorMessage(result_uint16_t.error()) << std::endl; return 1; } std::cout << "┌──────[Ping Test]─────┐" << std::endl; - std::cout << "│Ping result: " << result_uint16_t.value() << " │"<< std::endl; + std::cout << "│Ping result: " << result_uint16_t.value() << " │" << std::endl; std::cout << "└──────────────────────┘" << std::endl; std::cout << "Press any key to continue! (or press ESC to quit!)" << std::endl; - if (getch() == 0x1B) + if (getch() == 0x1B) { return 0; + } + auto result_void = motor1->enableTorque(); - if(!result_void.isSuccess()){ + if (!result_void.isSuccess()) { std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; return 1; } auto result_uint8_t = motor1->isTorqueOn(); - if(!result_uint8_t.isSuccess()){ + if (!result_uint8_t.isSuccess()) { std::cerr << dynamixel::getErrorMessage(result_uint8_t.error()) << std::endl; return 1; } std::cout << "┌──[Torque ON/OFF Test]──┐" << std::endl; - std::cout << "│Torque ON result: " << static_cast(result_uint8_t.value())<< " │" << std::endl; + std::cout << "│Torque ON result: " + << static_cast(result_uint8_t.value()) << " │" << std::endl; result_void = motor1->disableTorque(); - if(!result_void.isSuccess()){ + if (!result_void.isSuccess()) { std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; return 1; } result_uint8_t = motor1->isTorqueOn(); - if(!result_uint8_t.isSuccess()){ + if (!result_uint8_t.isSuccess()) { std::cerr << dynamixel::getErrorMessage(result_uint8_t.error()) << std::endl; return 1; } - std::cout << "│Torque OFF result: " << static_cast(result_uint8_t.value()) << " │" << std::endl; + std::cout << "│Torque OFF result: " + << static_cast(result_uint8_t.value()) << " │" << std::endl; std::cout << "└────────────────────────┘" << std::endl; std::cout << "Press any key to continue! (or press ESC to quit!)" << std::endl; - if (getch() == 0x1B) + if (getch() == 0x1B) { return 0; + } result_void = motor1->setPositionControlMode(); - if(!result_void.isSuccess()){ + if (!result_void.isSuccess()) { std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; return 1; } @@ -135,7 +159,7 @@ int main() { result_void = motor1->enableTorque(); - if(!result_void.isSuccess()){ + if (!result_void.isSuccess()) { std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; return 1; } else { @@ -143,38 +167,38 @@ int main() { } int32_t current_pos; - int target_position = motor1 ->getMaxPositionLimit().value(); + int target_position = motor1->getMaxPositionLimit().value(); result_void = motor1->setGoalPosition(target_position); - if(!result_void.isSuccess()){ + if (!result_void.isSuccess()) { std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; return 1; } std::cout << "Target Position:" << target_position << "." << std::endl; auto result_int32_t = motor1->getPresentPosition(); - if(!result_int32_t.isSuccess()){ + if (!result_int32_t.isSuccess()) { std::cerr << dynamixel::getErrorMessage(result_int32_t.error()) << std::endl; return 1; } current_pos = result_int32_t.value(); while (abs(target_position - current_pos) > 10) { - std::cout << "\rCurrent position: " << current_pos << std::flush;; + std::cout << "\rCurrent position: " << current_pos << std::flush; result_int32_t = motor1->getPresentPosition(); - if(!result_int32_t.isSuccess()){ + if (!result_int32_t.isSuccess()) { std::cerr << dynamixel::getErrorMessage(result_int32_t.error()) << std::endl; return 1; } current_pos = result_int32_t.value(); - } + } std::cout << "\rTarget position reached: " << current_pos << std::endl; target_position = 0; usleep(1000000); result_void = motor1->setGoalPosition(target_position); - if(!result_void.isSuccess()){ + if (!result_void.isSuccess()) { std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; return 1; } else { @@ -182,7 +206,7 @@ int main() { } result_int32_t = motor1->getPresentPosition(); - if(!result_int32_t.isSuccess()){ + if (!result_int32_t.isSuccess()) { std::cerr << dynamixel::getErrorMessage(result_int32_t.error()) << std::endl; return 1; } else { @@ -190,9 +214,9 @@ int main() { } while (abs(target_position - current_pos) > 5) { - std::cout << "\rCurrent position: " << current_pos <<" "<< std::flush; + std::cout << "\rCurrent position: " << current_pos << " " << std::flush; result_int32_t = motor1->getPresentPosition(); - if(!result_int32_t.isSuccess()){ + if (!result_int32_t.isSuccess()) { std::cerr << dynamixel::getErrorMessage(result_int32_t.error()) << std::endl; return 1; } @@ -201,43 +225,44 @@ int main() { std::cout << "\rTarget position reached: " << current_pos << std::endl; result_void = motor1->disableTorque(); - if(!result_void.isSuccess()){ + if (!result_void.isSuccess()) { std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; return 1; } std::cout << "Torque OFF." << std::endl; std::cout << "───────────────────────────────────────────────" << std::endl; std::cout << "Press any key to continue! (or press ESC to quit!)" << std::endl; - if (getch() == 0x1B) + if (getch() == 0x1B) { return 0; + } std::cout << "───────────[Velocity Control Test]─────────────" << std::endl; result_void = motor1->setVelocityControlMode(); - if(!result_void.isSuccess()){ + if (!result_void.isSuccess()) { std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; return 1; } std::cout << "Velocity Control Mode set." << std::endl; result_void = motor1->enableTorque(); - if(!result_void.isSuccess()){ + if (!result_void.isSuccess()) { std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; return 1; } std::cout << "Torque ON." << std::endl; - int32_t current_vel = motor1 ->getPresentVelocity().value(); - int target_velocity = motor1 ->getVelocityLimit().value(); + int32_t current_vel = motor1->getPresentVelocity().value(); + int target_velocity = motor1->getVelocityLimit().value(); std::cout << "Target Velocity: " << target_velocity << std::endl; result_void = motor1->setGoalVelocity(target_velocity); - if(!result_void.isSuccess()){ + if (!result_void.isSuccess()) { std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; return 1; } while (abs(target_velocity - current_vel) > 5) { std::cout << "\rCurrent velocity: " << current_vel << std::flush; result_int32_t = motor1->getPresentVelocity(); - if(!result_int32_t.isSuccess()){ + if (!result_int32_t.isSuccess()) { std::cerr << dynamixel::getErrorMessage(result_int32_t.error()) << std::endl; return 1; } @@ -247,7 +272,7 @@ int main() { usleep(3000000); target_velocity = -target_velocity; result_void = motor1->setGoalVelocity(target_velocity); - if(!result_void.isSuccess()){ + if (!result_void.isSuccess()) { std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; return 1; } @@ -256,7 +281,7 @@ int main() { while (abs(target_velocity - current_vel) > 5) { std::cout << "\rCurrent velocity: " << current_vel << std::flush; result_int32_t = motor1->getPresentVelocity(); - if(!result_int32_t.isSuccess()){ + if (!result_int32_t.isSuccess()) { std::cerr << dynamixel::getErrorMessage(result_int32_t.error()) << std::endl; return 1; } @@ -266,25 +291,26 @@ int main() { std::cout << "\rTarget velocity reached." << current_vel << std::endl; usleep(3000000); result_void = motor1->disableTorque(); - if(!result_void.isSuccess()){ + if (!result_void.isSuccess()) { std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; return 1; } std::cout << "Torque OFF." << std::endl; std::cout << "───────────────────────────────────────────────" << std::endl; std::cout << "Press any key to continue! (or press ESC to quit!)" << std::endl; - if (getch() == 0x1B) + if (getch() == 0x1B) { return 0; + } std::cout << "───────────[Direction Test]─────────────" << std::endl; result_void = motor1->setReverseDirection(); - if(!result_void.isSuccess()){ + if (!result_void.isSuccess()) { std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; return 1; } std::cout << "Reverse Direction set." << std::endl; - target_velocity = motor1 ->getVelocityLimit().value(); + target_velocity = motor1->getVelocityLimit().value(); motor1->enableTorque(); motor1->setGoalVelocity(target_velocity); std::cout << "Set goal velocity :" << target_velocity << std::endl; @@ -292,33 +318,33 @@ int main() { motor1->setGoalVelocity(0); motor1->disableTorque(); result_void = motor1->setNormalDirection(); - if(!result_void.isSuccess()){ + if (!result_void.isSuccess()) { std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; return 1; } std::cout << "Normal Direction set." << std::endl; std::cout << "Press any key to continue! (or press ESC to quit!)" << std::endl; - if (getch() == 0x1B) + if (getch() == 0x1B) { return 0; - + } std::cout << "───────────[LED Test]─────────────" << std::endl; result_void = motor1->LEDOn(); - if(!result_void.isSuccess()){ + if (!result_void.isSuccess()) { std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; return 1; } std::cout << "LED ON." << std::endl; result_uint8_t = motor1->isLEDOn(); - if(!result_uint8_t.isSuccess()){ + if (!result_uint8_t.isSuccess()) { std::cerr << dynamixel::getErrorMessage(result_uint8_t.error()) << std::endl; return 1; } std::cout << "LED ON result: " << static_cast(result_uint8_t.value()) << std::endl; sleep(2); result_void = motor1->LEDOff(); - if(!result_void.isSuccess()){ + if (!result_void.isSuccess()) { std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; return 1; } else { @@ -326,7 +352,7 @@ int main() { } result_uint8_t = motor1->isLEDOn(); - if(!result_uint8_t.isSuccess()){ + if (!result_uint8_t.isSuccess()) { std::cerr << dynamixel::getErrorMessage(result_uint8_t.error()) << std::endl; return 1; } diff --git a/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp b/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp index 852c71638..c20cfe7ac 100644 --- a/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp +++ b/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp @@ -20,12 +20,12 @@ namespace dynamixel { Connector::Connector(const std::string & port_name, float protocol_version, int baud_rate) { - if (protocol_version != 2.0f){ + if (protocol_version != 2.0f) { throw DxlRuntimeError("Only Protocol 2.0 is supported in this Dynamixel API."); } port_handler_ = std::unique_ptr(PortHandler::getPortHandler(port_name.c_str())); packet_handler_ = std::unique_ptr( - PacketHandler::getPacketHandler(protocol_version)); + PacketHandler::getPacketHandler(protocol_version)); if (!port_handler_->openPort()) { throw DxlRuntimeError("Failed to open the port!"); @@ -61,10 +61,10 @@ std::vector> Connector::getAllMotors(int start_id, int en if (dxl_comm_result != COMM_SUCCESS) { throw DxlRuntimeError(getErrorMessage(static_cast(dxl_comm_result))); } - for (auto &id : ids) { - if (id >= start_id && id <= end_id) { - motors.push_back(getMotor(id)); - } + for (auto & id : ids) { + if (id >= start_id && id <= end_id) { + motors.push_back(getMotor(id)); + } } return motors; } diff --git a/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp b/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp index 0febd16ba..31cf198d5 100644 --- a/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp +++ b/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp @@ -353,8 +353,9 @@ Result Motor::factoryResetExceptIDAndBaudRate() Result Motor::getControlTableItem(const std::string & name) { auto it = control_table_.find(name); - if (it == control_table_.end()) + if (it == control_table_.end()) { return DxlError::API_FUNCTION_NOT_SUPPORTED; + } return it->second; } } // namespace dynamixel From fb3609a1f40e0a2d12f7d1a0aeb6c4406dd4819d Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Tue, 9 Sep 2025 14:38:22 +0900 Subject: [PATCH 13/37] Bump Signed-off-by: Hyungyu Kim --- .github/workflows/ros-ci.yml | 4 ++-- Doxyfile | 2 +- ReleaseNote.md | 5 +++++ c++/library.properties | 2 +- python/setup.py | 2 +- ros/dynamixel_sdk/CHANGELOG.rst | 4 ++++ ros/dynamixel_sdk/package.xml | 2 +- ros/dynamixel_sdk_custom_interfaces/CHANGELOG.rst | 4 ++++ ros/dynamixel_sdk_custom_interfaces/package.xml | 2 +- ros/dynamixel_sdk_examples/CHANGELOG.rst | 4 ++++ ros/dynamixel_sdk_examples/package.xml | 2 +- 11 files changed, 25 insertions(+), 8 deletions(-) diff --git a/.github/workflows/ros-ci.yml b/.github/workflows/ros-ci.yml index ec270b15d..f9a4153f5 100644 --- a/.github/workflows/ros-ci.yml +++ b/.github/workflows/ros-ci.yml @@ -4,9 +4,9 @@ name: CI # Specifies the events that trigger the workflow on: push: - branches: [ main, humble ] + branches: [ main, humble, jazzy ] pull_request: - branches: [ main, humble ] + branches: [ main, humble, jazzy ] # Defines a set of jobs to be run as part of the workflow jobs: diff --git a/Doxyfile b/Doxyfile index 37205c995..8fdd42e69 100644 --- a/Doxyfile +++ b/Doxyfile @@ -38,7 +38,7 @@ PROJECT_NAME = "DynamixelSDK c++" # could be handy for archiving the generated documentation or if some version # control system is used. -PROJECT_NUMBER = 3.8.4 +PROJECT_NUMBER = 3.9.0 # Using the PROJECT_BRIEF tag one can provide an optional one line description # for a project that appears at the top of each page and should give viewer a diff --git a/ReleaseNote.md b/ReleaseNote.md index c0db5fe4e..226dde1cb 100644 --- a/ReleaseNote.md +++ b/ReleaseNote.md @@ -1,5 +1,10 @@ # Dynamixel SDK Release Notes +3.9.0 (2025-09-09) +------------------ +* Update Dynamixel API in C++ Linux 64bit +* Contributors: Hyungyu Kim + 3.8.4 (2025-05-28) ------------------ * Deprecate ament_include_dependency usage in CMakeLists.txt diff --git a/c++/library.properties b/c++/library.properties index c31b0f03c..fc2ea3cf7 100644 --- a/c++/library.properties +++ b/c++/library.properties @@ -1,5 +1,5 @@ name=DynamixelSDK -version=3.8.3 +version=3.9.0 author=ROBOTIS maintainer=ROBOTIS sentence=DynamixelSDK for Arduino diff --git a/python/setup.py b/python/setup.py index cd00d63e8..5e846243b 100644 --- a/python/setup.py +++ b/python/setup.py @@ -13,7 +13,7 @@ setup( name='dynamixel_sdk', - version='3.8.4', + version='3.9.0', packages=['dynamixel_sdk'], package_dir={'': 'src'}, license='Apache 2.0', diff --git a/ros/dynamixel_sdk/CHANGELOG.rst b/ros/dynamixel_sdk/CHANGELOG.rst index a2535fc83..7b8853c5a 100644 --- a/ros/dynamixel_sdk/CHANGELOG.rst +++ b/ros/dynamixel_sdk/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package dynamixel_sdk ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.9.0 (2025-09-09) +------------------ +* None + 3.8.4 (2025-05-28) ------------------ * Deprecate ament_include_dependency usage in CMakeLists.txt diff --git a/ros/dynamixel_sdk/package.xml b/ros/dynamixel_sdk/package.xml index 4a23c05f2..06f20cd93 100644 --- a/ros/dynamixel_sdk/package.xml +++ b/ros/dynamixel_sdk/package.xml @@ -2,7 +2,7 @@ dynamixel_sdk - 3.8.4 + 3.9.0 This package is wrapping version of ROBOTIS Dynamixel SDK for ROS 2. The ROBOTIS Dynamixel SDK, or SDK, is a software development library that provides Dynamixel control functions for packet communication. The API is designed for Dynamixel actuators and Dynamixel-based platforms. diff --git a/ros/dynamixel_sdk_custom_interfaces/CHANGELOG.rst b/ros/dynamixel_sdk_custom_interfaces/CHANGELOG.rst index ef6c64237..3fb297df7 100644 --- a/ros/dynamixel_sdk_custom_interfaces/CHANGELOG.rst +++ b/ros/dynamixel_sdk_custom_interfaces/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package dynamixel_sdk_custom_interfaces ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.9.0 (2025-09-09) +------------------ +* None + 3.8.4 (2025-05-28) ------------------ * None diff --git a/ros/dynamixel_sdk_custom_interfaces/package.xml b/ros/dynamixel_sdk_custom_interfaces/package.xml index 0a0eab411..764f679cd 100644 --- a/ros/dynamixel_sdk_custom_interfaces/package.xml +++ b/ros/dynamixel_sdk_custom_interfaces/package.xml @@ -2,7 +2,7 @@ dynamixel_sdk_custom_interfaces - 3.8.4 + 3.9.0 ROS 2 custom interface examples using ROBOTIS DYNAMIXEL SDK Pyo Apache 2.0 diff --git a/ros/dynamixel_sdk_examples/CHANGELOG.rst b/ros/dynamixel_sdk_examples/CHANGELOG.rst index cc063aa6d..6ce6ab90c 100644 --- a/ros/dynamixel_sdk_examples/CHANGELOG.rst +++ b/ros/dynamixel_sdk_examples/CHANGELOG.rst @@ -2,6 +2,10 @@ Changelog for package dynamixel_sdk_examples ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +3.9.0 (2025-09-09) +------------------ +* None + 3.8.4 (2025-05-28) ------------------ * Deprecate ament_include_dependency usage in CMakeLists.txt diff --git a/ros/dynamixel_sdk_examples/package.xml b/ros/dynamixel_sdk_examples/package.xml index 7a7e3dc3a..c946b18ab 100644 --- a/ros/dynamixel_sdk_examples/package.xml +++ b/ros/dynamixel_sdk_examples/package.xml @@ -1,7 +1,7 @@ dynamixel_sdk_examples - 3.8.4 + 3.9.0 ROS 2 examples using ROBOTIS DYNAMIXEL SDK Apache 2.0 Pyo From c7ecd4595250a7f07aae9ed2bf8aa50baff5ab24 Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Tue, 9 Sep 2025 14:58:50 +0900 Subject: [PATCH 14/37] add const return in dxl Result object Signed-off-by: Hyungyu Kim --- .../dynamixel_api/dynamixel_error.hpp | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/c++/include/dynamixel_sdk/dynamixel_api/dynamixel_error.hpp b/c++/include/dynamixel_sdk/dynamixel_api/dynamixel_error.hpp index 86cf1481a..88382528b 100644 --- a/c++/include/dynamixel_sdk/dynamixel_api/dynamixel_error.hpp +++ b/c++/include/dynamixel_sdk/dynamixel_api/dynamixel_error.hpp @@ -69,12 +69,23 @@ class Result if (!isSuccess()) {throw std::logic_error("Result has no value.");} return std::get(result); } + const T& value() const + { + if (!isSuccess()) {throw std::logic_error("Result has no value.");} + return std::get(result); + } E & error() { if (isSuccess()) {throw std::logic_error("Result has no error.");} return std::get(result); } + + const E& error() const + { + if (isSuccess()) {throw std::logic_error("Result has no error.");} + return std::get(result); + } }; template @@ -104,6 +115,14 @@ class Result } return std::get(result); } + + const E& error() const + { + if (!std::holds_alternative(result)) { + throw std::logic_error("Result has no error."); + } + return std::get(result); + } }; class DxlRuntimeError : public std::runtime_error From a39c5d07fe6bb9adad9696874cf97206d65fbf33 Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Tue, 9 Sep 2025 15:15:05 +0900 Subject: [PATCH 15/37] Remove changebaudrate Signed-off-by: Hyungyu Kim --- c++/include/dynamixel_sdk/dynamixel_api/motor.hpp | 1 - c++/src/dynamixel_sdk/dynamixel_api/motor.cpp | 12 ------------ 2 files changed, 13 deletions(-) diff --git a/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp b/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp index 5451fb6a2..3a16c7827 100644 --- a/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp +++ b/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp @@ -54,7 +54,6 @@ class Motor Result getVelocityLimit(); Result changeID(uint8_t new_id); - Result changeBaudRate(int new_baud_rate); Result setPositionControlMode(); Result setVelocityControlMode(); Result setCurrentControlMode(); diff --git a/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp b/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp index 31cf198d5..42667403d 100644 --- a/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp +++ b/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp @@ -213,18 +213,6 @@ Result Motor::changeID(uint8_t new_id) return result; } -Result Motor::changeBaudRate(int new_baud_rate) -{ - Result item_result = getControlTableItem("Baud Rate"); - if (!item_result.isSuccess()) { - return item_result.error(); - } - const ControlTableItem & item = item_result.value(); - uint8_t baud_value = static_cast(2000000 / new_baud_rate - 1); - Result result = connector_->write1ByteData(id_, item.address, baud_value); - return result; -} - Result Motor::setPositionControlMode() { Result item_result = getControlTableItem("Operating Mode"); From c75d8e731d0ca0d3e57d263716d0c68656837629 Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Tue, 9 Sep 2025 15:19:53 +0900 Subject: [PATCH 16/37] header guard fix Signed-off-by: Hyungyu Kim --- c++/include/dynamixel_sdk/dynamixel_api.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/c++/include/dynamixel_sdk/dynamixel_api.hpp b/c++/include/dynamixel_sdk/dynamixel_api.hpp index c452733bb..dac908d3c 100644 --- a/c++/include/dynamixel_sdk/dynamixel_api.hpp +++ b/c++/include/dynamixel_sdk/dynamixel_api.hpp @@ -14,12 +14,12 @@ // // Author: Hyungyu Kim -#ifndef DYNAMIXEL_API_INCLUDE_DYNAMIXEL_API_HPP_ -#define DYNAMIXEL_API_INCLUDE_DYNAMIXEL_API_HPP_ +#ifndef DYNAMIXEL_SDK_DYNAMIXEL_API_HPP_ +#define DYNAMIXEL_SDK_DYNAMIXEL_API_HPP_ #include "dynamixel_sdk/dynamixel_api/connector.hpp" -#include "dynamixel_sdk/dynamixel_api/motor.hpp" #include "dynamixel_sdk/dynamixel_api/dynamixel_error.hpp" +#include "dynamixel_sdk/dynamixel_api/motor.hpp" -#endif /* DYNAMIXEL_API_INCLUDE_DYNAMIXEL_API_HPP_ */ +#endif /* DYNAMIXEL_SDK_DYNAMIXEL_API_HPP_ */ From ca75cf2108f17140ae481716b43571b952bfa64e Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Thu, 11 Sep 2025 11:18:37 +0900 Subject: [PATCH 17/37] Make drive mode setup function to use parameter Signed-off-by: Hyungyu Kim --- c++/example/dynamixel_api/basic_test | Bin 72120 -> 72000 bytes c++/example/dynamixel_api/basic_test.cpp | 10 +-- .../dynamixel_sdk/dynamixel_api/motor.hpp | 26 ++++-- c++/src/dynamixel_sdk/dynamixel_api/motor.cpp | 76 +++++------------- 4 files changed, 42 insertions(+), 70 deletions(-) diff --git a/c++/example/dynamixel_api/basic_test b/c++/example/dynamixel_api/basic_test index 402e85515f53548030bb226b0f3b2145e73494c4..ed247747c0df910dc4a5a4ffd5b76469005000da 100755 GIT binary patch literal 72000 zcmeHw3wTu3)%HmsiU=m4F?b&o6cu777jB9Ml7R^XNRyycrH5fMA)}Mbm`os0FHxit z12yGSOTF~NYtf>OZMD>E1ntm%sHv@$s#UaJ20;x}YuZxtziVI4*>mQcF+|$u`JacQ zVV(W1wbx#I?X|Dx%*=J3@|o#rX_~wSX%}jg>YBunV#yd`$`ospwc%Q^Hd=FOM}R*V ze^Sm$ccz1hOVS}csSN_9e52^bhMvq}O;TApQOf6ZZ8)8CB-OHbn(}4PZ5~fs>D)OS zmsHKi%4-$*EcE>`9G6thN51KOnDcv|Or$Pm4E%f~$7Mcp7v&pP9VhrCKl7`b$ItuAS&QFrFDlcf&pz$?tOMtg4B1V2NQN%b zCz_n4N8n$tz$kP^4(b@3r@04Z9W_1c#%Zl5k`VFB@HZQO6xQ?bC$IUqslZ>Q0IESR z#@|BxeOq8f>!;oLW%j;(Q%>BsujSNz`{GaB{rcGYVI^h9S6rNS@56gTk8YWH$?w0o z@%97$rOO-EpV%FExqj&aU)FIOhLwDG$emvub>k2J;kkB>N2s$UucMM9Z8*0dZI91E zz}m^rgbq8r1dg-AkAgGp@HKFz9lp?^+*=&%Y(kLQ>3PaQ{}~Q`Sm>aCokP8%N7XFC`ZhYW%K?Y}b)Q4I?GEyDP%e#K*6TwD z|EzS-|1Af6tV6%vJ3oN8#tw zF)LqsB{Jgjkb`l(ivCM@)^~aOJWjN?K?#3vB?l%5{D;VAhZhPxcQ59GWkUWA*fRpF z-;K|6AWM{c1^hsIB1<^FO~?<2A4a2Qt!p@tBk(JMll+H^IWSe=j|+Qd3;cM2UkJaA z)<$W?V%<4T;J;$}M`^7BA0_Y^h^y1Zy-Mx|PWBgfaTMKzU6tq;gtvC_w5-?PQC}*z z&Z2*z@Q<~=Cjck;EQ_6*URFIGApgRef=5(pK|4M)R`?xLnZbXM3Gx;PLD`-9OHo+~sxS!`0UsoYaK zzrnLCkvpX%+|Us4Ls5y_Q#of|RdkZB`&(MvZg>6?UsKQzs~Uq1OUwLKQMcP)=WEoX zjlN*Csm$-G^5Gwkr#46`!JfRtSEDcUH3oeRQQhbF2b!8n-PQsvR#$|Zg3(~O!QN{3 zl%;{_TwjZnDi78NqYfJL0}bTN>Tu&#%>f6Qf|_8HRmj2ce6C`SJ-cvuozKw_G)QT% z5l*Fg&8y1Or*$7FA^ct(~8$wcSNw;LM# zzKD;Fv-M;aq9ehqs%Qv9BO26ptgHm1fDfx-jH>}(eOa|$ou_+H$y9@HRiR#0q*uGq zLs4i=sA3rvKCdbq!f?RuXec(iKfzJJRnfvgLruue9*?sr6It&cKxv#NC4`TB3VY<(Wk4C2Ii|4uX*fix^5?toa$9+>!_lJB?~`Y&TKgs%TMdsJW@`zZ7#*DCE$Oq5)53J^BP|s>T2t`dL#Pw4;sqLl;&e0o5}q zez0waVQsFCxl<1X8kR=uJiPB7v}Fi+j9;TYOXX^V)u6U9kkGvrG%OFIQk8+G=K6rB zWo?kU+RA{|+!P7~B3iA#AsPY?$D*`Ur3G}oFW8_({g}jj(P*OvO0$W`k4R|=Mm3C4 zXbi2nfw)N3uq&aUk`lcjH%}{BxN!Q6GToh9kUK@2RbDm&02qb2`EG@$a`?TTpId0$ z6_~gAxka>3Ny8cJApA@J4aS)-r3N!J4XF&4Is#}0Fyaz&B>sqUi8O79z)43s{sx1W z&hAKpw2@9i=`T$?iv3F(WXU8)Qrr&u*Sx1Y(xytkgFP6XC%f=hapnr7j?%uwzjRUg z-GWc&!7G0MGo+5x`UG}W?+>xl%+Q7n<2wHEvn#PP9ITxr($TZmV@o$kI}>TT$d*q; zUP>Pq3=Y>&=X@dm%OBj1o%9jf1tQ&iVh|?@>DnxjUp^m^`^1ftNeeEqzmxcDI){f? z@Wn!Ylm(ajRhI>q`}1)YT<+)dEV$e!ud(2AzdfW&*u{w^)3SWpzMqRfY7e?PgdW0+ z2@qH3bNmL*(&%hnURx9#TWI6zQt)F9RMWOA_?Zgct>EJoe5ZoTGXi4oQt;`Dd|bh2 zD0q*8&r;Rghl@l1l)@*IHhO9&8GaTmWKoIfGq=~e}&=Na;n=O-jT zSAwA3&+r6}9T2JBdo1>dCLGZp;s?O_ib_P}8e z9QMFr4;=QuVGkVkz+n#@_P}8e9QMGW^}ydUPyMHN)!rfA_Ke@(g5#RiUD5QuZttoW zhHPU6^%dQ*PSf`HjlB~j^E8d*>xi>|_q%<4eH|=K2Wb0u|K3Q`!P)-Z&l_nvFx$WT zF(XX}W&3yEW2EVTZ2#^b7->2f+rRsIBTa__`*&Yur0F1R|8Ad=rUS73yT4_m>40?q z?&(IF4#M{Db{lCr0NcNNw2`KRul>767->52+P^!^NYg>r{@we&lJ%tnF4W&h)4>+% zZ=~r!3-veBbTEth8)-V&Lj8?29cZEcMw$+?P=6y$2Uw`Tk*0$y)Za+cffed+r0Jjv z^*7RVK!y4nX*!rf{f#soNTL2l`c#%qGt#HA^uB+~`ny>AT_a5gPN=_;rh_Kb-$>H| z6Y6iI>0k-10~eoNYgTjg!015Rs(sb~S`WtCF z_(uJWG#xCV{zjS(l+gZ0nhuiC{s(0J=>Q4sZ=~tq2=zD8bYO(~8)-TyLj8?29T1`Z zMw$+WP=6y$2jpmfBTWZHsK1e>gCW%4NYjB3>Tjg!03Y=?(sVF{`pYzW^W*K*n{hxx z{TJyM+Y-~ySkjMM(vMiu4_MN7Thg~#((5eg)t2-%mUOcvU2jPTEa^p-^a4wIt|jfU zq^DWZMV9mgOM0v&eTpTWZAlNeq%$n(f3Hbwk1s6gPb}#VEa|^k(yv+4FImzrTGG#0 z(vMrxk66+VSkiY}(zjXC>n!Qjmh?52bh9O0Z%GF%=|z_G0!wx&e;GnF^&i!@EAzBglG`RI%_YU%={l@$IucF66!QDc^kiK2DY+RA~X?$s&=D8Uvo6lX~ zT{UegBsFi_2hm~P_G#ZkA|Cu#UthcilkIOZPJvw7BIGs7r~JE@gGo0Fkk5Pl-i^>u z<82>2b{tdX>2-H`*R75~$lLZCcAgwB!r1L?=UDq~EsW*q^tLV7;%)b|dfOMQ@wR!o zklv1T2htmm?nZhi(i?HVtKGAy%@c3;JiThy7j2%Wz3V9ZR`&FQ7cmk=#3$gNw$&lh zFD+1IKE;sR&e`n?dTYGr9>;8sX2fp<=eOGqu2PGU=RaJjX(-}JcvX9r)@-bzTPPX7 z@t;)DI#jfs{oA@@2)scHIYmf^Q2a!wNa2z<(b`-^a{kkhf7LVO&A#ZV-uCSOU9D-t zeQkegi^cVS^!06_#pa$2W)<2k!Kvp#x9QX>gi0L3YN*8BU9r_CLx0zACQv6>gl7>5 z8r(IZEHBEq3^t(XyU<;{ZGR#pNXWX3N<-8uMDw`lRi@~LT=Wt{^b{eQ!9{19qQ`R4 z*@ozT#*4+@KqXVEaM0q`8@f*45i>By$E_#L` z%Igt}f18WmVTvy0qQ@JeydJT5E*A}(q8D({;f5%$M=U;wi4%vT9=V-sY*;xqZP# z^k)nj!`~<+sh5efWGsU1Hi`}Jy1C$aTHA{%=V)4OW&=!MBIhxSY21j#|L02*1uL9_?|9LmZo zlx2)%v3M&P-L`r%b4!NHD7;h@KAwtd+lC^T2#Tm-Ib!jxr(+6xqKn$^o%rMAp|-8C z|HBee$Qrfh@7BEISYC*ck6BgLLkmwbJ&&*~q-T?^cJ=cy_e0UXa=XA6DS6J0DDh!Q} zGF~ixJ2Bd`U%$@q$o)X#ThI!tdQajJIP@tE*Y4A?8B|Jsa13RM#iOiUR~NH(=LBdE z>{`{6hWK5CJd6pn7ifDH`I}d?j+sSGNU=Rt)N=pl%%&34rYu7)7B2;t$N3MiTSc~C z%pgNZ!Jk=C*1eLi8vuI@=V@0TMqK9=0~dZP>Xh zjd8*mq$?J`>2GB0YAqo%72xmf&H z`(D-`Ns#4qP3T_Z1FlcLnK@$C)#h}?nL1fU$!k|B*NEkHMG-y8|q7R>G z7}SKb4rD7by!;f)0dH^x?_X`y=rN`s!)=Im2+pupD>53BUt??IwHlS zj}2GTc?ma|CCq0fh&~>RUoo0{XD8EdZLZJJm}cLR&2<2%+IPk?m9p=AB`khoAM?(U zX3>8nM0jTi?s?x?fRiC5e*Wy@22pz^H`lS~>ZU=`6(<2z4Z71Ph?=Wj7&Mj}wCuZ7 z0P86whFmPZyhj@J0*tb1~)LHsSWNDxzR(Pp=|Bh zU#&FC{Xat1W!z|#@G!F}7GKXr|6q!4l8FZh8PwVY*qi`(X4W)y;*>Hreh+2roVR8LMulY1mtYh<4AyJ#Y6# z%&Lr6jG8^ba>U~6xPq5g82-AGDaaUX_-myQUBX50Gesjz)ClkPQ%OlI{*O=Xs+nzD z%~OD?)tt*z%4(h_{Qo*>Y|kEN7X1YwqMB#po>y}j4ocD1exsVtvK+DagIvK!Ek-qe z$rKp2zj6w<{gF?Y?Ja!ZVgxlRqsGwMR=YSnjtJm8SSGAP3Fl+LaI!zCiBCX3OK#%P zK-Ic`%cv7|gfoQO-z1CLv&WmJ|AP=w_X)V?bzg}?V`OWbMin6iTUm}+{9#t51S@AT zE2-;L3vqOv8+bw6mK#3mULtro2@jL`P%MP||G z7;>@rcTgU$>QC{EK&k4HN>zEhoy+U9^~*Oh%eJoS9h!N|SA1dm!}FyFx>zBx_$F41 z1P^$4WB;ky@W6c#im^zBBemy>XJl&e>k_^4@QEay5J{(yL{hw;jgFBV?{1)Ky!*&3 z7RS_F1){n3e9Rnlfoa)MhFmN@4P5S^oAFQu*EM;{9J}B~<9PDP$x$@KvS_^=IZLwDu)D03h@eAx`xt zx%TWmO{9Qzf{lbI^&dqX)IV-p|AaO+>ThY|YnV!-{-TMFQ6+CGwxQQt+DsXQ$w>^vCB-_W^ z+Fod}y~ePK`tC$w`=_uDt$LAJ^e{s%7B4nzUxz2u$hOjGz}HxgSo|5T;H$7v`Trxt z4CxP#3{Q5P6`Pi6jz#gCAB%^13m^tNZ0n<~;s$n41{kD%hPzGVhjmnY+7w_qo6 zGKTUjUE*zWvC$d zSbQ**kl2gv;ZDpq&FDkXJcd4C1{wV+D~tQ$TBgMsGIt=hl0)WRplZmx3gxCR?iY4l zPH21fBUc!{SZT<`;*Y)~eetg&$QK#qc_c?6Q^L%S#n0smLZ*T#OhHDzAv#KkYFu=> zDS8YSy=8*b`U&I3;;*4#n034<`d1+FFX1xlMD632o$Ho59uhi!z;(PDBnE5d|0TqX zuqTgEChEWa1-zu*coBjy*|oEPs;*tm^va34OjI|GmtSd?f0`i|i&woZUAr*@2I7p! z@K>DWh{d}h0w;_x6}(M|S%H5Hr>fz+iQC@N$GD6DZK41$<}D+ZUJ#k`NmtwI#XOT7 zrygY7gmLO9uDy;4W)v7^P8VjL%@ieM`3+UHJ^PY6!}Vhz#6x>1@yt3Dp}=QpOGXv> z@mtKMNTN+6g~hGhVo^5LlNn?mUvC4strHd@MecE0|*{7{wH1{OBAh`X%GV;_s04 z?b$9<^b;WQH1rZ{)YJExp^(>drH)?+9d~jaZ`BfmMg2pBm=1s8XyU};m2U!03ix^G z6UhO8F;LZ2-(xCez%LXv&EiG-%%XD*IXquPdE8Y$OJ}x+4BNkCIZ%Hx3H3J>d_st6 z`w(IK-^qhX=kGHTY_FggKs{$$Z2uioDQ&+?xUHHO{XoEQTcaU|^R+jm?SEryu#7De zNRFEMd|~?;B%W}n?nA-qf$n46Ju}QAGLh#wI75rCMgdAaT|;>r&0|Wfk_~bVF zIZ(BY-eW3dJ%1_83=!I%{Y;Hf&zlW7oCp0`*7Imy&t2nXJ*$}6vH1C1!PTaMGN!;7 zUuurxwpUVhliKLVhV7T4zOs!TwAg;8VH3HyR}|d|>tK7DS@g?o~v z2{i5*E>lzg$jW4&1Yj@dk#17jUOXg0X%>{CsOBEVg)bV|Dkb|zml{^^d<%NKZ52b5 zdQ`zZqsXM^vZyXQ|3L1^WLjps9j1bYp#$D4*I^L)nq_ND3r~*&3~d2tnc3a0(G^kCkwAw{FlE^j1p>6 zFU;AFfavn#y-d|CyrqZkL9b?l9gDZTO6~nTl3S>C>0PXM;(sM@)iqrjy}-&RmGl2i z?q1O4P3y)Bf)w`jA}0|1;f*e)jlIX^!^U>csj{1kQf8ojqLbzzq#;fd8Gp$XFKqsS z%42;txh+KeD@VoF4{?k(bc@5qy`#^3YbF!+;6W7eQF)dF!hah(6<#WYU(q(DB2V5rk)v# zAIq|iLUI=+2eTwzI^LFf8=8a0Khu|n53slpZfIkDNf|>-nMAMf9wnUHiN-bUO6+T_ zZ+M^Ig;$rmll3Y{Lf=CVWx81}y&mdWk~(htNE$ko(h~Af^BcYUp2R+D@z)|6ZJCoY zNk1RC9;ApA@TmmHsH18fiBkt@8b=))qC3gWe>;*iparNE5Px{D`)tYkg7L)Ai56j% zk>w5HRg2q~gJKf=d1Xi9sn+Tb-Z9uK59VfLOAsl%4Fy+FO+18wDPyQsQXkld;lvM!r`xe^sN_7uP# zzHJaDb-W}^8fThx=PS&l%?qVTGfb1-VLWb9t<|KTLz^(^b&``NsdEpDMiHlrpdh9H z7DFJx-d#JSy?&SBrak1>_KX9UNPE{;8NBlpdnZ}#O*id5t3UQm_&R$N+q=Lp_6atl zvU&PKZtQ%+*bgcVV{c$Q?%WS(X{EOJ0%#M?T}g7%xyJa>pZ4DPf6{-GO_P2~LC~J@ z^u^MDm8MCbF&;N5Vl`{Ku6|@QWZ6Z18zJqGfmwziwsx?jeGdZWQV+BVld@Hl zWQ!X9qqWvLjP4kHN}vRTt2)xtSLiD<8;y&T+4$Vn+>Mpac6It@D;@gaNi6=?!EBX1 z0@}L0=Xm5uat9mn+O~ZTaG!^fB zl4cFD3<8tTqx>1uiy%Q~S4J(Z!Yk6bHURl%KE4 zu~5k3#n3)0bqQJO9I+lM%^oT_ibbM~1g#cNg%fpHxRFGzU7fi_vL;wfWKwljZG|JT z{E79wYUefVlLq7Mf5lWw!3eQF{%HuMf)*KnqHz}PTa26J%|lZ1PsqAey=mA&Ih(gH z(LCY1LsI?(3QYyrRG=P-nBU`ejz^pLT8d3k;~d4d*CU@isI^yF)o*?Ldjt zwy!4+<(7_ltB^|FsV);UT4Df9GU~K^kLl^uaru2w-aa&*v!^famsRX@rpa;Hiw7Fq z)w@16&7xCK=K{Y(9&%PX3U-#F2YO%MU9SFzT>vaGteUM@WpNhTKl!xU9O&sZj7E1h zY_bu)23j2sY4)5TN42Oh`&y!_JiS=mVBkOB)6@Ky4!g`^RBnK{voF3W7n?;zxd?pA z{0Kwrkfi=%C*(BO9D!Q(yKJ*0OSkzWj$j9%CApdwy&eA{z2%2auPbq%fIaZK<6dOj zFgiNt`-QZp6FS@1R{Lm-i76uS9;jD>#p1Kn+}jfK!W6dmRM%UF26GCy{3HXY`zax7 zsSaKTR4be8I|E6M&UM*m6{C-{4lK51_|!5Q!?J!%SliLiPg$O&Cr~uJl{`tU#~+5$ z_h%X3m<9gf>#zq7d*HAK{y*>l{b~$d{9jGG`3`P0espK*RQzbqRG0XnNmsB5x1r`h z2tQV*(NFmZ{_@6f!%`Q1Kq%1S!jF$hd9Fq)mA|6tnqE=nstPwZ`U9@&KvUFJf*&^2 zwD)dZ``)ds>i?H9Zbf65e%Er5%6aeB4#?l&a_71#@Kc3cjZ0qOFm8UXi~j_oYyQ0R z=FgnT#U!@?dBsm*D)}Y1FxMr2GSG%wl<2Mj<@iBWOM4_&g11mvDU2G={1C#O z%O%}#2>p;;wu)@v)zhl$0*}Ra<$@wf2Bs^ux&LKuf9$!;7%&Iq|D@cC|GCqp z1o>C`e3|$SX2s_cl0MJX=*OB}2F%Aw7x|d3ul3|*#%~E9TtLY=z|78qsS4qwj@C8 zs=r}9b*lOUrLKl%OiB&mC@mj?HN1MooM;+;`q1X*tnn>?Ck;A(Oopc22f;OX8nY1h z3xC?z*9uw(x&d_D&-(g`K}U4<^=-zf_=li7K+pdMUZ%R$%T(?>UgZUwy`bO=7^ zw*zz>=w8s3pjmjQd@X1m=#!uopl^XjKtBcT03C*pPj-Ty3fc{t584A-0h)!+x-@|1 zfwqEHfOdjLKz|F`0s0AOC+N}mkZCvQIiNkDC7@aO(7{sBJkYhE6`=Hq;Rxt9&<@a# zKs!M*K0d7vvmD?slCjetG{+5!4!&`!|LK)XTz4cY@bDvta( zAD9f92YMA~1?XL%5zuEqJ3!w8?F9WNXg8>9H}Zqd1kJ)nt3sf8puYmG0R0PS1auJ2 zL_0v|f_8#7fp&xb2($sogXV$W30eXA0cZsDXgtB_0Cj_Qf?f^U4SFwV59oH#ES$`Y!~>W-(9xh3 zpjDs|&<@ZJ&__W#L3e<5gMJL!1A6pl$e*ogr-0^xo(Eb1Is-HUS`FF(S_|3}n%YZS&A&^*v6Xa(pR&J{3eZWQ5zyJ79ia66Rh^)8!ENkHTUn{4wPdB8H0;PB zbUsQr-WAbyJ=E8CCP|bYi3mfi+ktmcesNJ*Yw`Ev-}m+5RUA$89Fz6E^pfF6RKp-7iN6_t zcY=Q*sKIYG`8xnV2|k*_NuiXv+_WPC{-fa6tNipDBYy|@2f)V*c4q#Id495iVu1MDKnHKb zpdrgZJEcF)$h6@%aMy{E7trEQ5bJ_%6KGev!(DofI~d|7!4y!KZo|=sy7d1>mDK&HQC%{*wo5S}ypLRespyUjY70@SS4ra_}p_pRMLc z{jY`!7qF|rUkQGlg`a+@S;j`lJPR4SI*viUr>PCWzXkV$e~U>>#>1?&gH{brXRS?g z??Y}3Udvxka*rg*-9RN$n`B`<_kGAYl{X$fTPyWk<*bL+n}1KCCkj2EgU{?PO44K4 zO)+peK9$4lJ|Rht*-iC(0dl{Qa{tDNo9G9lezacA!@Aa~e#7AJT=1D)&nM|I>?(xZ z?T}-3J&+`4l(!h`T@CBwA`ru-5N}_yn_}R2@aY3)gRfNdAWjBNV#6D2mrc-9h;_5w zxZr}G7odmu-@^UiIf|ZiFDs1Nme$@&AmbG8v?i|xKNqrwtpPI@$AeG#N2~mWCZF~U zH7WFmz`q!LJKvLiYeo4^_H71#Ch}u6GwqvgmcIl1sVVgD1%Gl1{xD3m6Tx>Xe?0i( zz&}qdzt$|j9DLHxmKHMpDolO|d^h;i?uLJr@op{nMc_O6XEXQ(Dfm0U&rebQUhvON zQT{OOdCmjhY3`y=vQUgU*;fvJs`5kNrz(G~DBnr{X7DGZD1Qg|<5TeWf`3j5KJE3W zeVy8GJoux*cWS?K@W-am9|Hf36#TV9ztj3{Gx+pTNNlyt`0<$WwF7+GgP*AKzh(0G zg0F#ZH-=FE8-_h7@u#TyBWC{b;GY2gIVxXSW0iwH27IUSAq0LZ|Ev}H9orv#`V=!; zkIVkG$h2<<_|$$*{Jr4krr-}7#^ZaATEFxetN`-Qc<}WU`OCo%q~M3ZzY_e}3HqlS z`qzSgTZ;Uf!M`&Fe+T&YfbV4gUhsFM$Uh8c0I#Otj|YEe3jO8a55n1vlmA2DAC-c? z7W_=`o&2*I{HfqO_0JvP7l7~7Klg$^DMk6i(6Oea;ExBt1bip^%E7Nskv{}}QwsiC z@K=EE)P9@6zduF(9pFElg1;Ah`VIsq`-f#}+8f}5nB&V#vww{T3D=1wRD-(<%6C!G9EdC;K;p|FabNcYuFS3jSX3Z%v_p7?#(!rN}=X{F_qn%fVln zf*%4ul%o8#;8Xu`YM;&E>nZpk{U{8wtY5v{;{^{U5^{>6)(|3e8@rPkyeQFB+c<{-7r}cL^7+ER! zA@B#M;I9S07xRmg{>@b0~# zd9~Y*&e;X%=%><=xbK*p-V*J-W2WQ&mzg;orP>#nJ4ou)5jmSmwI7Tm#$QL~bd_rN zAA1|+_+56+&Qh)Y_}fV8v7DTqQf+_E!<2aBgq$@V?b8$1Q{o3FAoYtAzmNTBD#Hz758)j-RjBTSr z-##m+bEfvc}wNz^E3t!HuOLzhf|dc)+wu$t@%V>b8%04_DzXbl;_SlXcXss(LnXdOR7BcM&oAl1Q7XG+ z`E1P%M9TfEkJk@=XAnLi%IdpQI1V2NVQFb6UIk$3F@paOk(T9%;tm+3PBeO8WFTr&<;U23fd`Xm!RE(?h>>|P_00e zFQ`kEtq^pvpb=YqoAFFb_v=o=q^F6?R}`yL%I?*%_=FG>KeCTNpnNA z+2wZU7Ut$na5uB0``Y~c+`Piv{IdlkF=t|m#;w8HzP?11_rEl7)`||~5(39M$W#AJ z$LQ2ENeBw!61dAUev`XM-a4+A0Z+qjwR0b*GRs-OPp5cT%n^3Ri`!WOpSq0W z?Cc-68w4H}^S_)Q_6WSVmdoG5i8cyxO!~(y=YW)N6ZlyxI6hLy_X+&{V4}Pm0YG~0 zY~(mQ>&ES^0$;K;QT{gqkBM@n{_`E^Ho=po0T&`=_Sq^wN1DAP)C?iih z;I9Zh*EDl!>9?7f_(!7LYo~CWz3+tE3Wjq`{Wl1l?7S_)QMvATN#OT3ar{h?=~PVY zBwtz2fzblrAn;D1hn+Ek_ZQ%!vC7C2^W``p-z($~)Nnx7eaR56f4z_6=Lq=+1%93I zr)=N93j7rjCsI#7CjKn#ByCieP$0+@;AH=lCgGTFZwwYk{@HK?-GH#gny0~^|}b_0vGD5bqPR_ z>mV;}P&q#VK7u%{97Pzj>j|Mh65#3C0`GIsa}w5*Waq{Z7nJS3O5jh5_LcQ|QQ*yu zTwePBcr1`f&!ty#oZe@l>so>TR>b)n4r}iU{C+v?2KF5>*P$1A7>fIzAx}P(O&0>!ru}2OyOsVyRx{R)}>rrd>3Wyp{9gTc+z4Lo7*VfW!CVF0&YwRK@~xtMg?jB22;0ef z0g~U+ui%Pr5&D}2ZjHBR1^%g+?@kf&uL2*Dev&q%O8|nrBjiU!I3VpAhl!BN{a?#G zvJ5z~BwcG9@Sh7k>n!8UrvkrwF;`V4G>*guiOT(*B~BIz{27a$I|Tlr&?A~ndm6Z% zof+9&|7;;I$APm1{!x@0BKy~m1a2L-k3~kZbB~ByIWMmic$M&{?7xo-e6MA``waMK zv`bbOFBiiyyN=G``nL&tWV=)g{LKa~C-H{`ewt;R96rk44}J%HtpmQ9@bnRy+=mM@ zwVwkgKX`>ba=!knz(k>DrvkrEjOQ{wFG2?=J%tweTLs=H+C}>LW8fpOk0|cqdS$#F0B&c$ z3l+4(BfzQLQ9}QDT%C5W!0Fd>=vvQ-Mn6Dlrzh@!Uxo>h^q0191*3(Y9}B!<2?vm5 z*M|bPj(h(U`2C{Y(Vf|Km5b}|s^e*C=Z^)xQ^?D9-zV@I5hpO4U5jBD+1VrJ(+Yt< zF7P9Se`MYNA@J^6E-1@A6O$}!N1?|n^sEs0)dD|D;N1c*5b+?_8zp(<=Pa#j5*L(l zQg8;>Khei=x6q>lm$nIrk^6-F{g(ba6b>Off4z)L=Zj3s1^$X?FRAkrfm_$N`3OiU zqn10%0k3hup9fC%bcuD;w}i5jP;tVoapD*Fd%_Q*dfHmxcJ@5rfd5(Ov92q|qJgN~ z8$_I-x!F}G@L`sI`V4Tpa)*!OdUP>9$o7p0{9)n$Ord`Va63I`pKXuNbijQM_;SM2 zu?R(csn<2YslHK5oO}tK)*GX`RB61Yh6NkRAKk=pxRG7Dz+D#qJSXs{E#vJzfm`=G zMd$K;Q}I(=FWkbeO5k?>c>p-sbG5~ulgD#>wdlXHzkeIJot~>5@OvEa?GE_cz{#Hb zEajds!M@xOa2n5ByLdgb(m1_=;rw3x_X*&n|8A>41s)OO>Nb&SQZARjegy|)-P?i7 zdsY7*g#1*C{BTSVRPN6OF8$de@GVzyEixWH6Zlt_el!j7NBdK)ODz?jF$2#0Cgi1U z_X<4QV$VOBo|CmLTNCy43HhzU|0f9c`4})sf1$8b_UoGj?iTSQ_gP;6mo}&+9F@=Y zJQ?60$HxRfZ~6oCZEgX2>s>2?esJQr+VEK;ey8q z`4=7JKXSk`3%MSj@SB`B7Yf{ZZnI3_-IjRR4&2VpcZK{1qQ25U{vuxPd<*^>a63I& z6DfX9P8yd^22OsMDdI=Q=W5_~dL9z;g?{cZukhOefv>-U1M3BT_9U*~I!~<<`2Pq$ z$n{LOz{gz46`(roI(9PGvv@I2Pvb-@5cnIyerfad0>9ZZFFz&l;bQ$D{W)xkeSObz zz-Iv`zdan~hMXYmX%YDLCJxB>eA7YCJ|Vw&8JC|e^i)sf`mN*A9l&KBg(xGB3Hjxg z`kr;Zz5F!bWdEltxE9&&_X^zFPyY(sr4}ge`-Hr8UcT%CuD?k1FR6b6a69{d=74Vp zPUnGjPw@griE`r(@*@!#Brl%_OZf@F?aFNhPJSpCevt9@vB0~9{b(k3Ex3@&%je4& zuG!Tj@C9NVll6U1;FnwG_emFVJ#I^TZ2@j)=T0I2L_II>SW&L4nCp4lGJakq@Y8F# zptR>cfgin?<1mk1f5H4o>*t3i^Ym*1f1#M>+2re8;I3p?>v6z`PPebunZU{aQH$S} z3EW!WXC3tXS;$+@6{pYO>x7NZb7SQ?;S%6fZl0KkZV`34QQ*B{4xAwH$AR0m%TXm< z&oh>G^Z~b%zfQ=H6y?fxdCWl`Ulr9Db?1gPUH2^s>QUcP4M0=0xwbafuhj$^155EW zPk}}~TCe*<;f6pHBx}O@(olGbFQnH*@zqkguen9@hwCGu0KQBqH_u)|uMIW?bzftn zZ-tKU$!T1n)i(O-1A0w!efqO{_n{xe;h^Aj!6)m5v>%lNdA~|o0 zt}kt9*8MFl<@s({7^wB(%aim_*ng!S35SCI6&@0ssOyoYXlP2HCE#z42Ez^c%iQG! zO@XUYa3j7(XYLem_2!0@8j1{lxqC|4ysBz_=5+j1_3e2jdZlN2DGW9fk`&zN40EX_;wGF`v42wzj= zuUiK9&a9k1*Q0ypmFhZDv*s<(JzfENODi?K^xN~M&n+u4RN$M^syx+t_4FC#9^|-m zp8Yqol(;<=bNrl-2B`@|eZdeKLwrkI5t=0sEi13j^Hf$vCkeD{c5vR58u?9dlS^Ad zp2o&-qld0i|NQDaS)g05Zbnq;LBtkQGP^>_Sj4`E%mc#;Wqa4)>*%6^vMRR@Rg>_w zGj#!9ji+)>RS{EZ7CMoCZB1EpUX`vGP{h8}#sh~T!V0A62UTk29CB4blo<|l>JjW^ z3#)b5S;WAy*`*Ow5e61US40A2-4wD8{=xU4G7z10pSTV5-h%`n#dZ;#;9dqW7Ghg=U}|Y({JCYF^z*q}JR(y}Ni;dD)B-JwG=;x9IEpp_`f} zPd}(gLZ75K3O4AGkk60aNZ;+K^Pq;SSZ^)5a!FkGISMpQBAN`gsaXvNmYSP|3~vDHe%ol z*XuqOkh+-}GZ!1BZ2FBc3ZeG>X)$#aGA9=IBwcS{U(schT2_GynMcC&_}Tr{4ie2H6~N0=>v8ei1+^|5A|@tj5nHP+^+=3k+h zW7gH<$&6q#>yYLlS+Hp8Og< z&lRj|o1?E<2ggdM7+{NJ^pK0upf>&4U8pa>*PG&N7HK&Ys42UGwbMk*_ZSiRSPEYb zAgn3qDA0s6Q^Z_VbV_}=Axvwtz}JPJGMn+hqGefGDK|99!xO=j-YpN-2bnQt^RW!9 z!4TZs6sQp)jbV+39vXP~3Q38W6r&I8SoyMJCFLo?*Z$4t(L~)lU%!|QQ@rO4$f*=k zG{T`}J*GZd(}_`P-oP6xBAVMaPKX#Xo(Ol^E*J0T& z<|{u{&d;hgSI5`F@+CQ&-#pZjCeyT)Czf``)WNn|$|}?Caar|_-;3P9=0&Fg=plRGgVj}x`h;Nk7&(l)30f+>F25f{hEeBHWZo@C;G75 zb{x#!i79Z}G+CD7%BG1%jBmDSE1{8z7o0F1I_(%!Oo#QpU;~?+3Ih!_A-j%C%RzSm z@7@^7@%6m*Sboyx49hV!+L)R{F}2bl$Y<9|j2cvu=q>oB?l3x1z?{$%dX##?@!wd8 zMj{Tg(-icPxxNqOG0#A-ePZs&#~E0(t}(nEGkT3YrxDvJ z<0uFF!vU`?Xc>kj$u}@{PJe7&g4&?<4|Xo0C5PIxX&!+hT7lT?lM0vD`TDWKaGDC7 z*6`RnIqa#`so=l7^~_hcp0w^0`v^MSEn~3+d)R^FfcF-KSO9w1@Fxzu*!tSJFQ(Yh z^>bqowQLMZFUiF@ST0T_TUzu8ew-mpYn12;eHl)zu_d+}E3TOB)QpZ}g}owm%P<>< zFv>uoZtzlQwcll_dL4A8I?=+x1K+L>tV6jA(BH8s)Ugy>8m;rxC-*M61F^#goltlw zPA}OpD@GetRxTSG15T45JwXti)jU9STFhIH-469WW(wbI>eUfUENtq=0X94TQo5~W zpOBB|43%^Id~{PDS z=v0kXTJQ}smj}^}gIv2M3}1M8s%G(d%8YeX(e4l+`7qom z6#9LjYu|6{W;>l@r0A+_XmcJqQVeYeKc}%SNHK^9a@a4}PH+R<2AHdH+eQ#OY^V9p z$#4Ce|1c-pwfjW=5TqfBJ`ry8EyZ~rwmpu6^kj5Y_DchDL1Lp*RD;e16OIs_HXZ6g z!NFPSmPY{v@JauDSZlnsEj9Thi6%94n?E(4W zjq^rulFpv!Irso4!r0PP)0vt&mzh;HCn~3=PqBj?&2*Oozn4dB3Hv1e zZ~%|~jG^9cKt9xCe~k#J*u|h>vu#4(vqmR> zoXn2l4(7Qof2?~bYfylL4Av=)hc`AA=*MFjr-L2q({Q6(9nyA?k2^7P9vo)M_Z`#M7;tvcEzuD)or(ZJZ!TfVpWUtWS0 z()$n0;Oe<0@@+tx+z(V*(?Sk!bvbon=N&k`swm&;62pOQE6HTu~n|~(Nv0h9sD3!;x5C|!$Yt7vPB3p0F$fktEs^Qs-|%5z{YO2JjEd= zEex5?qS{b%Q=Q#5-EP6`^r*JKuMGt)PY2i&fPD7KGF}`G;rRf2>DXqEW%t}q>=NhT zp)Nf`Ok8_VubPdu1fD0-NP^0K{h;g6rJW+Ve_fib7>xI@`!U?6TGX&p9)2{xsTpVmWtiFzf2hwDsF~HnclMsTGFtNS%I;l5 z7A)$MQe)-C9x9thdWtr6h8_4K4Da<}MM8_6#1#nzIh<#e2$wMF0z(hb##Eh4h z>BWzw;YPf(nd+tJNlEhb#FjnQR|*z$WL~g|AL1(S@~N{i?I;tUzwwm@EVFrE!g>C5 zIBjO*o1K@;SVd#8)%Eh4t^U0>>*>~R5{>*wjYCmbC{~jsFW5s zbquHIbuQBA83}(Cn{r?_hmrDd7n8ke{>g^AXy9$+x!Q ztApR1uyxcj_Ouv7%j9qL84K=dyRM7Zt1x5`zn^bkD5H`&cO|X4`m3!jIcMs9tL3d< z=fgw!nKL$qU|C@IMr-m5BbHGnVIARAma=$IXDX*%+Bcl3=2g*|q;f?h24a@O4Go3!B zSsy6I*AL@UWb}>}|9F|r^iWM(P<4f4X)47EF(oSRSIW;@utzpblfwcJ!N+#${r^}1 zy(r|zo5fLl(zD*?<*5Hi10+0lsle`<9=V{i9O?;-y?LTM;gL^ws3;o`mgX@Bt0U0C zEcw`|G=L|t7@BQ1CT@opNu0*S#B;|1jY9oeQKs0vPELGP&pd&&`M4!DLBgtw-fpow zw@B(3SYaxkyt5x-o!*#nUbV_e*7g&}mIu!^C$=e{8&Qunosrn_^`-cnmUw**`ve*c z<(C>H4TZ)V?B;==Tq(kv-(aVY^F11a6Fyn;OL?Q7xDS=zOVgTs`kfB zPHVYT@6;xIh|%0+@FtoRY*IdBr+hfj!BNS*VRFJ=i9$5N`&w?(iu$N;323yD(>i&_ z-Y$)3xeeiHAQxYmksE0YV_O(qp+HNTgQ1!U!5RVKV_g&Ii$R!pov*1*%SF(k2u`Do zoU;tCQ{!`}2?-s%#z4qN8N^*A6xDLsFrV+GGmF`0*Yw`{)NgOGk?trj>tf2-hL+Ne`I;mCR&ceGp}F?bz;Wx%l!D z`kDsuT`b>(oP^A2_)8Z`=zHMllJsPrH2x)7^*RHIBk?EmPZs$l#n-4MTtgC4_`PqW z)%@s*4E*Msx0%%->Z>S{$7nNU*?zf|Dnh~LnxHL ze6i?WQ4nkSZ(8zqQ9$C7bnkII zE&sKcqkRB|TK?vHxum3%rC~yWwfrxE(|0AxW|F^0)%!5dV6Cq#U(!J+JX6Upf3IrD zPkDYR$5UqfOFR?#$$nYB{GEs_k$;WMD5%w*laZPFj?6EAk7R?$?-KQ={!f=ISLz`# zwS4(|S6d!o;##rAe=@(MV=eY;T|A%G$*CMC1ZtEo)8`^T>5}#rcX7@rk-x=Kelceo z|K^GE^9+RF$ovy5IMtOdm0F5me6J{flNf+b14?tLthcP!QNX0_ETj0R=l}T4Yb7db YmMc;EE;_sXFH&IOnj#HlspS0s504+}{{R30 literal 72120 zcmeHw3w#vS_5TEjq9O@M2q5aBsHhOLdGJycNEQ+Z7)d}B>9A}zWO1{*ZZ;wKi~>p; z(A4^%Ra?|*)ndig2h~>NbG4$4{aH(W;;Lor{22{r z@AsU0?z!il`<&U?D?Jr6`ebBi^6IOdrLojRj!YGd=0T>+RBfVmq&8I>rMa|2!0(4Y zDQBhI`hbZ`(*8WD^#!GT!{~zz9m!!$Qdv4t%I9=VJ%Mv1)v|e-@@3M;e4e(_m9sf6 zshW?K*CO&+=trYDE~%Q2e3Q7J^AnFJ(|l%(rga|5ahZ?YMfukLisNgaNTx2~Pf4Xe z$wvB{DEw)q)@obx)rovoTFQ&pB&9Ows`@xq)0^@tMZOJdxgIMWE&L*>Ebm<8Bfs?Y z63wp76XiLz!!S{vq%zJ<4+fTwFFHL~GcFhiH8ziHo;ZHo_@caqa9#m3f$VbOkFw94 zJ6}VybPy)osP$>#X{nERLj<4XFTeHOvwrhd$Nm2M?tSgra|Zq@I)1`)Btv#n9+IJp z^ob^C=|T8+mcS@<2KR06m#?||W*;^!d-ar-5hO%>3g-&^&B0$K{^WHoKFq^kwEz}? zF2vsj_**2f;v1%{{#VZK-IGS_-raoc?%jz;Z~yCw^#e-EhgL4gxbyy9!C!Bgao%fR zuD*GnZ`q2_4I?`JuhuV{@68^vVL-{H{crvDu+_JG?YUyMN2s$UZ9oUxaNaz^9={#| zYbQU$LC>{toSl3ooN0$|g){B&+Z@XM9|t=hLy+3(>2}b6E*jfT{x%2wuQ}9f%VG9< zjyT*N|Ca;4)S=uv9q^kS@XZeTFLS8xkI|9r{P2Q<{!<4(&C> z!Jdr{dOkp>wyW1%2YZSf{4mr(&u#~M9(KUzIkZc{p}rd(+UryY{O=Cs{?4JFUXOBV zOtW66I>hs<4*mB@hjMRnz(+ag+2nxlaj@rp2Yj#tzR|(|!yMw_0f)F+<Z!pb?O*_e*-;)M+81h;MXADju)R*@;>lESnW^k;3&e6T{ok@5Z=UTJ+y0{9~=}V&EjNS?tvG^6EKyjX&yN7HEk1qt$atg5i+A+PgI9*K~bZ zeK@2y#Jtg%u5(d)!3*Y2s#zKG)(4vX!QwgLSUBn~Zt%xuhP}ZB{$SV_h^_Q2*Ys4e z34w+RPw6=!Pm>+Dpa4oL!wrF0ARH$^?-axFO+~=wC;vbKvHb4e|J#neGMsM;) z1Kv;PO^ZZTzXoJ5r5Jiw6l$ckltL(jER}KqA8Odn^ zk+)D$1hMAOz!PE4+;Fts8%(w)wF8ZZhh>pw$W+A&#_M{lE*f5;r$*{hJLBCGp{X+J zZ}5kV&cr=X-g6I(r+}!fb*hzFf;5?zu)wi|^c6v)^ZvNM9l4|+UDUXXVW=k>4M*po zUwfDNrx&wf!ZWu@pIF))WGoLa*>)tz=MLEaXm%IsRk|-6S*g?T)8MJ}p^8-#D5|UB zq)Gl}pFcve*}G~_&@qa8V_xX(odUNT8hqY}myOl+WEP?$$*iha5CbL})OG}&2Sy#{gHw ziu|FPpq)J)XH&+rQKT2GS3uqghfuLbU#uKW=kYXE>s1qVgmHdtIJ&|ctZSWVBhXS#rN>t96R~4?(QEALnUf!dTsd~X&cRriEyh{U3?gD&n z2BGhc1QZ~>tMN2W0htQHSUmbUo&mWC)YuxT&co`-`0O2S% zhGp>vF-3c0eq9MdeOYs}9>J&?4tax^zV)Vjri2;;lPpcw6d*||NdQyL{&ra-cG(($ z&BwexG#~G;GPVFfOu-fwuc_f`7aqQ9#A&f^j5^8fChA)iE3OSTHq`x>Vr~+J9Qskr z@0nMRKEawQ>SsegYl{8$jS+vz&4%h372jWXV_2J`V@lJ5{?M{morm|`{k9AtkMS$e z5D7Q%RS&wiFWiWZ&T5UGZbWo1VuBe-As*yoB*huxVv?Lkm0r<%M-=E-4uySsT>z_T za;aRkuo~1x{Yl+xerQDim73>oXsq{(TGj@rt6k#P8XJOse?+VGgSeP;VYzr28TfobVEYT)xvzqvPK3lw9V60hDr6%E$4UB_(=cUcOecaN)G+<+?ks zFmIAJv!Z-D05FR33fu}$q_BGmy$; zsY8Hf0wXTbzS^PqBOaH@(E2kD*V6}RKgR0=l=Nn@bU*PgNgXEgk}g>~$x)iyLjRhd z=@V&FrB4{Z&#_&oMde8=v9mi&I|CS9WWf-@r*r9*uRRF zM1FZ+M(#Jai~C+(;(m?9GnSBcP1|e1HwyXwVqYoc<-RxDg3EpWFbgiv1zZ+f?zfvQ zxZJn*?_g%3#)68pET4`6#^O)j4`~s42%nBWd2N1*e}c0#x+fs7EeejUxN&tT_)!L` zX)h}H$qL@7;PQ-v*xMBR3`Kssf|n?GLcvQFyi395nFz77JGc>MDKyVrN zLWm*H69_+_0CCB43Bu2lAgDZ-Abg>M%X1jQ&r$G>Kkx*OYY@BU^`e5K0~lAQg5yZV zxV9-cjy8;IyMj~M@=7Q;wXwXq6r605*IosevBh{#atV?}M>4Md3XTqKT-gdPx*X#T zQ*d-x<8mqZKm*mZF$$ig;Q0zJW1QF%6!F^c>K1wU57H!ApX z3VyGGyA*u0f{#-0Eed|Tf_EtR2@3wAf{#}4P6a+NeaGQ!N(|gLcvc_@Gb>E zRl)Zv_-P8RiE)PFKTpB?E4Vz{C3d!g=PUBV6x^-gE(I@8@G%NrsNneuUZmg?6`U<6 z1aGQ>k5}Z&6kJScj9aPTlN9-d3NFuxiM>R@&s5~=6nu(;M-==l1#edHvlYBW!KW(t zS_Pk`;Oz?TQSc24K10DbD)>wVzgNM_6#U@FgC02OfrB16=z)VCIOu_c9ysWMgC02O zfrB3Sf5iizXC3?RvQ@kKm$hZScD<&Rt?7vM>Fz9B^<4jFSrxjAf4E-L_H>`v29kB0 zM)GyU*|X#Q?(Xh(mZpQOJv&}A(sY2eXUEe118kp zNYlX*>Tjg!Kne9X(sYo7`WtCFKtlbEG#wnF{zjS(j8K0gO$SA&zmcW`BGlhV)4>qx zZ={c9=?o)%982&1kF39orQbKwbnt`v8)-W5LH&(19rU37Mw$+IP=6y$2Ro?0k){J3 z)Za*-%+ePdX*$?J{f#so=%D^anhtVMeKJ4C-&B=|BecH_~*lj`|yEI+#KIjWiv|p#5bUz5S*(>g}Z(>+h6qO-`@3 zq}N!|ms`?}mUO)(?YE>CThj9_={c6P$C936Nf%qv<1FbDE$L${=^RV?NJ~1?lK#)y zWPg2WNq=fde{4y=V@dzTl77XKe%_M)gC+flCH;UUeYYijyCr>-CB5E~USmmLZb>&< z()E_K-;!QzNzb>W=UCDnOL~eWU2I8@v!qY7q>r(rb1dm2E$K{4`ajoN+TW7?)RO+# zl77dM{);93iY5KLCH)6W`VmX|0X6OZw7m7TAC$FzP`2u$uF7hUyTkob+4?D8U>fhv zxfPOM)aGR!w;F>476|L7ybq*oede1)Chd*oVCirjea6z^uqTpYrcm0VaKzk9=iU?b-+p zHDzu6P8`EjdG@+H%GR%mK&Y(s1$NGxI1yuiSsTaNZfa&M&*rk$`CH1`JS}By^VgQO zdODDP5$SfMHz3`K^fsh7;`8=4&%LdlM4RXLtG0jH>iK=yddmI`yQ{#9=z}5>U+WvK@xWyjB-|bGH6x*4dmmA_KJysd zQ$Wz*Qz6PKLm9JC2NZoFx=UH>8-$!s$okB|hG>5w>f)kqQ#6l@<{F~!FkU?IB`k;5 zLrl@WT=Z~5^fyBEk6iSvRfYlE2w9)`pHgY-wLaEt%U(C`H|+pD27J71jDIieMrrqLSr^Cw_fArqD+_sQunc+)W;8 zeFhot5h~EA+rP5r9m4WLjC{;G-xbiplT6QgmWA}xnR+%5*p_o&3mM09-9rdd5>Nc` zOZqIPhj?NI)pb3e5+us<91F(kj;uA^*fv4`QASmVGsnggZeGlEvzW7aG5nr8HFuOb zGM>1OQJA@A3^|S)mBp%(d8(nfgK3H5WkC&hcSVE1hg%i{LL%6keNkINRjeyVbh<#U^ewNZPE<6cw#iTJkBr2ZWq}a zrjsG0;3q6cJn;}$@XQs&U`_TEA?q_QHAF)~^crF?k4hUmVPn=~_+JF%7gTmD1=3uR z8v%I^FJXyU!eU;+c%y^?q6D%R9xgUT3%Ka2Y0~cZ884pr`g10FxG8!lL=&gM!`8;Q z3Oj$LF)~jeUGc;+($Ko5j5h`&K(9|~42j-iX^fXCFY5~A%X(qHhuhS2x#7zOLoS}U zWshu(E*woL2E~r&2GvqsTh~-_gBXE9zfLkpqK{e(nrswA2E8qu^e~EqKR>?AFz5q9 zL?1rHFi2+xWqvu8XpN#;ixp;p!{0 zkwr(o$c^Z5^Ae_;C7jJl5PdwJSb*M2&GjPFZ*8vaXiT&3$mU7_Rr}6Jrc(BuCxyi~ z?`GbKUut;gIYNYY5?!*nrr~5sX|Cs8+@Qy}L9Djxd8@(9?j(bxD-Oi~pc>R>6hz)0 zCk)Eu28}ljDm3KciSrDDHsk0E*;buRhLD1-ECz)N z)f{N4=5(e~R`VlK^zTVyTh6D=Mm7IIh^Xd3qngWb#)@VhW7xim85>XB$`x!f72L-Z z7`AsD!)?FeQ|N77)6B;#M$mARF`~6;t!rEy9zz6h8_R?xDB%cPgkB%AKc$HeM;A+N z;=w@GCZ1u`iTXf7nBGAawdL$yVbuMfgowHi#%JEd7va1a*(Mlqdo8PMJn?f@r6eoQ zWL8qI87{#oEi9B$59k0b1JNlJW{vjzxvkZYHJ3i4^{b!v?Mv?=sW!g}GW-jVa zV!V~ktq_HjM zc(dr^47qrs1YBO#+i-h9sp?)*E~?7o?KEDWXa4nOR{1ll_8y*f{kMEgdf^$;18Z0z zSkJRsBza&mZ|qwe4G(O9P@F|F9H~7ub$XVTSe@*ZTSt&|QX~x~iIjLBfsTbBc6DW#FMS8U{+GR zpCZH&@7MBzHpC1U+{6kp;(gO_;=~g_+d&F>)S1Dz1*4r=M;QbiK-D@%nMxT1Lqr{S zk;b;1W6YvQ7;^E%S>W56gxe=QiNCE2vHxXiXf=$DSgZd|I>z~xdM*S^q9Azqv`imyoMH*3mv*;0q9O@6Q zs6Xz>sr4u2={iAX*7ZAhCpd4SbV~~>B%av7YLVoYVjclEM-31Bn1oCZ+@H%mFp{iG ziSiNraaRqN)Y}o!+Le^()G(@ivqCexJ51OKzxafb1q}D}3w1JDRHbt)jlDHd=vaWHy z!TnSe_q0)5uF!Eh*D=-9F@+Up^sDNjRBSwPh^<%OMbxEw^%J0~S4S|F(yPB0Mc?=d z^Xl^v!zs@YBF2tSK9*iRo7?U;Y+uBT#d!@^u-a5`6;oi?z9EO({%`0?jfmrIZ9mmw z`wtA8sM~77_HCrGE$1+^=tB&-c;ZyU_EosAjcldGWC$tvIm;1GJjNBg7&a>ZJRxQ@ zzdeN89wOhRtZ{CpxJBn0#;5f$BAkCIG7bO8F4HIsk(6mP%anQgcxlw(!l>iP1R9J< zf=|9raUZ)(JC8}WvxpU$l&9@ORyylr>-hp20rXo05(|~A)g>7y+2lM1jv6jI$$Dd| zSuc$96WW$@bI9=6j}1Ba6J_#Fn0*8(&-58CAIofyCrU}2YDZ4vwR@kp!Ud+zvzbmK zf)>yk2F53tXRNcz(xeEIqv~=?1dTMzqyTtQSaUNusVyg7Z&c?wLPYc@K9JR!h9@VK zQT29GE*hS<(`mdua%q84RV*zUjRE6SR=;>+I%}9D54=alwdG7T&6onAr#fC3rzHx( z``7KGS+p1)DIH7(C$;yj?~`M#JEdn47^y?U%Rtq(?nXt;w!T9cx{}bgocn`@bAD;a z#S<^1Jl@u$4K( ztrbtqBeX4N)kRbQTNJbyawu=Rbklzhg@CyjI-ivHEQZ_#+*Ru{KVgN$6W>Csr5D}G zow#qgVaC@G;xY7DvM&zFCYgAmnc$R=X-90OhRg<_YRGIQ3yk3*BwA($p=~)UOe123 z9M%)>Nnh+fgnW@XG+$QiNKvs9xq|VgfDC-DQv_BU;f1DJ3 zOo;x8i*64PgEi4RKoS{<1?wDr9=GgE!?FgUqmAph&D8M|Ld*!;Jdm`<6HCd86t7N3 z{H1zzI#AWCO-!YXuwlX}UGK7*jxvir){w(^0xs`}w`RhE^_e#qwm-&l#1ntw3bxmo z^(Vx%{fi^H?X~YP+nc)?ml4z`L~U?V1 zU}hh~%xq!i7^WyG%kxyxww#kp%TIt1kLuyXGwYC#0-vOf7&XFrUQ{I6rXz*LS8|I* zHKoO$4d8iy#GPQ7Iag-cHQ-f1)dqZo@*3UZJW;XJ32n<+vdr-RVnZ&TxC*xN27I$G z1lDKn%Og1&Nxxz_;)#J=!EvU7kxW5m{i#y)Wg+?@)x0g|i&~?s-9QpKh)Qd$U&M>M z&nWI%q2uRV$7WN<1B95qe(ng;9#7183vf!T&qYs2jr9dURagCxsg$ukmbGv^(T^8h zY!+Q$$l)Xy<#AU%*asG1sxfSTndOKlJ|vS+f4^COLQLEH3)>IpK~TvMGR@K{;TlbP9g-heBpG!@KY3XGwp<}hx1Emb$=e#p-Z+b=+U zWgFdNvHeuTCUWssQS>ih9c=%L&#?W^goqX2SKEZ`JLe%!Kf~}4voxN#hGdf(r~{>{ z4K#$j-Imj6>W`8?*tlfI;A*Phde27cL-b}ud~T8bsTGgP%ug84v}COwif+1N0REw; zT3G4SAb6et#_l}S1evzc%))KN{|Ovma#@z4$g<-mM2iLg_OHPGsrV;^&q>8!CA>Hl zf0XcJQt_V=POliYlzR=~pCU{wc!==7q~Z$*|3fN%7U6fN;wKTlJ{2EGcw;L5)f>PU zr{eDtPOq%A*!cqC<5KZo6P}Zb-%R*_5DXSQR}lVjDqc(YE2;Qg!XHV+#}j^gDt;{C zYf|w;fbWc0jM@DcfQu~P+XT+AfN=ud7VtL&jhgUVjb~hS`CdGaq%{_xE5ExG zoeWuvkrm04mQa#TH=k#W7s}c)|E+V8C`@fPwLVEYo~@8@YL~05_3{Kb2|V?-cMJ04DTvrW4#X4pL*vdPWh(D=Rwnz_0DBmZ zbd%Dysr{3bW3s8!aZX=>F~w!Si(*N&~!`;wu^^wP7YcVI!1AKIw4O!|FWA1&l~-j+WToFw@~ZSb64*r{!ZYk%R4lBSd~vIXZ(lUJ-?$YqZ1DUQrOc&n?Ue~ zr?;3k_RN;IKk9p|?B=4B>8PLRq(>v9Ax;w+f1nf(Xa1GSV|_NYEkzxlVKe_T$cf-D z!`E+!!Uk66W5g7KGx1q%X`{zficI~>T06^5d66EdO>RLKTn4j}pL4~Oi*}M>bQv!w zrGbW`0_M;F_uK12pIQ$yOnqY=^u2%-nPji<9wnT+8I5b&mE6}@-za-}J04T+Ox3F(NqrAJ zl<8)@^eU)lN$R+5gK6kgN=wQ|&2RMXJCggX#b1kQv}I1pBK>^ix`!fCz{e6Cr;e(% zCr=%uX$*C2h;Ab{e|{)wKnqYSApYU0J%zA`ZySV3t6z~O?Q{sn%-JJVX~=Zwcqidap0 z6xxLUc9NVlNw&9*MYEwJsYUx37G1)Y9n4ANxJBR2mlmx)*Km?ovFK>4MWaoN&bPD3 z+Jgr{LD~pRZqfOFlJHzv0>`?V<4H!si zbs7*4<(xtlZ)4w7L1s2FEuh(gt#>eytlFE^GzHJB(^S0eahf&6G95P$-gA)R_)Ee7RuSY-O1(&-yM+h$6+vVF8ChC zHEV8h%+9o#r?BD47}E}<)y>JOEoSXb=2F3{F5j!MU8-u15;G(t1Mf7?UbXL7l)nq* zNpZ;cH#|G_)m4{wX|Y^B1W?zVgD%WAX20b*4GW}VRxYjajSl`qY8gqrbsP9ta67l# zUm&24FtR9CL_9&iSL2+Senp2y($Pt?cG9>9?)zi|=%WZ=XI=O8NNUVJ(Fty%Zzm0v z1LbUFrZ4ft6LdxiS z4>pqRt>31T+6&F!?b1fFibjlNvwmur)%GW}VohWZN8qe9&3wmVLQRL~=xZH~y8r;>}|6!U%r=ZRS{tJ1? zS$$BjvlQLX`|j>?+1GXfu+*?>mSUB~S!n;%(`s{|r_(SR-PxeZM)(?NbvUHiIb4ov zF=6%MMb#?e9Qa@L+p^0{$eMTGU9Vi;!W$8UzxyS9wCYk`08I_QCe z9ysWM|4SaAUvHs{|Ep;~{3$mCKY%lNGJZg3vP=9Fq$|*XkHJQN5I-EJ(a-7#{)%Wg zw9JJc^YJ&k@Izoyo~zMH<*#D8rd5`^s=|#?pWjvOZ-}`{@H>{8_Q8$oKDe<({eKbT zRz}10%ae;$&IdQPL;h-)JI_^#9~k6nT=D{kaSQTX{1*{j=gd9*oEbB?nB*2BulPMp zCBNhr<+e)|H=H=ySr`PzKPLIV_&aaxDugwh$4qB-( zR5OYXbD8Tx5i6s{fe77#}JdgXnN@7cSwk0~9yX9q_;C zZ;MhpiD4YG$>6&T!(fv{(XR|xF&Fv|RGgzVVRdRWT98FXcP`<-kIOoqQ59%O_rI2s zoAq6EtUOItbuK=7qZkW~kz+D``s2uw#%L5jDC)vbwy~e*MZ+B_GM2|etg+w zz#|7p)IK>C^*Gci!#N0M zPR;Ji(iDK=gUOl4MkH6Ax4{((yV$R0=4r_wTM{64)nA#OJX!s*P*bH-OeX*xfx9^xVze z-J5XIebPhS-CIE~{SEHT;t7PK@DOAtzJ?e8r5Dj$4Z0cfKPMdYUBW@X0L?}|Hy*vr z2fYNe0`y_fAm|@K*MaT>-359Qo;@A_{gt3G&?e9uK|cb07_=WANPZpkMbLeq*>81s zyYK|eM9`_At3j85z7E;~I^^x{?v2C;?Esw%x*aqKN`U}t^d}VJdXf^0A(0f3$@tpV;(0tHOKZpxOB9%1=S*TP~YGD?#4|jevd$+73D#UlZL7 zIsvp3bOC4=Xc#mbUnKZ3Xg=r`&`QuZKqH`^fwqGl^>^e4b%S<-R)Thc(zjo;@kN() zp!uM8gI0pl7pfzme+O*`%}gLa=+U5^pr?R#ftG@1W6xg$nh$zAXeH>2pb^j=pzWaj zcOXCLNuZse6`)<9^o{szd{b%zXg=tNpp~G9;e0a!dOB!3=t|Jdpg#xg1brT~3p5*t z#@YA|(|FK)&n?bukJ3;&IL4MFtpxGFw3PJNh z&jqan^?^n}n?Tz^e*wA~lrFf9T^X0m(=wW~Gmai`Xn#8MBplCwXs-LayH6sC(nAqp znl>7LMfe+YC(6Xxyf!d<#=v2-vW{5MzePKH#N<;8M;{NA<-ZWL>u%UYfVfC+41WpW z^!&QKsH}DP`{Qff-FUo4(>w#Sujx~ASp{K0AX zlr7_u$_f?*hu%6VAH$Q31!{BpJNZ|bH}Rq0W9aO%eacq#?R)KL!_G<^1iTI(o=6I% z%!Q^M5%Avxzh33{sWI}mgFgm#;-NV+{{o($Y@iq*{!J1L@%V;}lb#s#xWH$27pLek?4}r43pr-@@Dw>_ zH`VVs$nBAG|G|ix><6QMv|hdN4QIOsz~2{u&+K|SMUP=u5#*kM9JA~06gi{3C0Or{ z#rn7yHX1esdHa&x6a&-2r!S55yGYT4IO#iq4R5Sn?uDKuSU=m13oht+4tj`xEqPlZ_H6<`iu@SOO#5b;1?LvDGr;$79CVR`6*LK0@W6Yw~x2KNftuF@*Zx0PI1D zKS|9WG4qcF-vj=sDqmS+Re)a!zSH;+1V5dB)`|R%?GOH3=x6J3*}oQ>_H6~9+Rur< z3;YYx@COXw@jY9uU!Uo$0P@dR@Y~YluK@o?Y4}0#ZvlT+lKyFi{&nC#ohJV#@Z)Lt zTfu(?d?)*NfxkCR{sA}x_%;oHEcpF!M&qQv0{qj#ck+J_{NgnHb>N=?zLS48fnN*0 zQ~%rwz7D=q|J((>FHQLa(6It(_+!BjgYRTt1^7QnlRpUl&(rYNfqysnPX65lerKBe zTfu)L4SyH-Z-eh-|9~t_I|OHLAm;co!|Y#U!T1vS3sio<Dt@FQu;AAo`{PLqEu_%wbv*;fI^f;9Xf_+{YR?FHES7yQL( z@^1qF9Ppjy@2%j^1>dQE?E=3d4SxU@*0a*^$AVAxJFUMfz&JAvKL~z)8vZ))>AfUQ z`Zs|wEDe7v_(y>6Sby*j10QZM$2VoaKLCew-(dc8%0CwTufcc9UjhEkH2fg=?}9&0 zEuSC#usz;7@OOcKy2@Af7n{I8;}6~43sim|e!xfZvlaY{!MB?qDNA>E#`S%tY{O3Z z(SDgXmuP?Mmw8o*)|Q$1S()~D<}H2REYsdPbnL?=+F$$U;`8Ri{(_ehXg42`yA{w8 zPxL|Ju7SB9m1rLfoQBU2XXWlI(Z0;uN>YCwlzU^T_M^eX_;7IUrc&(}N3~Oq=W}wO zE7h(UdJ{=)%FTVdRQojdeoEXsJok%IZO8B%D6#J7+;tx9*`x2G#J@)7{=%dE`k22_ zV#9H{zxQZ=JC2F%bLGD7(SAE>3vu=xpSuf*6CS6;x1)1c&(NMaaWxhC`pLPs&CqTd zvmBq>Psx3FhW6SicMxOM*xao%wCBcNj?aG{m;2EStu>F;^0U)(_s!5Y=kqETtQ+J!csdn*I2x1N|W;zFOMy^xW4O`nXhf}$R5O$8+JILD9XM8lSfUF^&apY{qq zt;guXkt@5t{I)wxA2wt4MHl8+eu;u`G|#RjILD{!a89&agkeXBG~K7Ai`^6BAKAY` zt#DI9~Zbg;Cv6ORJ0^akPBuSHsxBZ@m5~QP9nTb_m)j z=ypN71l0;e`GUFx%@=g4pp}9y5i}xbi=gd-ZWMI0pdEsC3c6j;E!paFl~Q_18Wa zt@177HsvFHi3KOO5pEsl%YkQLx4QWmd|;nBozFh!8si^FaeosZNROO9B)*o(4>iV9 ziT@HG?CSd{lOL%)WGVM~h7Z>Ix8MUVF??#f@Hs;ptPN-()SM3vheHWJvW3CMJTXS# z11&gxPm$!aEO=1h*%o|_zz13Ip8+R7{7To@#rE*{^s2x!mT+9oGfwUJccy=ccJG~B zUj5h3Z(l+lPwZ(M$l=P>zYo=GxMm#}2QhqvF^|YBCj&no_ODyQ5q8Flk23{c+r)8p z_K%Mn1b*u!9GCOOPJst&IetAS+Azd1>91aqEZ+)zu#i#7V?w@aB}b(Uz08C3WG_$F zqYFG9kSzV3fe=`CG!@7JAqjBY1xUK1#!^FXqoNLjDUOKgQ1iS;rsr=X##>a{N>wf2Y8I zCHyAi;XQ$WBjQBr$-_b*8;g-)9YTR1#lXoiYwMR|x+M74@QZg^Lzn9Rd*K zQpigiRL)(%2N9=*qX<`a{YL1&O|;7_fq&zmCl~8WvU6*Y3(9uCQsAG|a9q~wX@TDn z<+$|!V60k@%~DdK!KhqZSE-s$5&g}{q&kIJsT=K?=o$fzXfApbiD zobHj4okK!gi>%kMERIjLjAK6(_>H2yP8Ef}CGe>5v&2Vcb3L1ualspee7V3U3wb%+ z-Xw6Bh^tXTej9Ka2aY;}1N7V>UGFfQe^&n;J&2cEDdI3sFs}q|=g+%@{3g-9LcKN| z!gliXOEToQyyaZ+^+Nx}0=LH7;{qSCgbRvohW0A(L4A(a`gaHgg1jZS_3 zoJ|Zwt+&XS&AtTwDE9NOVFEu11aUI8(3 zWT8F&gwSIhHwKL0p2yba2wM#3J7=@Y96-($Aj&AB26x)DEs! z#@k-tcJ_}%0Ndd~;8bp@X!p~(I_+lyzaqea8wCEcgPxBZ@bfSslKx0DS0Kmdp9=i4 zr5u;@<_7{Fv4rEO4!gb*c&BK0bZ2(eySV;cbv!NY{HehEiTOhIyH5q)F5(1cv+F__ zMs|)A`YQ$gkiZLsf20rTN03Q=Uo97u@Q8U=o@z)u$VR)Omx9%c#r z^b_pc>&AS-v+*S+E-2&ee8^L|OT8R-3q5Nb^t>eGJ1zY>AC4kDA2)FY1wzm50{=#| zqx8d32uPB*u6q{(moh>HBlQmW4;}Eo3;nx-T+6va*)&v~%C*K%tH1|}xDwUVegoXj zo|hePdI*d3Th|-2(Qt(STEq{+fL+%Ke4?fQehA#I+`=(jPn#GwWV_!i@Hd42vxNRV z!0q&$bBaA4bil85z;7p<4u26}>U9@zs_#ZioE&p1Uxy6qP^Ix*G~gsZp@HLYBfHiJ zyu#w2j|H9(wie}Ygz7< z!0q(h>43)_@P9hs{l@Wf|70omY~Xg~-T<7&`IZh|&+H6N?`1gutp5A9(EpOvpLx98 z+eE*3R%BW%@CR0MK-%ySaQRu)|Fr`?8WRV#d#y#!4+Q?Mz@?vG68J|KbL}!trXv1G z LA;yU28pVc~4Yc%avA^#`Ky#Kktr&;Woje!xPytd_;WIdI@soZ~tI6hpk*9m-y zuv7N;Z315`;%c^#pHX1%w+n!i{!jc|uujO|D)1+Te(8q+h4y;rR~kuAev~Va4{qA{ab+#7Ijz2Q$pT4j}M-}^*p(p zOWzq$4|1a?-U1oF>tcyjTqN6T-bA+z`qdfBI9tkgPubti+VM21+#>nG{hQfYx8_sjX&yNhSx&*qk61f_XWcte*+|I!uqmcc&Rt2*Tlln2Ho4(tog$A zk)R(hU&_n3m(Xhip@8mQmc)6>-~C7V}1QfWKmK&Bx6aU{PiRBqNhAx zB%Utndl1R>-r0dHS7xuU=g3;nfTylhDihJ6?5k#I2JTj?RO@wy&q zhy^G4oBh7VSRfoKXmVE+Hux`2!;N^O&fH1h>W!ha8j1~mg?m!@+^TAQ#x(p>b^hEE zeV%7pDGWC-BZrE#fVuIm>R1?Enwk#CNhSlS%)M5E!ThptlJIo0{HK(}7qh^W#7h%KgMR;7}$n7y~m1H+1B zd)MIgbuoW=m0O3Z33yFSo!?vInK!$tn5i@i9nW8LQy!aJr7H#$vzOp_;4nm3p)`HJ zN}V^GTvZrjhQpkC1bg|yY8`eKGq8MCX#`b-fkm;E5kFZs$zmO^SwR6_EhJxHCEmV7 zuTK)@O~BjuR(PW|;H^RIG=>l&>D+!!wN7f>riaFbt;`fJhm zLq7O&f-VfDR-T>e!a%YMCj}a+8h!L~zZ5sdl*Ut>?qMZQ62YN+gBWi@2=XSs@S7Po z7SC0UXO6Qms_8q7>V@+hjQ4nY>^rpw)H$dtW^Y<#u5;*er3K(NhNx#xomo*ny+kj_ zE66MU?mp-yN9XJN6&L7_6cvGx9tnDV=!x_mNSy~XD$aUp@kLAP0x{|{bE~}NJ`Wkl zEnB!yFU%`qoynb#(E(G5mtqP-hKFib96(chF^A}}s5cO6C`UJO7uN)8MeymYgH#pj z^na{id=!ITxL)_N@Y2o9n6=mlWz!3c5eT*KiT~9R$edK%6LdYqUgu?$T3(3??LUjD9*!U*D57jP50K;YY7o!1&QVazX_}B-qH`>{_SL$ZrQ@sEx z>|VY_V_OmR@^W~j7gZ=AYtOMCEzp2`R6D0?BCknrN}y`M9p_XPkq$8uEB-YHk6CqW zte=PBOKn{-PVlHe=#^m_(6NZ{V-|7S%@~Su(RiMUrEYZ&VYc{dyfN>0$C_ofbL#7A ztj$)pYJcmaU zb?*Xw0UM@x&*_y@DWqtGL(6(heYBnvqtx8qH&#S6w{4slPQo5+R7ITHDl^v+bm;TqY=q* zLHvD}M#@yvn?9j7U-44x1zB*wH(s5kbe|Xdi>W0G7fzdAuDkOJv9};5_Ij*)rY=zS z*Ry<9EYj5lMP7j$FQ|?N{h?I-z3ljIH+mknFSLD#^3ItqtPb2lind0yg0$(^G}rWS zRe`aKhCwzIniD7bu-tO&&%TK%aN00g7S_s!iAId?w_#%=6E8SvI&|7CrkM`wy@3#$ zoQnLRnxI|BrRAWzkauqk<#?5EJ(i!eF~f39jW(v{KuoPP2=du=9!3o+N%R)H)jN!i z@*2|WRAC4Zb^*^3I-axdAYH+>KWn)l!ULH=h@^B*A+^k3Z(S|UsQDQ6gCY)GflW8|rET84n zjE-Z4y&`qXFbjt;%0QuR@X~0t-({(K9dxEUpTfZd->&zrL%9pl-?1swu@qYttMk;S z_Aa;svBL+QV0anMBH0luMjKUDE*qnMr%90R9*E9r9tS!t<}HV42YMefiElRb>Ifzl zHg)6hnw@_s-PW>CD8N02d9!_dbW?6u(1adn(CH=WxKR-ZVfk5((=pG|N?H@@!N5`< z-2jNjbXw9F^F|?^s?mx|9jtr1U{p4Q#%+`kPU)cjKpt>;5ZySIwOhjQg_oylCZDIw zSVtA@4gpe+ww*$u$H%kw{kCqF(>X?(uF8%MoQIAyL)-q(X{-xU4B}=S_6xQXTral) z6c&6nZrccAhwU`~Ir*(;^B?A9yLKPXZ)t>L=o8_ncNxy}um)44R(7#et6@&5uBj2yLk>iz=<&WM>U-rs&koHRdb?pYWg%g*j}x0v2>e8Gm>5x zz`c8U#Fn&A;x`9yBhMJ>?FQrnJ@(g#fJ$x7I;ZBO>CL!L(wmt`J-iAud+HHR#!X!e z8aCS|^gU~I@>|L57;b;=>55yr2e1Z(ILKg~(ztnJQ-L1bmT@}RvEB(cy43+~2X^z4 z@3{_G+ZHw8Mu2dQdC%Ty;m(@!d)#a`BUoj^j^<(_f4<9^Jj{e;Rbbx|8_`$h& z@@0B4J7N}wV}L^RSr(0UXH-6Vl%QBoSwO2d_4f1BkKOm_LFn+u6kz6fW7#VY!s(2^ zw|x}H=lL~$c?gC&kJl?vL*8F|u@rP_`5rF?={9z+YC}^2yZw!!rY2a~RIi-lsDdrDTKw0qYx>9Gsh zGkbAzh+S3{?(aob>U8%HJ=lj(#BFk$b)Ol7e0~^D_f%_ZX_%*tJf27N)m^05;xRd6 zky@n1dEEY8|wn&#xH~yCwYen>=1T(mLPKkVGkd^`av3=^v!4wWc<->ma7xq0+ zZtVePvpDP{EYGc>w!Jv~pb5adzfMo!n0F}DSn1_xpHG{sKm&e%63wIPGsgJuXd#E^ zznr!w&U<;pmOBa0XolDW6YQpSFNPUUEuC>8~ve2A5%Ui&{~l(@^W^zhIF!E6!2 z48Y{7duwWNE2|+~+q^_>gOEH#%=*(2C-(J+1`mDj19Jx5W;33Mrl(1kg`;>7G~JWd6H?^qHZXfu zKq*)Zj`@KGeln~)AgE5u)6a~e%) z2#lm>YV2pUK)8WDep*zHXMPc5Ny}d9AqVh?Rfdw;>EvF)`ZQD0;phQBSK;R&wB3@= zbHi8reHT7lX?%;tte&e?uMWopd1G56@j|=99R^S_G zI(>Pw-ai#DF~;}H=;N3UBREia1oufQ?DZh!qZqYDJ4y!Z-AKRJt z{0js02$2uZ9LMne&w883rv6tNAn7(sC3e$viv^wKKu=)onH1%Yj=Y;gMcH_;G@m(G z9f53i$jEzO@6BRpw%J{{9Ud`p8WWSx8GAJf^=Mg`W(zt|eTdIoyYP5sUm}))Z2b~3 z8pJ%GwE5a5HCWObjeS?erX^Ck5f+uoH~65(#??+w-Z(E*<& zVKa}JqDUaLjP*$hfCGHj(A3Ai1t!WBxq6b|Wtm4dZMxwo-wXHU@$<;{r7T!q>yb z(aS}cc%8SQPRm2kp$JZ6QO;??W7+sVYf?f7FX|6^DTDYF3C6TMc5^HbAM=)lk)#g| zexH_y2f&bI&kN^8!}Khgmglb%?-!}7fi{^!F+Mzx%)UZ`@8r?ahiT=n9^pFVXVODv zKqWKTTOUAKTst<$S{`12La%zTd1p;;prer~1Al!)W_tfTU6PLEN#kFVRj(6}I23;} z|3r~r(tK4IpZiH#wv&c{OlbzCj0c-2=fvPO?%iqnB zRQ_&`EMMlA^}j{rpDq;2-{Fz8Gm|qY{|Nk9^WOoC{3`Vu@4Xa(NAay**7m;*Of|nt z8>F>E&sKcqkRm9nt#)sTvF1B(lDXGTK|6mr*|&OW|F^)mAId0 zu+~?WFKJ&CPVbm&$s} pdL0H#+RieHe_UJnhvB6vYL+WedZ(RT{z>8mag(H>B7-HV{U1r@LD~QS diff --git a/c++/example/dynamixel_api/basic_test.cpp b/c++/example/dynamixel_api/basic_test.cpp index 916b10dbd..9eaa0ebe0 100644 --- a/c++/example/dynamixel_api/basic_test.cpp +++ b/c++/example/dynamixel_api/basic_test.cpp @@ -66,7 +66,7 @@ int kbhit(void) int main() { - int baudrate = 57600; + int baudrate = 3000000; std::cout << "Dynamixel API Source Test Code" << std::endl; std::cout << "┌─────[Test Process]────┐" << std::endl; std::cout << "│ 1. Ping Test │" << std::endl; @@ -149,7 +149,7 @@ int main() if (getch() == 0x1B) { return 0; } - result_void = motor1->setPositionControlMode(); + result_void = motor1->setOperatingMode(dynamixel::Motor::OperatingMode::POSITION); if (!result_void.isSuccess()) { std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; return 1; @@ -237,7 +237,7 @@ int main() } std::cout << "───────────[Velocity Control Test]─────────────" << std::endl; - result_void = motor1->setVelocityControlMode(); + result_void = motor1->setOperatingMode(dynamixel::Motor::OperatingMode::VELOCITY); if (!result_void.isSuccess()) { std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; return 1; @@ -303,7 +303,7 @@ int main() } std::cout << "───────────[Direction Test]─────────────" << std::endl; - result_void = motor1->setReverseDirection(); + result_void = motor1->setDirection(dynamixel::Motor::Direction::REVERSE); if (!result_void.isSuccess()) { std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; return 1; @@ -317,7 +317,7 @@ int main() usleep(3000000); motor1->setGoalVelocity(0); motor1->disableTorque(); - result_void = motor1->setNormalDirection(); + result_void = motor1->setDirection(dynamixel::Motor::Direction::NORMAL); if (!result_void.isSuccess()) { std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; return 1; diff --git a/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp b/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp index 3a16c7827..bcb6b0103 100644 --- a/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp +++ b/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp @@ -33,6 +33,22 @@ class Connector; class Motor { public: + enum class OperatingMode { + POSITION = 3, + VELOCITY = 1, + CURRENT = 0 + }; + + enum class ProfileConfiguration { + VELOCITY_BASED = 0, + TIME_BASED = 1 + }; + + enum class Direction { + NORMAL = 0, + REVERSE = 1 + }; + Motor(uint8_t id, uint16_t model_number, Connector * connector); virtual ~Motor(); @@ -54,13 +70,9 @@ class Motor Result getVelocityLimit(); Result changeID(uint8_t new_id); - Result setPositionControlMode(); - Result setVelocityControlMode(); - Result setCurrentControlMode(); - Result setTimeBasedProfile(); - Result setVelocityBasedProfile(); - Result setNormalDirection(); - Result setReverseDirection(); + Result setOperatingMode(OperatingMode mode); + Result setProfileConfiguration(ProfileConfiguration config); + Result setDirection(Direction direction); Result reboot(); Result factoryResetAll(); diff --git a/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp b/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp index 42667403d..6a1de6a43 100644 --- a/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp +++ b/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp @@ -213,40 +213,22 @@ Result Motor::changeID(uint8_t new_id) return result; } -Result Motor::setPositionControlMode() +Result Motor::setOperatingMode(OperatingMode mode) { Result item_result = getControlTableItem("Operating Mode"); if (!item_result.isSuccess()) { return item_result.error(); } const ControlTableItem & item = item_result.value(); - Result result = connector_->write1ByteData(id_, item.address, 3); - return result; -} - -Result Motor::setVelocityControlMode() -{ - Result item_result = getControlTableItem("Operating Mode"); - if (!item_result.isSuccess()) { - return item_result.error(); - } - const ControlTableItem & item = item_result.value(); - Result result = connector_->write1ByteData(id_, item.address, 1); - return result; -} - -Result Motor::setCurrentControlMode() -{ - Result item_result = getControlTableItem("Operating Mode"); - if (!item_result.isSuccess()) { - return item_result.error(); + uint8_t mode_value = static_cast(mode); + Result result = connector_->write1ByteData(id_, item.address, mode_value); + if (!result.isSuccess()) { + return result.error(); } - const ControlTableItem & item = item_result.value(); - Result result = connector_->write1ByteData(id_, item.address, 0); return result; } -Result Motor::setTimeBasedProfile() +Result Motor::setProfileConfiguration(ProfileConfiguration config) { Result item_result = getControlTableItem("Drive Mode"); if (!item_result.isSuccess()) { @@ -257,47 +239,20 @@ Result Motor::setTimeBasedProfile() if (!read_result.isSuccess()) { return read_result.error(); } - uint8_t drive_mode = read_result.value(); - drive_mode |= 0b00000100; - Result result = connector_->write1ByteData(id_, item.address, drive_mode); - return result; -} -Result Motor::setVelocityBasedProfile() -{ - Result item_result = getControlTableItem("Drive Mode"); - if (!item_result.isSuccess()) { - return item_result.error(); - } - const ControlTableItem & item = item_result.value(); - auto read_result = connector_->read1ByteData(id_, item.address); - if (!read_result.isSuccess()) { - return read_result.error(); - } uint8_t drive_mode = read_result.value(); - drive_mode &= 0b11111011; - Result result = connector_->write1ByteData(id_, item.address, drive_mode); - return result; -} + const uint8_t PROFILE_BIT_MASK = 0b00000100; -Result Motor::setNormalDirection() -{ - Result item_result = getControlTableItem("Drive Mode"); - if (!item_result.isSuccess()) { - return item_result.error(); - } - const ControlTableItem & item = item_result.value(); - auto read_result = connector_->read1ByteData(id_, item.address); - if (!read_result.isSuccess()) { - return read_result.error(); + if (config == ProfileConfiguration::TIME_BASED) { + drive_mode |= PROFILE_BIT_MASK; + } else if(config == ProfileConfiguration::VELOCITY_BASED) { + drive_mode &= ~PROFILE_BIT_MASK; } - uint8_t drive_mode = read_result.value(); - drive_mode &= 0b11111110; Result result = connector_->write1ByteData(id_, item.address, drive_mode); return result; } -Result Motor::setReverseDirection() +Result Motor::setDirection(Direction direction) { Result item_result = getControlTableItem("Drive Mode"); if (!item_result.isSuccess()) { @@ -309,7 +264,12 @@ Result Motor::setReverseDirection() return read_result.error(); } uint8_t drive_mode = read_result.value(); - drive_mode |= 0b00000001; + const uint8_t DIRECTION_BIT_MASK = 0b00000001; + if (direction == Direction::NORMAL) { + drive_mode &= ~DIRECTION_BIT_MASK; + } else if (direction == Direction::REVERSE) { + drive_mode |= DIRECTION_BIT_MASK; + } Result result = connector_->write1ByteData(id_, item.address, drive_mode); return result; } From 2cc597e4ec94f094681870ba52e1e700ec81e3f4 Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Thu, 11 Sep 2025 11:52:39 +0900 Subject: [PATCH 18/37] example code update Signed-off-by: Hyungyu Kim --- c++/example/dynamixel_api/basic_test | Bin 72000 -> 72096 bytes c++/example/dynamixel_api/basic_test.cpp | 7 ++++++- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/c++/example/dynamixel_api/basic_test b/c++/example/dynamixel_api/basic_test index ed247747c0df910dc4a5a4ffd5b76469005000da..be2271f3c8b054b35c953d1e34ead5a3f562b42f 100755 GIT binary patch delta 20839 zcmaJ}30xIb+n>2|Q4|AYxgaRuz5{NND=ut`2BxO@ni6ifg5s9FUQAI5QSew;TDi2j z24U_ME_pRKG`G}p$!ttB$QIL#@Bf^0W(F>O@BH-qXP)Od&wkF#9pILH;$TXc>K|U0 zmI|tYEZg#n&yO3L9gLlqS>JWQXQ^XT{@CkrBGa8MtI@(94`~bj*TnyNHS%1-m53Ui zZoNDgbzfAU6cN8a{*&ne_@9XX`jLc_LHIvd5{3eY;r|QxKSI(vuI#>O#v3&)fBKe} zKU2TFd``pia&70^7n@FT?-TEr$iiHr*&G*tbu-I%v8kh2sMW?wT;kNVOs#2CC-dvb zTK?*MmSnZ5r-{lkt^VLZo)udC+34CXtjP`^bt$_6i7PZ(9kme+E3$a2?YZ9Rd>luk zHB;xYG}myJ2ixT=bcYQdz_vPs+v!bqthaPYbv%ox8KyR5$u(ou16Ot=5%goCyOub25Ynq`)<8fzjFP`J)Bk7!# zy7{ZIT*nip)(%rUlMYr3nYhl9Yk#P=BH3s5xGG8`#ZzfES)@N&6&On1C_&1)1LE3W z($BgG`Kzo~9h-goJi%=%Kq-I}X>+D`l(SNkg&_3^9owaql7r&fQ(Bpb1|vP)QUv{w z0HtmXMF~~vDJ7z1*~L0u^*@n1yGWT&r4N3|%;Mfd9m;yS+X9N}h;X7xgj6jlJdUNi z+bp$fvDe%iTDqkaZ+3rTtu^qu)afc;IjSFf+?`dmhbl+>vO>pWEd;wRqGR<#D} z6)+Ryrvc6F51`i*xqGKQEo{-ErPjcKE_o|#T$H@lm4&(VOo_C zdHwarldKBngvslSmMh_A4W)~ruZE&Z9$D~7O>I_JQEDkMlDH%P4OG)rNtD+!>!-TW zS{=a_)VD>HMA9T2`vmAIST|Z;JJB=QvHD>FdL>&paj!D3KwE&H%>7a)ZVBrZ*jjDF zrUcs5UhHj1(n{Blk}t#=oKR3EMRFRZgchoT)n$U=z1k?SJpNtETB)NxeqIG zXlK$7eH{?DpH2koolEp*i+>LK zSXI4?-);zK#s-9pQd_faAs4asvO;6kF6?CJY&D8SH+rsaq4bn?(b(erMvvWBhSyQQ zDRvLnYWPm*2I{YOLjSI)$a~j&E<|^kqV}~y zwyq&NLdvEH*=43|x)4Fsd=4s_eNh`Al78e>BRgBf?9+Un+ZzJ+uVcP=ylWx4S98ktc%NyE{V&nyPj(~MV#wRVIp?`sL`3(mvcHL z_E(f~9#i23XIpugm;}+(KPfU&i1@p6e{hjIGjq@1X!5=IkG^wgHN+HdcSSSMvPwKw zl`JaB=me)b8PVTcB*b@+)*Rn&7tkf|+Jm6%!`(@_;}#5JuGPz!G@A$M2-ECh)<@>7 zT*JGpaLRp-065g487uGZV^1Ytva{xZdFML$nwyozi{#KO%{eQQE_1E7`_L7o_z_nR zKIzUr1f5(2I&X7j4y|uvY%)7bsA|`=9Lh=dK}_5#RiKnd{t$ER=H$(MGUO<)JY*lJ z5tm%iLOzESO*IwGCt{B8hneIZpS6rAT!}*~`JK*_nM%nhHDXK1bFd?9gh-=$q()WVPU#@Oq098b!zaHI!!gvv(k;VHUN zJNg^GUtK@%8W76f&Mm}H>u|Afj%~@JXvu$!klK-6)#utVB_A4sfI|OxqWhso1GTVvHI+`8@7;+A6fZ2l)(#_7y3}w$p}{ z$oDsgwgZ+cCwdrnn|A7#@2q%Xx9+`>b z?!QCJByoE9H2r8miR;RDkNQ#`-VGC$7MPaCaZ5(nZ5xQNr`=l_?_+-kVVj%jFu8ns zIdxkfu0pSouMFzsJsy;+Y3eRfWKh947xz;e4=PJno*^^s4`a{YxlQe!Ywh2SyrPuN z;pz}E>7s~9PGr7;sOk1uB+H*T=5d2YY`ud-Y>QY$oUeT#I<0zaL!J6>nBg;7ZzbF3 z=3O3JplM1mWK_0$ebVjF?02YZaBRV`s#QB|a6iWH}S zg%_JGcJ&t#K4HlNeJx+s=Z$$1 zb*>yzlHn@#Rb$y#SAOIn`Ix%@B8m*DEzU(qO+6?i>%OOTe>dl2LiE3;=u4nz)XUDd{+y?{YX=Q;wMa*Mw792G z@{FmZA-88F?gu|SCaBbbknYX1H!*-`Z#tT$a`yhXt!IU@cT^TDlc+hqd(ub)@4a^n zIfr)k2R(aV;dLygB^xfra+mOOODLFQDj3NXcpEM@fs5qb?H~BAv)!M&w-Xhw>|Phc zEV<_{-8&2i;NFvIhI==OB8%&1xc8KBFRByCk%AoVk3(ZZ!G2T0dal59Z-TFIuiuZX zJjN%q5%x=J^&o85l^(}BNU7hyC#i|sD7lbo!lkUOJL`7;;D#L9KesTQ76D0!JJuSJ zDIe37zevb@|DJMY?`e}u-eXZ86dQBm$S{)H2b{`jba0jWvVT(+DT$~# zzOPLs1H8)93^|9k@q4|>Yw#8y$s1*)(VsgT<=ezqUQMSn_~FC&X99x=Wgo$pTLifDs5XW zWtGThY+FqLt+Rqy)L=h*y0KpUxc_+HMjER|xR2t&rxDX^05@wLV9dV6d3YxWA0X;x zirxq1i;^R473~}*{rNUoH;Qd@Xz$mfV5(F*;RatJO?>$ZX@i!lT-ri1!8@N)^QvsL zF2eAP$wq318FCJ7*$q84i}3RjOv~;-a-=|&p1YBFRX;wUDDBzw5ou$DkjHK626Q3B z+NPynK+zxC%G&FSH*o=nRz&hu6FQI-=lEWmM6$dOw~~;#1ZH><$Ds}RzJjUo-gZ0o zj_OUb1k}oz$>vJ*rXL{->P^%f-vrZGgdvAF@7MLrjHFM{=FNRCDVH07Q`11qG`rfO-wgW{P>G9^UrnO zBS-4-9yx_3s+@^I+?1Y)CbGm=uW>uRrkzJbk-KO+oQqpzvzn9%>;5ouoqA*=pW)E9 z2nFX-jhOau1$OHOLw1go%@nemOxfjJ)>_dRzT|>^Hef_PTz2jEyVjsiBIPQ`|OB4#`nhIXz3e0;@Q@BXQ{qZW@UF`2t zRgJrg5lC5GA@vDN#${)Xt{`s>?pe;QvY*Vo$^|EIL2I#5s`cCxhqj3n<@lZ*Z$!3> zD9O3jj}6&bQg%K!T19=J$owjE`dkQ!_8r9|G@R=U=RT*;C=})QS6JTjK5e~aIOn-$ z{*u2^IKR7Bnlb$fZ~B)#c+}IT);n#D2&H!&g@Tc$g5F$#-Kepfa8K(m>X|6olLLz02X2}> zxlZlxJW_J_ZLuPJ{F`Rc0g_jaPqj>y{?)?M)aAfUdc%R1Ib!c-)zRK&@ zmteW_TR~S*LAz;V4CZ`+bSTlo{0+2YO{vlu;FxfD+uUQ#q} zGMWRVEywp!iV@8WE-Q8r$Yl?i)T=3I%&i1@CYLEsRo(brT+bNa?Kh z*8E+TKEl`jp_8-o3@6Ch0kZ5?VXKE}tFa;H(4M~}Gk?;`qVoNm|S=8?TijQ6WgSVHvN>hx(upIM#$Jkdv1 zr}rc}{W6u|s9m&fb7Hiu%n)=BqWe~-|9KwtC#bZcNXTC!`VZCV4x*o|PXBl$F+Q!% zC?NXV)#-MkXH}<<2mNlElQ)A{>5D#7x)UqLiPeIL5l%#JA_h4T|2YSu*`UTkeFeQ- zi7p!V21^>{Q*Sc*CoY?}uNO|WtLNG5QEl3el1pnbl8TkKWE2)dHa)4waK;h*>Prfz z;nCzAD;?Fa9{)X?A^3%sxa|KvjLY?1_yVgK)yFdbYu0ykPs_>SENgU_8pJ*w-6-~( z;iQfKJbOOEpdk3fh4(zOA1;mlJM+JPD;Zue7Kax?w6OBA$=~$=;dx2O1R;+_P4i_P zl3Q7xJIh8T|Kw8h>xv2%krHTmSjr~8)Wz?_84B${Tu$_E@|}Kd;hOd|+xyZ8-$@V! zT^keE+NEg=lqkchHD|3-N-X2fuzM*3Rlnk{W7=6dj6X>#@M9ipqfo==S5MPq>%3ul zPTU3?yd$UC+Hrw(-^9Fx?Ca$F=cn1?iIMHkozOMC7h*{Na*Auxp4T?xE*8p@6gGI$G5v1{W4>kh>{>DIhcTwALN zVI5{4*IjFGYFkN>=U8)w>aGQt+Af@A%O(WY-Gq5k+o_X0$Xj3Ons)F)@e)^%vpLp> zLv&4xlMKK5u!gBY?tZ4Ct}G@svcn)lPa{*$6rpFFq35?mLr>8OR+Ji8cM;}Ek2ak^ ze1qD*e@ypiH2)z4_o##d&av*{WuvB!H8r`gkrRXJUc@x1=P9KC~KV(TD*JmA$OM{`>>wY&u+k0Oz|k*GPj@VKcXkX%ZYv%m+dLQq3woH9<4go zVxE_!U{7(^d4JXTBy)%A6FoI!?DUX~31dTEN}f4pdUD3tE+MRVLEI;=UU8{6Xj(>c zM(Wh@Ass?yj7^y~b<7NQd2t|1Ul7UymOR_6Iy+=`YQ}_+p<^dcOG(X`8LSnK18iYQQO1QtTRg6O~=xW|_P2LnAZFCQ2VybMeL`ryVt*;TPCt#Gmo zg#Q5V0T%<^VW1G00lWZQ1?-E*ksUyK_&Wz|fhVZHfoZ^yT8gq77zMlq91Wxg|4d*8 za2@f1B|!SDaUFO9sJPh`r3@z_c#5&$={O451vnZw2AB!l4qONH!w09sz>dIMz{5ay zJa7)hM~RNW4B%j(D?aJJ0&D_Y3)}%b42;2hgL}YBKp#Aj{{if1$H`)Rs{RkqAMaom z0qI@Ld%#tA$#nv_19%VUir0FccuMmJh67`O^lP^hfN8*&fqB5yz(U|=U@7o(U>Wc# z&=Vi+9s$FF?s)BKPsB+#oTLGV0P}!zfrY>VU@34Xunc$_=!vKE2f%QkKi+sJ0y_iK zfX@Q+fHQ!FzQfb?^lLf~RxDR3*W40smkiB}5`f#JYDKiQ!GC#gR{ z0dNH{5BLqR5Lf{$1vb441;8|*C*A?P0SpK32POh*-otz#{oE`M_#Chh_ztiXxC>YY zyaM#Z%UAVh%m+5N<0KI$>A*B#0Wc5v8L$vo3qMRP1-1j00S5y;eV`Z^4y2#JCjzel z(||$0U_P)5un;&5SPGm4ECUt*J@NW-6EGZT--nY#oSXut0j~q|fWHC@f!<|M0Bi~@ z14aTp@giCwjR3%dy?B&UB^O~+lH@x zA6jd`x;&sqS%&|u&Q?_5uBj-|PkAo3^r`DQ*u5q;8~jlNx)A@HgWm&a@MoI*6ClU1 zx+?;E-@{oqNOrF*nIR+*g5r)x_Tk{iSLR1AtU*>ENS+*Q7{VdzQqGiv&!mzCeLVF&j;#YF`f5&G0+`lQZunTRg2NSoj zMva)J+z&F9h|fwL&oaNfhL$d8S!ck~vux@rKQ=k97M;$@Yi4QkHG3zoQF0uf9-CM8 z_C+)9AmlL-{DjJUOCQ5bH}GEpKd}lw*5LmK{H-iBKUh7%dSNVO$rz8Y1sD&r^%#$` z0~q(R8yL?rb!D)6m4&Vhwz!^Sy;n9;`>_TrkY%oPvBaHYi&r*N3)!}n%`NrMvxclb zD+AsCJo{s1Gs~+)X=uqj&w8+Cs>a5$=9WPh*mBm)vi|}XE@dvTa~||Y7ui1$TE?2L z3RX9;=vB?pN=2(}ZXpQcunT^RpXIJ%sVon+S~8SrW#x zEFI$!mXGl;E5dk?ox!++m2-;M8^P*lECS;JmW1&OmX7grmXC24E5f*soxxbb$~oPu zfannz_pv05dssTg-7FvDPF93*2Rnmt8!N|H#JpAqs~@unjN4ce#*Hi;V?N8rxC}AT zcn0J9tQ=z@^LmqJMPMvsNf@`Ybc{P#KE}PQ2xAF5gYhma$5_t1-U?R#WDyucnb(>? z>sEIx6qdd=$OR{DS<>1->ph$eV?}F%TyP(Hk)07IV>mAXq!pMLtiH}7Fs@-q7>}@Y zj9;*PjE68YSUrYm!T95>a*Tg5ueHG{KKrZ^)`BID{foi;5VzxA8GdP z3Gh4r5B@#yyMf=Y@_b7#L%+L+lA?@*0Pb$)I*Tz4v;kuX`0Xq6Q%!yX_~XIFq@wYXXS#@pLx9#)EBHCcyezGw~gD!7_(EB zfu8_=SY`e=lfM=Gw^;fH3Mzj?h`qyW*fDRpSkL!W7Fw+(eU(qG*6sb2Th;>0oBfom zT5TTmRTj9}aDLhCyt<&DQc}m36Rj+8-)F(a$){`^qtW}laQ>Z#ZC|wVuICAo`rFI) zb+odt9x-ygZFi%UQ{J!BjNg213uBZ|{az=jD>l1rO^mYEe=}WO46qf)D68wQq>Ful zw&O9%uYnus;)4dZ?_-p28+=0-j$qs47-eY)7uynQW3kH3(A~t@(%7~I7mfGQ#kQul zL$S*JrrBh5X$#wxSmjuYi8w#p()J)$$!fKp81`LlY&mht!#4lL#o~6hjd99n?J|jT zD%`d&PFWGbgZ{m}?dv#YYlmM*^VUwbyK&0Tow(*Dk+y~L%F)PoYk>2xt8I0>vZovO zb7K$Nws_@M53+f!r|n?8vM%Zk9zZYK#du|DZ(=;Pu#fFtymFw=Gq||lx47GTuef&6 z|G016XeuH$l1PF2n91K2;{((@LAfdse4A1 zV`V=5c_JMWuwRR7+!grY{)#|6((&WrUJk70*7#6G3gL4nKSra==)lJ`evD`7TYKWp zShO`bbQpiG6MutD%u@Uk6dm+?B&-Jij%gDvRKMG zP!3qF!jB?6ozfNpc?5)xy~H+s5}Vv6S;+9)22Nw~C`W(%O1~yb2fyC|S4(TS{cONgVS4g@);w3%5v7};~{lQ#8=JyMnR!DB)L_ycJMIh}oYy1pB=XXn- zu9Eb26ROG|k#sgk&~^P?@xp=hJe9`j-iluH$&#^s9(z4GaOshk9tv`)>aI;$bxJ`la(R6!3FpsbekSJK(p9qsxZ9V_U%lP{rRXxfV} z33@9z?E^{gFPvBOti2 zUQE!e?N0Oyl0Hbrq&ra`i;m>WSm;ih*FD)i_&mptp)7vqvzBh-*jqc>*(XmCM7$W~ z$3iSjnz~r#L09>ur1z3V*8}pynk0GWzYd=->95P;^_Ti@NP5~BPGGUS(gSulxgX>q zbmBRCmo2`dQ|7NpZoP3rrCyN`-0aDd1lfKorNse~jvw~!wlykoO8ixDshJ*h5#rA` zWfONd!()B^?l4QjSaxuCJM|PR-)*bkP1@@x^?r^PA$xmAvqpQ`so%1sJvPgvd2HgI z82fO!0QD}qkDD;bI{$ThJ={2n{)STqMoN0Nlb*LE-T5!t_k*t6f_p4%ubcWc3*Bq8 z2jMLMSEFq9qGx$L1W10V?9QfAeuboWm-#>% z_^}6c%w@$EFH}a|Z?j*JlFt3z96K3lc9$yzbGZ}giJL6x zA4s}hnGZoHPYU*X*`G)m=e5)W54YsQwG3fPU+smG?lVu&(boL1;%-aw(`JclU4E#f zD^BuT1ih7VT&7!3p9(MOOC_R8gd)8=yjjs_HuY&5BbGphjNOupCQOJbdnCQ#L_t_7 z>9_gZ3L5RxvCq+5&$)uB@4uf)dQgg>>)YWIN#8qK(BU^fyh4N?`o#!3uxj|RG(^ys zrHX6ahff9FuDmBF=vAr(56Gc#SyjDCvn74M*e8weNI4VVarQGL4?#Z3v~W< zDam`KM4FS!p~wM+o;6!cX)k9km-IEVMtX=YXcCftMJ_G9M)B;x7dG`pKF*^t{*shj zCKqjz)Z89UlLqH1)8D!heY=c7PE$6rq!OFDpT+@YQs%NOV5HR94T+<<17!jAj05i-)xIJ@YzX!ueH^v2x4796tKf0A5RCvKB4 zp>4jT>k+Mx^hMK!Z|E<6xMHD^S6?|b)L_s(6>QigQcyy&khd$-9r2w`G{>FpZxJEwg}TM#gq{gf zU9zOVDAnn4<5e$a{UEuelG|U-itAA2dwmq#{rQo?BEGZ*ECZKZe=c26pOW0G#8v;H zaX_<r}UEpo<0kP)ZazEuoh?2}^-a^m{Dyh^~6hl2(M!iqgg3j&POQeL@%gwFa`)M)r#Yw{Rl`?`TNq61`-;(rEGBSN5 z{ZZ1(q#lI9k5-Xf{|k!q=g@wFp?u)fd8;LTs8i>imGp1qj;EKtF-v#Y)E+cOy!ea= zW|oxEyKsf1ugVZk>5cI#pH-`inAK*s;Pw-sOp})IM`|U@tUNE%EhHe^p~W!m?z}C zrToC2Tz@O&iBo}3gYj?Q{dJ4oIA*iEL$RM!=(ucwB2>F?Wg9A9vQ$P=W`I_@qA^;W}b z>+jM+>KDA=t#jlX^YFI(3kLp>l&PNUR^2Fv! zhmNHMHy}_?fBe+3b>D(UzWCxo{qvmrD+Mq3sxj)tg7v;?n49s9g$3XEs!!KemJ6lI z@`Cz)YG=258^ra!UBmszf6u*l<~^Q${`1lMy>rgFXP-Os1|H>AUez1C zibDdbdn^1%5}+t0o&M*$JVu-?k107B=R4vaaW>)}7O2Mgo@*yg&$VM|YA;K*H=C#a z5wLubTw)+Au>2Y{^xs#y-1}X}%#(@ZhAGKuk#lXgI&q_Q{Sp{af>?}A= zK31`SrkdJ@uXvP?Bl$RuLD)m z2-ozDh-af08)Xer2XIeDvRa>bwL8~xk@FLfix{pYm(Hz6t%?%BDlM&8v2U>&Otk3r zO%)|Zu_+HdDe^l+75KH6pxanZbItMN96{|Wi%VXDFms|bgV!(1SkQ$8-G{njl zh32wa~m8I_Cm;M$%SVhJAfv`j%?v0&JA+B-qN7+!P>GX>AdiY&VuDbVQS4y2#%_iCwWxNZwqpN;YoQ&C1;83NZWq=y zLJKvjPeNzhxXEl>M0{w9+ge+-I3YiJu^m`zY%4^cBE3Xqln2-qa7Pna9(mPbLfkQ| zb!$x>Q8l9V$Eq4s^-mi>NA^+Fc(p5Q5q(_k&3=wfQSV}1W1dq7vG-!`uZn1U&W}a4 zT~M0PT&=D0OVFFNzt+(HlR`!Dj~uKhzxlVhiSy428lUedsPgIYVH0+)ck8rM3l-)1 z<8l6x3%DPtMg5|2C8QQv!wBxYk!w#av>t*`YJPRDPs17i$UG9vptiY*{XNUR@b~y7RK&k$` zD|tF@2$}dCvu7ck3ZaLg1Z&?!Ojt#fCx}vH9chZDO3?%%n&T4fFGL?OMO#Zzs}LQ! zKyesqTM5}TQ}!CyFf?6nqZ2GK%DvlA^Mra zC0i5~u#4kX_*toh(FyO;JR`(-kK~pLU7fEks+Gq9PEd-h_)f?1Nph0YWy; zlogRU^%FM`alxKKUMtk6;J7|Rq_C-L^$`*(vRcg58#vFYuOLRD{nR`&Eo->wkn+@m zu}*zB&;01})S@BPsYNYL4kocfRO$)s(J{-YKT?ZEfHx$su;<8o6y9An)CS9otoN9Uc5^AG{()F@X)d|VJ9M+*2aD|=5K@UCQVa64DT>1U1m6Bm{X%<| z(LdCYM@|*w_u(O0qfF(4rSdMMs$ee`auEbJk*{#-dt$H?Y^tVW{GfkA`7GE2|IQC0 zg}g#_e|WBq7`I3&bgwG_!?5Q#-(Tt5ew}X^GG$1a1&9u7N>sccbbd% z;9p^T)^C93=!U2lRQu;Q{7YQx{Zd+^4fi;nW;_lAox5tPq-hw#*?zu!*8lFJL3pTr9-01?)~vH5i> z8gd*KaVDrYKrOUUenqyYaKET#)U17FoY$^%rv|y4vYB#Dhdvl=(YYnz zzU51t`fj1Y;?i)KC`Hz7rs#Sp`Wi8KP7No|!Abwk_$OP?i>g;Zo%1SIJ7f=z7bePI zBHO%f(}jtdritOwL=Pc4!zDUUi0(}`9FLZwK0-9jC8~jLj8T+Bdyu#xmcObLyL~8Ts~V zY;jSHu=qHeJJ9af)2QOf+w>UHB@a1pi8>Hq~?O-Y)Im5JhI7 z8RnwpM~6@b%%XoAB}{H+34`s9R=6Zx7o8DZIH>LpykRP)5=@s4v=|`Ok#w9xg6ukib=ee`}7EB?TIejit zQFb;_NTXBFtLL@66-~8tc%kFnQ`v)?H)`2bW@6{BJR*OW>C;4!wX|U_YB`SEwI1fq zlke~G6;8cKC^$6ROvxKuL4T{4nUgtEc7l*y?UK#rvSv7iZA2u0UFDJ3MI^1!tL~AE z;ERk%B4rYfU*(a+x|IJ;6d6e;%ta(K?UX_5V`d~f`3l@5gn|pR%t$tJ1+k`khg%Ex zKDq+M1^KhYZG;2TKa8cP1lw0|89by#fh(@Y7Qa$7YA7pCu{)x16}ShJVLC}!Z7VZ% ziYye`ySRM$izqUfu9%Bp<^=Izp6N%9k%B#Zg;QV4U2Nn=3Z*TbZxy4YJX(65u(bC% zGwNb)$qf3#2odxLmwC`*MbI1&^kz1GnBB3H#x-vhPJWP{cE{@h?wL=;!{LL>t>QL-zVN9 zA=l+o9ZnplzVny=itRuY%N!Qu_{O7R^SBNpxKT3Uenc&_k8|0&!<2LCkHUZm?!!O| zzzojYEjZCLokak9{ygbFY|s1~z7GE-|BA=Avb&6+tGN}YzJ~kVDCQxep2udGF|US@ zleZ&sr|e1^=&$IG#(7v9M$(PCr8P+uPsQCX7ko@Pr```-k&0IWC>7RyeT}fb<|~}~R#MQ|od?LB4BosGr97IqR9HHa zYX)0R6jy<_g%Zc9e|nKLimbbm?`*>hd>FfqdsHuS9Y%rglezX5w#K?_wKwJP=@14) zRPW<$id%s#*rCnt{abYJvOCR)p=~-jxRpEDsFDwe;vz%;lkyZX$rajvdDay%QQRU9 zkrC_v8F6b7u}2+W_X>KIx=#H-HE0jF%Qj*H;nVb7# z!FYa9sVgBpP3P&;LwNe?&`9p-i{dMd^j(u~u07AA|2x}^Xg^Wpjq29VM*4=~wJ6f} zhUsGo_sOZx77Dhy6fEQlTA4nsvJ3ZY=ec`bgnKQhsiGC@7~U#KjOIZkw?L zNOh%L6o2l&7Int1MT^?;Mb=5Z$O46xB)y9LIfd5YDj^YZv-?aWa*WHD`6)Vwch(T&d}NCOs1%alD|nZRFEJ__mho(GxMIW0bt> z(*8QQhyebXZU*orQDn>h`=b%SJ-!rxd8hs#DVLQLdG9PDFz(b=*PZ&q9;BO$X7Y%f zdMcS{9Q4oRU7>xZ%a1`2+C?v6^-kdZ`Bpn#e=`U729nlB1*|+vYH^pQ0N6hxeJlrq z_|?b1qARK`|cZMw^P;@N78sriNlyldLAih4$et6)Fk2ZUYek zWDZY)KQ-7XEBpb{RlwL_-XsYgUH(0#(CqRGX@3e)3+whq!{ktheYaO0Ia@0tJxL;1a zvrsV2rQlAkz+vrX%7#c;MaU+*WLt7s>yoa9+AEyr)Q`e4{0er7{sfBtCz9fM6U-8> zbxt&Nd_arnCRiereEkG*cq!i@imPw7wsgA*{`MW;5(fnE^rfJs+|&0QUuoO~hslUk zVPT}pLZm6@)bIb^NME@XF5rEHnG&6^aOyP>L7V|D1>Y0J?Jo0I3-TT>H@*Y?w!6%; zRAuvqVRpnmxu_c{E6C4mAe|=nea>yPo%jgtQ@EhDdxC?EQk|2fqn)^>Mr%G7!9Os? zOm8~~i4OJUX)rDJgvFioA%r4%`!shdyRlP&(&apEDAM?hZQM7n`?+c8dYVe5&SsTk z?T)$rjVo12p_rHTeChvPL@l&0o@_e%pecuU8E{E#cqj2r1!2d=lN===hKJ|WeT9O1 zTna+Cf-ctA?=WQlgj14US9k8;Ug0Yz8Ou$%`&`yGulN`TI z{^TnV4MZp!mx3!qDRMoiHIwfB@f}MX9OM`XzixlSm*(N^mt2YA?VBURkRFSry8;|g4fpXejp8+n>1{%!ar68{ilNT&-7zQ6C`Z3oi}%G(WTtOEYP zTh(Rk5K-h4fyMClmyuYLV7j`3i#zpsBs;EA)~jLGJ?l16*_Dr(>3E8i@?F8z=?f^L zq9Ntv__d~do=5X;rOo#Qm`J;ZHoVTt4HtbSZjSSg)fLBTwf%{k5@9}NzhK zjxqo7!C}u3`{3>DhmNrUU=JkrTeq|CIL4kHAKW<*>|e4yH2-rH#D_hII;$8T)a3}V z5eZOJ0 ztO1tSA7U9<@#+aSH!G&adk+Z}_(3o7QIQqVDhRK3sI^LPh5h0WD{Uj$4_Rpz|0AsP zgiBskM;aPf;)EcU_q4ZVV=e3TSl{4TUz6XvQwxV|A;;*a=RfE_v6mlvI9&S%timDs z#MI7S?PF1dT&sSB#0$rU&v4!lodP)JjTDDbDuxy8Baena+wg)j#G6{fiYNE! zR(#OVAM4Wp#$m31-B?5aK$rd>*v-ii!5gqlDmqTW^Y_^vPJV=?KNZG?PU+VD%mG8| zV^OAOzfdTJ)>{u4T1)RYdGUwYhf~6XEiSbkJ=Fd&CH~>=rrJ%sMEr;9{e{~5Otn9c zG}X@IJduo_sR`YaF&f%r408_gWR!npxZTI4|3k`9p>_Kh!|jnS{lBtHPel0b!7{1n z0t=klr+Z6N&--|1M*WH0hYCG+n0l%+Oy0PI?8T|!!3i!!qe#9o9)rb}}pcE1jS*>$I7^%j6FfdVaz3r}chh8c)I=v_7_O{?yZ#%odJ5 z<3?J@jgo{@|B!oyYiRU-NYwE74(spTzNmCR3z;6##|;fhz<`Q|hIMG#^S}eZyEY*E zxS}s$yRN~Tq<+{8>wsf`6_9@xSOeTZbl`rV4ePAfQxkyq0*3<^0jD*^?`FM?$zl*{ zfR}+cfqpR14maFfU>a~Ka0_rF@C5K1;G4J$-vXWkMj}VJF~BpxB%tDh&ma>qiNX`XOkfglJunk^0+|&l@WZ3q zm%tuC9XJX&2Ompb04@i<35><(&wW7pSbrHf93QNM@Gv_S*u#OzO-vpEF2)DX1;9PP zcY)3Dn&tp77I+yr2WZ0s&thN#a4Rq!cp8`uyb3G^`r&<71+WdU2G|W)2TTXr@CoQC zU;;1?=t#$;0+Vdu7r$HE(1uS^nZN|#>$J+L#d4tO8XhL?>q zfC<1i=tpJgnEVXP1~$I}1;9jL1@Hx64R95(4!9p^!`rvOt55)p2Brh=2WA6{fyKb} zzzX02U=8pRunyS#SFFb?@7BNspd%iWbW8>Uvw@?4#lXjb6~KkS8sO`|I^a5>4ey~2 z0_kPf55RQbHDET-s}Add?SU1*-oP4Q8n6!d2+*b}%8S4R4cGq?OwvJE1~3jZ%fAj6M(tEbl@UjHtsy(#-J?tj5 z%gO_Kl*Rb}c5Oog?q-TIq^0d;%b*rbNBPl9Jp76~QiuO4|C@ancsxW#DaITmc=b;qXx0sSHrDSK=5VLmqe=L zSkaP5OW<0zen}h4=~{LOaK4sZUD9d5oe7E(9u5n|kZuPx^#8(IZ2p*dZAR}J5v9&= z(&UYH+!r#Jj*m5KYT20LHkS3ZY$o7VEnB}dn5`>rM$=8j9W2YgX5SabWNrVtp`jy| zn5rIi1s{QA?gM|gJKr+MG;=rjb>OEr;-{GWN5GF_StXHb5}S*0Fk6YSFWZf=4?BzT zZl;z-s&OnD<4~52F`Z?VMq1_^Ve?93)SVF3*!I$9mhDH_=cOIgcI--NN6V|#ow@Atb^qbqO`GG|AxK9I;fAZ_gF{E=SSHguo8}OVcSwK*s(9|XnE}z>%X*v zs<25*Bk{$G{G}b$9;|-3<`ac5_Tir0gB@JjR-M4=!5_r}mPKNoh;bszz&M8GU>wCt zFb-vv82hqXjD1)=#$GJowMaFVC1UKuGB75x9E?3#3C8ZM5@R=3i?K7S#~9B7$|7Mu z5o0%&fw42o!5Gg@%RC7_3%(v4|f6M3-G(S^Ph0>hl76#{QtVD=~9{o{tZ@J9&TNX*&lqi4zufg zb^x<$Eax3~gxPPber1Fg?rA@>fO0-L&JxAsOO_!f+gS;u7ei=0U$+jkk6GnATzn;~ z6_cf`9uwT$mT^s(EaaLnnZq?-D7 zymnQTqu&W^9-F+ZYX>RcdRdnZQVOls`NNd0)-uc0ROOU+m;Hm3h$$*l-&%Mhii^|y5L zR-{%lMEO3Fi_MMJ^dZWY=q<#V+g8gPs%&Yyl}_fj*Om`e_O&k{tKWCjDuycW#7@P0 zMJH|lP~}vomBerqb~{ zx*l3}igKj~*SxT|b|OXD-}^dgJJ?sdlA^qN7x(kV-P*iV<xj;qW8r zd4YIl;D`L$74u#A(jYeq>_Lrvsk@_;EkWSvNo}XO-(Bqi@h2fCC?D_^}8- z-KT?|ALz(t_3H*ipO9woTIgw_qILO!$0UB@Mk<>}-W*so2AAR>Z-#PL(N^%029v%AH}3@j}HI0m>odkTe|~&(b!; zscYEm4e{!7_Q8gftjtUy!#6ujQ}CQXU+L>RQ_wpIP?k&j({ls??<)B5i=-z#F3y)o zdI$n0J+UmKN^^9aEy#F2;YWkyo_j)^8`=_)cUtwvGlHHbKv^p36DBv7|6J0Y&k4Gr z|1Nx_B|QUqu9a4bQS@n&@#7qJvubGQi!;PJBg&y@fIzH!i9NV6K5bdH;K~9k2f-z+ ztDbGVstsC!=p&~Kdb9v#xujQQv#&SCrQMn+=!TOM&@r^EG)vI$kjvhebcb+WF_QC> zq#vHc(mshxyfatG8@|7S54)u9%~^tOcyUnDeb@(|#1ES!xrP(;(U(^J=s`aw=`YKe z3@7L}I!L}9i{7LKL`m22iGd#j*sx9aS+Xaw6`SH5<N*{ zLnjH9Mn$4P!S8@9#*|##q zV*XhJ(?HxkiTlY9Bb`>ZgKpS@S{A?6$6>`MOwyeuWsUN8l=KU^ zki=nB@^wk~yg*uEA?f)=ZZ^gyG)L0!mlvSXDW3?sL%Hm6z5fbEv?W}zRdc)MUfdj? zM9IKj=}Z9g?0ZI}#1ekE@ctT3+WyHw|bl^z`tMUn=NvN`p+dk-ke{(AU?Ks)b_XPJIjX zK>9*G9(d>O&=RxJXXIdrTpusQl~s~XzuQ4asQ^6VVwI}X=y({r9IYKZOE8Vy_iaf} zpCIVQW_VuGuZ|aVxWbQ{lK!qN6s{b8vlJ4ix`a9U!YOQCBSg~rnyugeM zXG{88nG@XQ_;CPq(f^%T@@|b@w_wbo(eYd-@GQ79qdM-#i=dNdxgL4{6Z8P=z#-M{ z(s=I0J(T2|dpt9Zm-Hx)NOlSO9m;l(9j0C`@O)b8$rATy){&iMD2eZ3Mh0K-Fc{HA z(DP+~8`r~wtYnX-zQD(oeB4iC>+hxfY7hC)u13;bso<7U*#CF2{W?%mZ#^E z?)g_TddCU*xwA#ejFv0}otGu+h?MB-A<+Um6It3I=|(KGBz;Sc&}dvrze@VQ9_@se zEc-M^8cIysR(sq7R!O?u!}?9oZP-q?>}g!;1}VQsW-L^)@4}T!`g=&PjAmXa=)p>& zES2#f_d6Io0f|1id}--cd3{WgD$^vrRH`%r-9|UT#6yzn^^6cV^d@$1^k+#FO==s? zNBgVsK*jknQ_^cZ3bmirf2Q?3 zE4e>QKaD6J>nW_<=RrRTy0~8PI_7gNG@_S~8!8KDv_U?b`?==$Kyt+f?b@GjNqXs2 z;bW=vwNr1Q&2xX7FX>FiVC;}JlHTq~p#aYDqZKwW zCH<9If?&|MNqP%;!!Xk0$8ww+{)Pv}#FRUQ(`$3ZTBGM?N&0?|);dM2de%#BrrZ$I zq$i{L3T>WO>I%?#;#*friRV0g?Rb~bvi&flHo7`nC^d?*O42=B`6pJgUrTI|T+c1~ z;k$*lp0X(oZOd8xevO^j9}>DB646SC`4>?_WGwg|$9^p|5XmR5@ean|c4Zk~XpTH^ z$=Bi1SEGcNB)vvjMY{Mgy1$S&-r!(c;m5O*K3ZOmhL=A|`Xe5jeD48VPoR?Mk;W}7 z;7iSMLa>!h(}nt0(o9s6F#o;BRi7j2F^>yD!~AMVZ=NaW@P;4XsVEl0{-BRIKPu__ zlIS^6VgLIwziMUY9sr$6lOVUeCDO+Ql78c9K?s%fPe2zf>N7~J+~JYiEYO97LrMfn-A2ADr9@Cw z^nqPLtb>+V6?^uJJ2p4Y zH+1Gd{b|SQFYb&)^fbr(cKqw8uYU2f)f#qfR1z(3+Q??si-#Tp5bkub(yBVbR zDys`pZEAH{vR#c2G#()hbPOt+XUAVgGd`CkJzuuft`1ep%G6*r-pBmYXk~W>tG#`z zi-hLtqOv)`YA>I4tHk-b&7TIVDXN-OriQ2v%PdRTyb$#zbxoNXidA)A38|xl$_9qw O&#ryuUp6OH?eKp)BGm8z diff --git a/c++/example/dynamixel_api/basic_test.cpp b/c++/example/dynamixel_api/basic_test.cpp index 9eaa0ebe0..aa09d7379 100644 --- a/c++/example/dynamixel_api/basic_test.cpp +++ b/c++/example/dynamixel_api/basic_test.cpp @@ -66,7 +66,7 @@ int kbhit(void) int main() { - int baudrate = 3000000; + int baudrate; std::cout << "Dynamixel API Source Test Code" << std::endl; std::cout << "┌─────[Test Process]────┐" << std::endl; std::cout << "│ 1. Ping Test │" << std::endl; @@ -76,6 +76,8 @@ int main() std::cout << "│ 5. LED Test │" << std::endl; std::cout << "│ 6. Reverse Mode Test │" << std::endl; std::cout << "└───────────────────────┘" << std::endl; + std::cout << "Enter the baudrate: "; + std::cin >> baudrate; std::cout << "Baudrate set to: " << baudrate << std::endl; std::cout << "Scanning all motors..." << std::endl; @@ -269,6 +271,7 @@ int main() current_vel = result_int32_t.value(); } std::cout << "\rTarget velocity reached." << current_vel << std::endl; + std::cout << "Rotating 3 seconds" << std::endl; usleep(3000000); target_velocity = -target_velocity; result_void = motor1->setGoalVelocity(target_velocity); @@ -289,6 +292,7 @@ int main() } std::cout << "\rTarget velocity reached." << current_vel << std::endl; + std::cout << "Rotating 3 seconds" <disableTorque(); if (!result_void.isSuccess()) { @@ -313,6 +317,7 @@ int main() target_velocity = motor1->getVelocityLimit().value(); motor1->enableTorque(); motor1->setGoalVelocity(target_velocity); + std::cout << "Rotating 3 seconds with Velocity Limit" << std::endl; std::cout << "Set goal velocity :" << target_velocity << std::endl; usleep(3000000); motor1->setGoalVelocity(0); From 080fb4412832d03d2e0ee5b2f4e5892e6ed7249a Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Thu, 18 Sep 2025 14:52:03 +0900 Subject: [PATCH 19/37] add motor group Signed-off-by: Hyungyu Kim --- .../dynamixel_sdk/dynamixel_api/connector.cpp | 5 + .../dynamixel_api/dynamixel_error.cpp | 10 + c++/src/dynamixel_sdk/dynamixel_api/motor.cpp | 6 +- .../dynamixel_api/motor_group.cpp | 434 ++++++++++++++++++ 4 files changed, 452 insertions(+), 3 deletions(-) create mode 100644 c++/src/dynamixel_sdk/dynamixel_api/motor_group.cpp diff --git a/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp b/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp index c20cfe7ac..026b849d7 100644 --- a/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp +++ b/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp @@ -69,6 +69,11 @@ std::vector> Connector::getAllMotors(int start_id, int en return motors; } +std::unique_ptr Connector::getMotorGroup() +{ + return std::make_unique(this); +} + Result Connector::read1ByteData(uint8_t id, uint16_t address) { uint8_t dxl_error = 0; diff --git a/c++/src/dynamixel_sdk/dynamixel_api/dynamixel_error.cpp b/c++/src/dynamixel_sdk/dynamixel_api/dynamixel_error.cpp index 2cd07b89b..54965b063 100644 --- a/c++/src/dynamixel_sdk/dynamixel_api/dynamixel_error.cpp +++ b/c++/src/dynamixel_sdk/dynamixel_api/dynamixel_error.cpp @@ -55,6 +55,16 @@ std::string getErrorMessage(DxlError error_code) return "[RxPacketError] Writing or Reading is not available to target address!"; case DxlError::API_FUNCTION_NOT_SUPPORTED: return "[APIError] API function is not supported on this model!"; + case DxlError::API_ADD_PARAM_FAIL: + return "[APIError] Failed to add parameter!"; + case DxlError::API_COMMAND_IS_EMPTY: + return "[APIError] No command to execute!"; + case DxlError::API_INVALID_COMMAND_TYPE: + return + "[APIError] Read commands can only be run with executeRead()," + "and Write commands with executeWrite()."; + case DxlError::API_DUPLICATE_ID: + return "[APIError] Duplicate ID found in staged commands."; default: return "Unknown Error"; } } diff --git a/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp b/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp index 6a1de6a43..15156cecd 100644 --- a/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp +++ b/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp @@ -14,8 +14,8 @@ // // Author: Hyungyu Kim -#include "dynamixel_api/motor.hpp" -#include "dynamixel_api/connector.hpp" +#include "dynamixel_sdk/dynamixel_api/motor.hpp" +#include "dynamixel_sdk/dynamixel_api/connector.hpp" namespace dynamixel { @@ -245,7 +245,7 @@ Result Motor::setProfileConfiguration(ProfileConfiguration confi if (config == ProfileConfiguration::TIME_BASED) { drive_mode |= PROFILE_BIT_MASK; - } else if(config == ProfileConfiguration::VELOCITY_BASED) { + } else if (config == ProfileConfiguration::VELOCITY_BASED) { drive_mode &= ~PROFILE_BIT_MASK; } Result result = connector_->write1ByteData(id_, item.address, drive_mode); diff --git a/c++/src/dynamixel_sdk/dynamixel_api/motor_group.cpp b/c++/src/dynamixel_sdk/dynamixel_api/motor_group.cpp new file mode 100644 index 000000000..85a88ff18 --- /dev/null +++ b/c++/src/dynamixel_sdk/dynamixel_api/motor_group.cpp @@ -0,0 +1,434 @@ +// Copyright 2025 ROBOTIS CO., LTD. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Author: Hyungyu Kim + +#include + +#include "dynamixel_api/motor_group.hpp" +#include "dynamixel_api/connector.hpp" + +namespace dynamixel +{ +MotorGroup::MotorProxy::MotorProxy( + uint8_t id, + uint16_t model_number, + Connector * connector, + MotorGroup * motor_group) +: id_(id), + model_number_(model_number), + model_name_(ControlTable::getModelName(model_number)), + motor_group_(motor_group), + connector_(connector), + control_table_(ControlTable::getControlTable(model_number)) +{} + +Result MotorGroup::MotorProxy::stageEnableTorque() +{ + Result item_result = getControlTableItem("Torque Enable"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + StagedCommand cmd( + CommandType::WRITE, + id_, + item_result.value().address, + item_result.value().size, + {1}); + motor_group_->stageCommand(std::move(cmd), CommandType::WRITE); + return {}; +} + +Result MotorGroup::MotorProxy::stageDisableTorque() +{ + Result item_result = getControlTableItem("Torque Enable"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + StagedCommand cmd( + CommandType::WRITE, + id_, + item_result.value().address, + item_result.value().size, + {0}); + motor_group_->stageCommand(std::move(cmd), CommandType::WRITE); + return {}; +} + +Result MotorGroup::MotorProxy::stageSetGoalPosition(uint32_t position) +{ + Result item_result = getControlTableItem("Goal Position"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + std::vector data; + for (size_t i = 0; i < item_result.value().size; ++i) { + data.push_back((position >> (8 * i)) & 0xFF); + } + StagedCommand cmd( + CommandType::WRITE, + id_, + item_result.value().address, + item_result.value().size, + data + ); + motor_group_->stageCommand(std::move(cmd), CommandType::WRITE); + return {}; +} + +Result MotorGroup::MotorProxy::stageSetGoalVelocity(uint32_t velocity) +{ + Result item_result = getControlTableItem("Goal Velocity"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + std::vector data; + for (size_t i = 0; i < item_result.value().size; ++i) { + data.push_back((velocity >> (8 * i)) & 0xFF); + } + StagedCommand cmd( + CommandType::WRITE, + id_, + item_result.value().address, + item_result.value().size, + data + ); + motor_group_->stageCommand(std::move(cmd), CommandType::WRITE); + return {}; +} + +Result MotorGroup::MotorProxy::stageLEDOn() +{ + Result item_result = getControlTableItem("LED"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + StagedCommand cmd( + CommandType::WRITE, + id_, + item_result.value().address, + item_result.value().size, + {1} + ); + motor_group_->stageCommand(std::move(cmd), CommandType::WRITE); + return {}; +} + +Result MotorGroup::MotorProxy::stageLEDOff() +{ + Result item_result = getControlTableItem("LED"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + StagedCommand cmd( + CommandType::WRITE, + id_, + item_result.value().address, + item_result.value().size, + {0} + ); + motor_group_->stageCommand(std::move(cmd), CommandType::WRITE); + return {}; +} + +Result MotorGroup::MotorProxy::stageIsTorqueOn() +{ + Result item_result = getControlTableItem("Torque Enable"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + StagedCommand cmd( + CommandType::READ, + id_, + item_result.value().address, + item_result.value().size, + {} + ); + motor_group_->stageCommand(std::move(cmd), CommandType::READ); + return {}; +} + +Result MotorGroup::MotorProxy::stageIsLEDOn() +{ + Result item_result = getControlTableItem("LED"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + StagedCommand cmd( + CommandType::READ, + id_, + item_result.value().address, + item_result.value().size, + {} + ); + motor_group_->stageCommand(std::move(cmd), CommandType::READ); + return {}; +} + +Result MotorGroup::MotorProxy::stageGetPresentPosition() +{ + Result item_result = getControlTableItem("Present Position"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + StagedCommand cmd( + CommandType::READ, + id_, + item_result.value().address, + item_result.value().size, + {} + ); + motor_group_->stageCommand(std::move(cmd), CommandType::READ); + return {}; +} + +Result MotorGroup::MotorProxy::stageGetPresentVelocity() +{ + Result item_result = getControlTableItem("Present Velocity"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + StagedCommand cmd( + CommandType::READ, + id_, + item_result.value().address, + item_result.value().size, + {} + ); + motor_group_->stageCommand(std::move(cmd), CommandType::READ); + return {}; +} + +Result MotorGroup::MotorProxy::getControlTableItem( + const std::string & name) +{ + auto it = control_table_.find(name); + if (it == control_table_.end()) { + return DxlError::API_FUNCTION_NOT_SUPPORTED; + } + return it->second; +} + +MotorGroup::MotorGroup(Connector * connector) +: connector_(connector), + port_handler_(connector->getPortHandler()), + packet_handler_(connector->getPacketHandler()), + group_bulk_write_(port_handler_, packet_handler_), + group_bulk_read_(port_handler_, packet_handler_) +{} + +MotorGroup::MotorProxy * MotorGroup::addMotor(uint8_t id, const std::string & motor_name) +{ + if (motors_list_.find(motor_name) != motors_list_.end()) { + throw DxlRuntimeError("Motor with name '" + motor_name + "' already exists in the group."); + } + + Result result = connector_->ping(id); + if (!result.isSuccess()) { + throw DxlRuntimeError(getErrorMessage(result.error())); + } + + auto [it, success] = motors_list_.emplace( + motor_name, + std::make_unique(id, result.value(), connector_, this)); + if (!success) { + throw std::runtime_error("Failed to emplace motor '" + motor_name + "'"); + } + return it->second.get(); +} + +Result MotorGroup::executeWrite() +{ + if (staged_write_command_list_.empty()) { + return DxlError::API_COMMAND_IS_EMPTY; + } + const auto & reference_command = staged_write_command_list_.front(); + bool is_sync = true; + std::vector sorted_list = staged_write_command_list_; + std::sort( + sorted_list.begin(), + sorted_list.end(), + [](const StagedCommand & a, const StagedCommand & b) { + return a.id < b.id; + } + ); + + auto it = std::adjacent_find( + sorted_list.begin(), + sorted_list.end(), + [](const StagedCommand & a, const StagedCommand & b) { + return a.id == b.id; + } + ); + + if (it != sorted_list.end()) { + return DxlError::API_DUPLICATE_ID; + } + + for (size_t i = 1; i < staged_write_command_list_.size(); ++i) { + if (staged_write_command_list_[i].command_type != CommandType::WRITE) { + return DxlError::API_INVALID_COMMAND_TYPE; + } + if (staged_write_command_list_[i].address != reference_command.address || + staged_write_command_list_[i].length != reference_command.length) + { + is_sync = false; + break; + } + } + + Result result; + if (is_sync) { + result = executeSyncWrite(reference_command.address, reference_command.length); + } else { + result = executeBulkWrite(); + } + + staged_write_command_list_.clear(); + return result; +} + +Result>, DxlError> MotorGroup::executeRead() +{ + if (staged_read_command_list_.empty()) { + return DxlError::API_COMMAND_IS_EMPTY; + } + const auto & reference_command = staged_read_command_list_.front(); + bool is_sync = true; + for (size_t i = 1; i < staged_read_command_list_.size(); ++i) { + if (staged_read_command_list_[i].command_type != CommandType::READ) { + return DxlError::API_INVALID_COMMAND_TYPE; + } + if (staged_read_command_list_[i].address != reference_command.address || + staged_read_command_list_[i].length != reference_command.length) + { + is_sync = false; + break; + } + } + + Result>, DxlError> result; + if (is_sync) { + result = executeSyncRead(reference_command.address, reference_command.length); + } else { + result = executeBulkRead(); + } + staged_read_command_list_.clear(); + return result; +} + + +Result MotorGroup::executeSyncWrite(uint16_t address, uint16_t length) +{ + GroupSyncWrite group_sync_write(port_handler_, packet_handler_, address, length); + for (auto & command : staged_write_command_list_) { + if (!group_sync_write.addParam(command.id, command.data.data())) { + return DxlError::API_ADD_PARAM_FAIL; + } + } + int dxl_comm_result = group_sync_write.txPacket(); + if (dxl_comm_result != COMM_SUCCESS) { + return static_cast(dxl_comm_result); + } + return {}; +} + +Result MotorGroup::executeBulkWrite() +{ + group_bulk_write_.clearParam(); + + for (auto & command : staged_write_command_list_) { + if (!group_bulk_write_.addParam( + command.id, + command.address, + command.length, + command.data.data())) + { + return DxlError::API_ADD_PARAM_FAIL; + } + } + + int dxl_comm_result = group_bulk_write_.txPacket(); + if (dxl_comm_result != COMM_SUCCESS) { + return static_cast(dxl_comm_result); + } + return {}; +} + +Result>, DxlError> MotorGroup::executeSyncRead( + uint16_t address, + uint16_t length) +{ + GroupSyncRead group_sync_read(port_handler_, packet_handler_, address, length); + for (const auto & command : staged_read_command_list_) { + if (!group_sync_read.addParam(command.id)) { + return DxlError::API_ADD_PARAM_FAIL; + } + } + + int dxl_comm_result = group_sync_read.txRxPacket(); + if (dxl_comm_result != COMM_SUCCESS) { + return static_cast(dxl_comm_result); + } + std::vector> result_list; + for (const auto & command : staged_read_command_list_) { + if (!group_sync_read.isAvailable(command.id, address, length)) { + result_list.push_back(DxlError::API_FAIL_TO_GET_DATA); + continue; + } + int32_t value = group_sync_read.getData(command.id, address, length); + result_list.push_back(value); + } + + return result_list; +} + +Result>, DxlError> MotorGroup::executeBulkRead() +{ + group_bulk_read_.clearParam(); + + for (const auto & command : staged_read_command_list_) { + if (!group_bulk_read_.addParam(command.id, command.address, command.length)) { + return DxlError::API_ADD_PARAM_FAIL; + } + } + + int dxl_comm_result = group_bulk_read_.txRxPacket(); + if (dxl_comm_result != COMM_SUCCESS) { + return static_cast(dxl_comm_result); + } + + std::vector> result_list; + for (const auto & command : staged_read_command_list_) { + if (!group_bulk_read_.isAvailable(command.id, command.address, command.length)) { + result_list.push_back(DxlError::API_FAIL_TO_GET_DATA); + continue; + } + + int32_t value = group_bulk_read_.getData(command.id, command.address, command.length); + result_list.push_back(value); + } + + return result_list; +} + +void MotorGroup::stageCommand(StagedCommand && command, CommandType type) +{ + if (type == CommandType::WRITE) { + staged_write_command_list_.push_back(std::move(command)); + } else if (type == CommandType::READ) { + staged_read_command_list_.push_back(std::move(command)); + } +} +} // namespace dynamixel From 1276415e5538df5d26409bba94d16503ba045007 Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Thu, 18 Sep 2025 14:53:20 +0900 Subject: [PATCH 20/37] add motor group Signed-off-by: Hyungyu Kim --- c++/build/linux64/Makefile | 3 +- .../dynamixel_sdk/dynamixel_api/connector.hpp | 5 + .../dynamixel_api/dynamixel_error.hpp | 14 +- .../dynamixel_sdk/dynamixel_api/motor.hpp | 23 ++-- .../dynamixel_api/motor_group.hpp | 127 ++++++++++++++++++ 5 files changed, 157 insertions(+), 15 deletions(-) create mode 100644 c++/include/dynamixel_sdk/dynamixel_api/motor_group.hpp diff --git a/c++/build/linux64/Makefile b/c++/build/linux64/Makefile index 3f8ccff44..faa272984 100644 --- a/c++/build/linux64/Makefile +++ b/c++/build/linux64/Makefile @@ -65,7 +65,8 @@ SOURCES = src/dynamixel_sdk/group_bulk_read.cpp \ src/dynamixel_sdk/dynamixel_api/connector.cpp \ src/dynamixel_sdk/dynamixel_api/control_table.cpp \ src/dynamixel_sdk/dynamixel_api/motor.cpp \ - src/dynamixel_sdk/dynamixel_api/dynamixel_error.cpp + src/dynamixel_sdk/dynamixel_api/dynamixel_error.cpp \ + src/dynamixel_sdk/dynamixel_api/motor_group.cpp OBJECTS=$(addsuffix .o,$(addprefix $(DIR_OBJS)/,$(basename $(notdir $(SOURCES))))) diff --git a/c++/include/dynamixel_sdk/dynamixel_api/connector.hpp b/c++/include/dynamixel_sdk/dynamixel_api/connector.hpp index 001b491ef..15641cbb9 100644 --- a/c++/include/dynamixel_sdk/dynamixel_api/connector.hpp +++ b/c++/include/dynamixel_sdk/dynamixel_api/connector.hpp @@ -24,6 +24,7 @@ #include "dynamixel_sdk/dynamixel_sdk.h" #include "dynamixel_sdk/dynamixel_api/dynamixel_error.hpp" #include "dynamixel_sdk/dynamixel_api/motor.hpp" +#include "dynamixel_sdk/dynamixel_api/motor_group.hpp" namespace dynamixel @@ -37,6 +38,7 @@ class Connector std::unique_ptr getMotor(uint8_t id); std::vector> getAllMotors(int start_id = 0, int end_id = 252); + std::unique_ptr getMotorGroup(); Result write1ByteData(uint8_t id, uint16_t address, uint8_t value); Result write2ByteData(uint8_t id, uint16_t address, uint16_t value); Result write4ByteData(uint8_t id, uint16_t address, uint32_t value); @@ -49,6 +51,9 @@ class Connector Result ping(uint8_t id); Result factoryReset(uint8_t id, uint8_t option); + PortHandler * getPortHandler() const {return port_handler_.get();} + PacketHandler * getPacketHandler() const {return packet_handler_.get();} + private: std::unique_ptr port_handler_; std::unique_ptr packet_handler_; diff --git a/c++/include/dynamixel_sdk/dynamixel_api/dynamixel_error.hpp b/c++/include/dynamixel_sdk/dynamixel_api/dynamixel_error.hpp index 88382528b..912a3ced1 100644 --- a/c++/include/dynamixel_sdk/dynamixel_api/dynamixel_error.hpp +++ b/c++/include/dynamixel_sdk/dynamixel_api/dynamixel_error.hpp @@ -41,7 +41,12 @@ enum class DxlError SDK_ERRNUM_DATA_LENGTH = 5, // Data length error SDK_ERRNUM_DATA_LIMIT = 6, // Data limit error SDK_ERRNUM_ACCESS = 7, // Access error - API_FUNCTION_NOT_SUPPORTED = 11 // API does not support this function + API_FUNCTION_NOT_SUPPORTED = 11, // API does not support this function + API_ADD_PARAM_FAIL = 21, // Failed to add parameter + API_COMMAND_IS_EMPTY = 22, // No command to execute + API_INVALID_COMMAND_TYPE = 23, // Invalid command type in staged commands + API_DUPLICATE_ID = 24, // Duplicate ID in staged commands + API_FAIL_TO_GET_DATA = 25 // Failed to get data from motor }; template @@ -51,6 +56,7 @@ class Result std::variant result; public: + Result() = default; Result(const T & return_value) : result(return_value) {} @@ -69,7 +75,7 @@ class Result if (!isSuccess()) {throw std::logic_error("Result has no value.");} return std::get(result); } - const T& value() const + const T & value() const { if (!isSuccess()) {throw std::logic_error("Result has no value.");} return std::get(result); @@ -81,7 +87,7 @@ class Result return std::get(result); } - const E& error() const + const E & error() const { if (isSuccess()) {throw std::logic_error("Result has no error.");} return std::get(result); @@ -116,7 +122,7 @@ class Result return std::get(result); } - const E& error() const + const E & error() const { if (!std::holds_alternative(result)) { throw std::logic_error("Result has no error."); diff --git a/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp b/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp index bcb6b0103..8394eeb56 100644 --- a/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp +++ b/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp @@ -33,20 +33,23 @@ class Connector; class Motor { public: - enum class OperatingMode { - POSITION = 3, - VELOCITY = 1, - CURRENT = 0 + enum class OperatingMode + { + POSITION = 3, + VELOCITY = 1, + CURRENT = 0 }; - enum class ProfileConfiguration { - VELOCITY_BASED = 0, - TIME_BASED = 1 + enum class ProfileConfiguration + { + VELOCITY_BASED = 0, + TIME_BASED = 1 }; - enum class Direction { - NORMAL = 0, - REVERSE = 1 + enum class Direction + { + NORMAL = 0, + REVERSE = 1 }; Motor(uint8_t id, uint16_t model_number, Connector * connector); diff --git a/c++/include/dynamixel_sdk/dynamixel_api/motor_group.hpp b/c++/include/dynamixel_sdk/dynamixel_api/motor_group.hpp new file mode 100644 index 000000000..33dffebd2 --- /dev/null +++ b/c++/include/dynamixel_sdk/dynamixel_api/motor_group.hpp @@ -0,0 +1,127 @@ +// Copyright 2025 ROBOTIS CO., LTD. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Author: Hyungyu Kim + +#ifndef DYNAMIXEL_SDK_DYNAMIXEL_API_MOTOR_GROUP_HPP_ +#define DYNAMIXEL_SDK_DYNAMIXEL_API_MOTOR_GROUP_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "dynamixel_sdk/dynamixel_sdk.h" +#include "dynamixel_sdk/dynamixel_api/motor.hpp" +#include "dynamixel_sdk/dynamixel_api/control_table.hpp" +#include "dynamixel_sdk/dynamixel_api/dynamixel_error.hpp" + +namespace dynamixel +{ +class Connector; + +class MotorGroup +{ +public: + enum class CommandType + { + WRITE, + READ + }; + class MotorProxy + { + public: + using OperatingMode = Motor::OperatingMode; + using ProfileConfiguration = Motor::ProfileConfiguration; + using Direction = Motor::Direction; + + MotorProxy(uint8_t id, uint16_t model_number, Connector * connector, MotorGroup * motor_group); + ~MotorProxy() = default; + + Result stageEnableTorque(); + Result stageDisableTorque(); + Result stageSetGoalPosition(uint32_t position); + Result stageSetGoalVelocity(uint32_t velocity); + Result stageLEDOn(); + Result stageLEDOff(); + + Result stageIsTorqueOn(); + Result stageIsLEDOn(); + Result stageGetPresentPosition(); + Result stageGetPresentVelocity(); + + uint8_t getID() const {return id_;} + uint16_t getModelNumber() const {return model_number_;} + std::string getModelName() const {return model_name_;} + + private: + Result getControlTableItem(const std::string & item_name); + uint8_t id_; + uint16_t model_number_; + std::string model_name_; + MotorGroup * motor_group_; + + Connector * connector_; + const std::map & control_table_; + }; + + explicit MotorGroup(Connector * connector); + virtual ~MotorGroup() = default; + + MotorProxy * addMotor(uint8_t id, const std::string & motor_name); + Result executeWrite(); + Result>, DxlError> executeRead(); + +private: + struct StagedCommand + { + StagedCommand( + CommandType _command_type, + uint8_t _id, + uint16_t _address, + uint16_t _length, + const std::vector & _data) + : command_type(_command_type), id(_id), address(_address), length(_length), data(_data) {} + CommandType command_type; + uint8_t id; + uint16_t address; + uint16_t length; + std::vector data; + }; + + Result executeSyncWrite(uint16_t address, uint16_t length); + Result executeBulkWrite(); + Result>, DxlError> executeSyncRead( + uint16_t address, + uint16_t length); + Result>, DxlError> executeBulkRead(); + void stageCommand(StagedCommand && command, CommandType type); + + Connector * connector_; + PortHandler * port_handler_; + PacketHandler * packet_handler_; + + std::unordered_map> motors_list_; + GroupBulkWrite group_bulk_write_; + GroupBulkRead group_bulk_read_; + std::vector staged_write_command_list_; + std::vector staged_read_command_list_; + friend class MotorProxy; +}; + +} // namespace dynamixel + +#endif // DYNAMIXEL_SDK_DYNAMIXEL_API_MOTOR_GROUP_HPP_ From 4ddebcf564f266ab2405506c0f0386f5feb66f68 Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Tue, 23 Sep 2025 15:55:35 +0900 Subject: [PATCH 21/37] remove example Signed-off-by: Hyungyu Kim --- c++/example/dynamixel_api/basic_test | Bin 72096 -> 0 bytes c++/example/dynamixel_api/basic_test.cpp | 367 ----------------------- 2 files changed, 367 deletions(-) delete mode 100755 c++/example/dynamixel_api/basic_test delete mode 100644 c++/example/dynamixel_api/basic_test.cpp diff --git a/c++/example/dynamixel_api/basic_test b/c++/example/dynamixel_api/basic_test deleted file mode 100755 index be2271f3c8b054b35c953d1e34ead5a3f562b42f..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 72096 zcmeHweSB2a@%If71VssI0L7O@K~W)Q^8x`y17TqU0U`;ArMN7c4O!h}!)61)*C?Q* zAxf=QT5XG3YgJTieL+zh#Tsohdde7c_rKfCikBkgMT|JF+3`*@7BQeEHSTMliDK?6X!;NBNsNphtgWd~& ziq8(W^#Botsn$(8!W$%9lyExiW2s$COFD zf_gqtUNh&j!C}KCUO_z{`DX7ulD_xxMCcOAz|YDdtS7mP@~ypB@~_>L2wm(?1yy#^ zk^YLelvmFAwylzK>~I+SML|{GdB{h8+1HER zY0TpCoa&(um#3hLvoph?g`*433|EZ`hw2)eMl}_U9yPikry-J)CsZK4T==8xQ)bLI zP%Z6*V>cQ-nq^waBihkSC;orje#$dbE^R)x_w*-2!N7kX@4uzFAMudhl!tieB6(cp zB0T{AmNM=Tz$1IE>6L4^duH{WkhNm`ierh1=+p2=Aw3;`<@i(AdAON{ze+~T0i27! zAK-5u;|gvVzoOx%hxGV7`;$+`9{b5BO(%TvN&JytyfM7CUrFh}@;Mo|-?KY>|HjGZ z|K+n4Km9teXldOI$8HV2RJ&-lKkMkV{YozFbIZ4VSKR!C=Za|_7MTRzfRRTgVGZM+ z!&2jY2-sBolOZD&z66d-h3^Yzro!I}XQsl39m>7KLC-4?q^ab*>>&SS2S0=z(Rk^GBrpCYJfbTdoHGiIiK2ZmJwFAD{LH-N}Kg@Kn>!%L#KXKr{$Dtl)JJi=i zhx+=!LH|V#a(;02l+t<{BVc%zRLmsj6=J)!~ws-LC*UQ`qw-7KknfFm_uAuIPlMR z;72tYF1wSFp5oxQZyeW}wcn3X4I^btG__m=6UJ}#y=|L4>p<^KbY}z5N{{(y$&7(p8QtaF0p7n z;;Ke_A$)VYOe?z%1p}2^W0OCZ{bRTH7~qLN%ciH{E3KUFs|rSgi$V>tV6<|2NjOp$ ztn@Do2MwQZQEjBo*AVkZV?Li`P0jd&8Dp!K)%j~fO~G*C^hhicbr&`SV^bpj@SI>c z5(veXc`h=1$!udn4P~B5GwVD{5~*WLB6W4a00fn|J+r3GsECd6`2tN%Znrycp}!#% zfL75^-J;S!Ma=CE)cB*mSkxbiHIxQC6@L8V@l=OMB*;Yz{Z+mt{%FWw7xVc8fnYw{5$3;{7cQWX@&x$_foDYNE=8f3+A z1Xfn0_G_xl1@$P-p+ZN)Gb-?{;HZe@jrRFsHPOgYUvf+?OjU@x2$IU9!G>U6Ohub0 zv$AyGH62YASzYa*t5$+iS(Y%zv4r%7pt8u6Zq#;Oy4rRZ!WEuqG!mVT7VKXXoLDG2 z1k|>#Xi`&HkUX;Flx;p&I$;0xa_9Rhe1S;)G9Ps`4W9APU2{KU(DrVI1^h{609koJgXv7;e+)Ur~I;&BX{MT8SY#$bom#Cmbmk9 z-w^Tz!v2N^Uu;=@&=;z!j&Rgk)ig!p?grN|K zNK`5#viQRoa{aNOPYX=nqNXMvI?0Aeoj;7>*taBC2%%cR5KF_a=u&i%#DIZje@9+3 zp2e6hMq_DDxB3Jc{t;ZzMFehg+@^|?KqUgU&}a5UiS@?zR>TUc!;K9!|3|?$mSPVL zDi-w2szu`vRTT}2;XqWyemmNXzr;v@d8Xm}TX*Q{ru#6S`oh7wMX?%>Y{dJm8A2X? zTBxBu(jezNXy<`QBU-$$8cp7e=x&S?LP-kouA%10a-MjytLlpgr&1$qy0M;s)Nq!5<=GK{|BUy`KCCO+bl ze^MWdMt3An=l>LIGuoGS;jjFZWk~fk4(o>mF4Euwrqe$2vcEil)FH-Epi`d3dvC@H zG}CZ#`mYaMj1^-q<4jJ+PP+jsxt_*2Qyw&68p%I86gu31)sq?b(2swLm3MEWoYRfR zhOqO{!Jc-1zDmZjm*z|3o~ zhDSp;FPDbrrYLA5G&~xzdF5*OBTOuwUuyXN8opS=tJo*9SHtW3iRBtz-`AU~;g8bf zFVOJW8ooxut359w*K2sScSiUo4X^gR2;Z#Xk0AtKt2DfLc*poP8h(gk23)J*)w6LT zuha0yYy7{}@F!^a4I2JL4Zl&tyEJ^eh99copV#pEzTQ?1f3n8EO~Vh<@H;g8a19^V z@TX|_P7Oao!|&Dbr)hYD`xA=&(=~h_4S$A)&(iQY8h)^bS9`cbc4_!rjemrOcWd}u z4WFmsi!^+`hA-Ce1sdL~;l(tAY2_Mzw8lSI!>gf@$O|<5SdG6%!>heuBG+s9vo-!E z4L@GPH*5HFH2f+Jf3AjKqv4A+{8|k^NyD$x@E#5STMa*1!*9^=Q#Aa++k+lB=z)VC zIOu_c9ysWMgC02OfrB16=z)VC`2W=dpY}iDOYidCeY~xif4d&rDJ$D!J-W7fmp|KQ zvoN`<@aJuYv8QYJEdc#bG>E^3D0_Ck*VWavMx<$jYtPQVm}%PB+OzX%Gff*>dv-o( zrfDN<&(7P-G;Lt*+4*BLO&eExc3x$sY0`yo|&eN%{@CO zm}%Om+OyMbrfGv}&(5J{nl`5P>>OaGX+vty&I~h68&P|9e)6rdmo}haznP|uC)jVM zX~PNjn`zo;g8gQiHdtZ5nWhaV*l(t3qY3t#Y1&|d{brgrmSDe`rVS<7Z>DJ@3HF<5 z+CYN+W|}sRV85BB4I|iZrcV&*3^RSANPqH;vfm}r@0n@Z5Q6n`zqcf&FHhHhN&cnLbsd7n^C?@PYkinl^f1znP{D9@uZDX=4ZWn`zoWhy7-H zlt>RS)3o7(`Zv?G(S!Q`TG>w^IZ2(T(~y)3k8|`^_|M*uZ`>O&c|^-%Ot+(rzEmtbY+L$pTRPL0{$^F8zdy63|7A;mXiL9iOTT7Izi3PU(UyMFmVVflzR#Av z)0X~)Eq#+M-DXR#w52b%r5kPOT3b43OV78ZXWP=#ZE24!J>Hfsw53Pc(!*`(<8A3| zTl#QYI@6Z^=2~0*+tUBCr9ZT#-?61%)6?#MmA3rlLT}6a-sK;3mREY*?d})6ZR7ug zVY(}OGdTZKozwrs73dT&0caba0o}Z9nO}n79lJM{jj6#SbdRY)->x0~Pi!W>%?!c+ z7b3P5{)xb8-+Eg*y-$C9uJ`G^J-r#vdH?cl>}UvB$pZRx?Wh+0jLJ{_P4jpUx*wzQ zjM?7h<4*#o;cfXa*3a8Iz8Z=61z&Y_#j7wFKbLtt_%h}ruUS6j-?xWgecEmce}T3Wj&a@EzgO4-*_SVc5kc1wcgYuNS+Pemf0J+DtD z7Ee3U&m+AC>9t62MS2_3>u|rL)$`jHPrTLh#PS`VwRoQJwo&$H#M1{^#E~c>J_`S| ztPGQUrGd`#F}mPZNp79Jx5|6Q(L(1~W_$%GFKpgFqZS~~|JbPUDB^K=)p&~Lbi&Y$ zl#Iu|BtvUpXsh_Qc}pL7gC=?!(@ZeMkA;XdE_n;JEkz{f?~nY;pCoT~#ZK_HW?#9| zFxY)9ueWr>XMWMuwUH*HyE27VsJ8^Co(b8OQ!5ZEaWLzl5~onfoPs9^2o>DTM+sg? z4x(ffcpVyvx8-%hG!UjO^K6qfk6Fh_)_}$8m8=s?)pW%+NY>RB z>te}jn5?(KfUH;3OQguFzQ9m7#V+jL+(%`I=X7M4#ljOzY%P(qM1g_YF9 zbi_lSlVMWec%cpM+Zvl6`x?P18XIx-lsQDP*JFZ=?zsPr?JL@2g$RN|Q~TX3Kmz+i zB(ydAovSD*0`?hTHc{i)+4n>s+cF23nU)G0JK|T$Ouw}lka0bWK(#*iUwhtrL|(9wkA?k5LyAlaIm<;Bl2d2Nxq^_b*>^RQ zaw6Ai!U!Q9@%GQ?o(GbS_~BGUn;bk8R^)jIb-$v$|H>{bpdkMkmfx1ypG_Skiz&8> z87_;FPw}b#`mpW8lpQNah`<*G+<6j}oSAD1end#>h(ARd!iR0l-4VZ?;t=xaM>nPq zr^tG4ztS}GPGMT+KvQBBOAHgCHTx!u^%7v>*P#}c?;Ro|@X$>LuAO&aS*evgf=iw% z>UCwYsCNmVPZV}7@615_&PN_W1ojeOTeHaD();HKwWtayQnENqpZ|~0DZ|p~Bf@Yk z9gBMz=SK>KGM7yxGfBYpB1cF3W+|Zk3d?>WAoD7dwTfB8L=YZTI#xr+{*U5+4#+o2 z#}*2tO3sadER-c&Xq8YROE}vsp$C_6sASEzSO-himI+GlHw3LCzWq~Scpr=PU%NMt`!0@KQ~z?GwTc@i0Du{7D31U zk62CZZPHOh$7s%t=olnRD7H!%E=n+4?e3wZR7d=NA>UqIONQxfM^)Dq!0K(MhY+dS z&Z`_kt6@L9^T$g~@7zNe_RhPV$~&)Pk4dYpd8`oCXL5Djl%SAu#V>3MEj9}xZuqwj+L-6mX{{ z;73A$srz{+NZlJC3%WPSj!OVo^k=FJqh+N_qDK$_`Z$pZGf~3RJm!t~-$`sz72o1Y zW&0h#>b9S1+C(ks3wHa1q)}`3KbM-ezf2gmy{BpWSJ-|=wjwiln}o3)@n&ICf|kdV zms_am9LwxbeXcC%DyyJsQIP4QQO8RkMRp4t=SpV?0B4L%2xEnvU<>0CAyUvTZ56$XFl^(AxR*o!AYtQ5B|OH}G8mfd{KydYPdl5GBnLJLX|P|Y3;T&p*3$7Z z9PVF2ykhQksVL7hb>eZ-)D^o@nK~mOl$ADfY@s~YG=a1^og?vmlGvI(#46e_`8whS zph{Cm;^_kV5hc8Zgd+mU1{rT>NPC|7{LTKQ&n(}2X#eZKl@rtXXDJUX5ruTbuMk!w zcwmUE>>C?R4=e*yhlpf2()dI1#QsKncA{5C4k7M@NcvQWwhv|ZB2tpu`tO0&<9!U} z6?IGw>dWE!^go4zzKofgZ6^%J!cn-F4mu2vW037fgvR^83 zcLNC!jbMpT!m|Guwm+WIesq2*><2c%ej(Ddp9At8k_h{)qQ4@HvLE+sKOV{H_LJ~* zjUcoC^*d!Fm|LXWvRD*?`Jb>N!7T%21Y93AJrEOWm>xK5u=Kz;BCkEl|A_doJm977 z5Yh4?u(}7%6C#xdPUawe>mR}cM_WaIK^XSHDBMdA497!CRNJO8%ADIpj*j>}Qovsq zo4Ril0y1wiS(h^Fm6G+h7HgYi{kA|!Eo0V7$$GiPx)7K+H4OVirxlM=q1iKLaeY`u zmXtBpl5v74&h+Ys12HGcpzwveCZ=@lMF_oQ*G7TWUHjjUl*_37KhG82OxV`!$LdYb z{Lh`;ioa_yh+d>GmMrU6rg#@KI^0-7uV^MwG@fMGCzbpHslyh(%o{Ro<5-5&u~ z*L|F+6Y1WMb$^y5wq}15F?D~RFdP;GP2G=>x)&D`9|>p`%67zmA_Y8Q3AjxNFm@?;k~_b;Lz|Ck-thCq$4MTK;Q`puOY{5g8N!f8qeRny{_en`%vS)|q@A@mJqh z=4`=f3ax8Bjq8wXko9ziv`5V^(6#dXVzJq43>5ZZzm!Ty^uQ+R)dEY6V}u%+)6Lx5 z1Pyzt&?T`5WmcFx}yn z>==gMBh_28548&VH!x~C+nJ~2-Ypn9;tvW_6T)o<$!*QvelhWi5MEDAR%2=yK$MR7 zY@vXCCUYgaGs{gX+^z*ycT8JJx;>8 zpZSh(>tTA}E>TEF`~gU{g0dWFRr z04BZ^-n9?^t9uL0?#)#)`m&4xQpPw-#)+ahv-y5>gf{#K-htUk4fkTiYqD!8ignk1 z{;qNvdFBPK->V4Qn*Df<>6u@dd>!$>qCDx^KW7R9N=yU1LgS8jxfHO>5^$jqU>YzS z22gAI$J;{prY=%LZW#dkD+&N}G&D1H9xgNeP8z&GW*R{NnO+gpgf4BFlwKnkGkuo}>j`-aqs5SeQMW&sP5~eNl0h4tJvtA*}OprcgyU2Z`m3tYOWGnC`nmZc0JwItP_EW&w)4X4TN5~hC*hU#l=OYj(8_|pf&p#OUAc^vAp@m!>Cw1&v+B)q_!~= z(U;sd&IeX^)muWOa@Bnt2tir&FN0Ko7$}yTd>!#eQJ!?w13iT9J4X>8xoRZqULXa` zw**WS0?aV)%eqgKkx(w8S3U=TMI{Nk3nH-7Y`ULl>O@6eaI{H1ZdL4~1^(Q;~vYk&;L9N+e224A*i=wG6JK{%~b{;0}oNspG ztAyHE|4RXnSpt431ekRc>??J@i~OAQkmgsW?my66b(c-|OHG|f_vsuSnX>3}tfEgg z`8wj~Z{s@JH4Ay}JA<^K+%F6Ej`$m(CvN#ajZ*ar3X;cKv)^54`erl96rGdRepZrw zZJu@HuTOi6`)0bo7P(t+(r0$uJd)FY#lW6;#@P@5(4ibrx^(nCZ*+C-I>bT>X^ld| zZ6xfUFwCO16KW~4?7RulV#E6gA56wiA^h}Y{F#I=OvVo){PD^7uGfLjOve9%@c%-X z*z|ds@UJD~A0_;g$@pIp{?26lwS;d=#zzR>n2bN4@bi=L=MtXYv}x1xRKkx+#`h9!C;fKo$wzf<2wldVlw^#!atmhznSpANXB1D_?5}{MZoW>w<$A=kn?QF z@r0agLyjP%+lD-fkjL4O-@Xc@6{}`LzYM{z;bn_>53#NzMbii)w7h5=h~fdbjlYdz zysh*1TF0vJYl5tn%fD`E+`IhpuQM9Yy6W=1U0pccmxl)I;|6>UDhA1GeQ0wn^mJje zAUx-7%?t<}=3h{CjV1LdlJN&rr4j!LhKl){#V-%Ufxni^_j=p1m;V5=T0MKcEm>>k zTLsM=#Nt^MMq)v0wW?LJD-EUoUzF zCr$J^U%opRC2!M@?$IgF4o@dl@UBena!+T*bDn=AB*c-;PHzTW>G`+t;~VkoP^cJo z8(*gq?B@gzL8YX!Q-oJXeDBu8D4{C#LY?Ok5ba)^A=TBQ6NRnw=@==<9q}!Hq56Ir z$&D2GbQ)}Xd<^+^`Q_~f9k!LD%30r#yJxq1Gq&O|BgH)(It2!QI8`R3iIZghKCt%$ z)y}z;iLj4b>BSUah%Ss|@<1#Om!2x>k{WGtU9yez9&A#OlC56+JcuALqRdB$$c$5P zuh%rS3&C8@nfiEJwt7!{o(=>j)}RY6gIbB_bTQ_lp3rDS7+od{N~)l0c+u(`;QrLI z(B|8qh9z&LgS>$W^2{cR`h>fw=fHeQWF3s;4odbCNgR;g-2Wz2N1`uHA7FODZfFs0 zN$W!_oygxT zYNaLQqw1%=Oys*QvCZ22HJ^H0;iUc~U-n$Tp@?KOz5CwVK@HVdlQ?#eq!HAx!Mcsy z{OKVi0X0CifcV21^wSjWGeP676Ez}?QRVfKro~g2gJKf=d@{H#%O_Nl9>gHag&v!s zK8B66WT(C8MQUDHAY`62)U@#Gm&wA+OXm`X`t81zCha*vlY#UW)ZTS0HQ3UYXQ}o7 z4ARP4c96MG5#RD>v*u0sp%xV>^vQ=la^1ixy}CuI^lO({?7v?UD#g!NDlMrnY5AH; z)pnJ7TPmHQtE7)TaF1;1(0e`vr1jqdFeKq z8clXjljej zz4Ha-z6CgIz;1J^0`-eU zqJM5_+=`jb^ZM{DW;*mE8EpRb60__9khax(mgo7{A^OiU(2>&YbO0ZnX?dR9E`Di) z%wm95c#f?q680_R3-9jWRBkqXG`Md_$E<}V({w3ZG4lE zo;W|SjXwX8lVvkn5@NfRXW^~dJj$Gy^?@3wIx#7G2~uRTBYp$?hoRh($bEIftiUCD zaH{_nOuEDrfP@K0K5m=8TQd(#6tZM7w9ZOjLY6*8+yIeQ3sn?NBQ7IBs?Af8L>V?} zeIhlL%~B(|zUZXcM1L}8`7>}Nrav8BucY!C)=9l^c3xA}Rxlb^7ldJrq8Vu82_!0K zqrNSuN!~mlCErEbE#I4wR=rOW)nnfskn%^NGjR6HZv@q{+~$~FX)})(-ILj;9Z0R4 z5=Gn8qR&ieHc`RLFW+m3RjRIz7Bj@70`CmZUcK$ugue^n32`XbH!?f5)#aCW8nHp5 z71DaCbvl}`=mUN)bDA0`!J=H6L{lnGyN>4BYx+{ zqEm;H&rP+O{;!G4Cb@^|f%S{J5PtJGBMz1{n&mX`~x0CZL>YoFBW z_%c-;{vI8wtix%VR(Gmk-jA_%-U6!ZQ z$K`cg-Y2L$XHU=BC#{YmPbbIaUOdr|uHNxaOD)<3buRF8cDcoqh3rsaPy(!a4A5<0BMFg-L2JshFI`n%=N#pUbvNvNfAmu?5==DamHq zj~+7r*JviQ|mQ}hIiy0@DjquP^CL+M>$=G(tOKX@JVz(EiEf9nDI+6!IsU&Hv(&!wsOV9vO4 z_~6bs7k@a?6>7k3xG@;U$L9?C3=h+nMk95LT=@7;u*rolh$((4#+anOis_nAUh1ld zG)4nKS7opv<|@I53Jv4^8&|)7W3&GMJV7mwM(8`0^L5JmH?9Hy)h>6As~n#mlww@! z0!2{sa$NG0hOU`2&YU@UvSd@#eB|X%TWa|gwIIi(J^`45TA1U)hXgf(C|czmo#UDn zT!Igb1zq$FMrI=_UGLv`W1=YuHt!ELM|2ijb*nxJc;zp)CRY735Y858-|g%2aS zVi8~}0{*%>@)JIX=BlM{k~ZYziRcu8V`q;9ts&&uumahq^`}=<5a9 zQ&B>s#f_oZ(9>kjl>VG&hHtX}gu?_KA{)bKfN&Qs_SgaPmvRUE&-FboxzU))37t&3 z%Tx?HDJ5P|D{y#;gdw7)SkKKKe*% z`wa$tpV?({%AV6D9RJ;Rn%@rIzW`HXgq2+cSF&=l3sTmix5X!xjS58Fq6DSKp%0JN z!VH^089w_^UusC(=O#iKW}*>DXf=D0roiPJi!w{}WUd;2gR3s$5?|{?btE9g)B-WD zf7^Q8IQn9w`dX>0t`Xx{T_i@+kWiJhuNa>_O!0YZyd`i*&$EVMOY{!#dGQ2fF78L% z+tt+!SOmBhu+IZsUB!T(0ItVQ^m7}!y0!og`5hkT;)FmH2OPKJ=Wfpfb^^W+xB>iK zga~R@5g|-fNuhp0UnK$zhS`B0apY55O6o(HGusfe*<6)@O8i&0Vm;!xnFbgOBKLyMM9Q+UD2OJGp4>%KW4PXP{ z2EZQ!ZUx)`*a`R&U>1IfvKufLFf)$)fTsc015N>40~iF{0C*|jR=^g(PQYISW?@bK zEMP7m{g}KQaKujJ2P_3#14zHe-vD?G;8wu(fSrJ^0A}H*PkRA#0ZXv&SPmEhtOvXv za1G$!05<@30d55xj?LUozG{+zR+8 zU?<>PfLZvxY6hMTBj5(WYXP?c{syoU@L9mD!RTiIa{=+%^{#TjqXFvyM*yw? zJR5KWU^(DczyM$;;8lQG=%;=Hm<#wIU^(Dsz4MwDm2t@|BcmxR zV@SV4`p}*v;c-60XurFw>lEUcbO<60Z61FG_>1EV;12Aa8%JbKK4S3H{)a8?(`=l3 z?6}kOhn)nh$p1sYb$3E1Lhwa$WB6MO{8q}(7nQXdf2Y6F)rAu|hT%CP>zW=Vhxe|8 zLP!#QJ^twE>N$WWz0sm?0eUj%s0xcd-J-3nU!+l0S*nI)$r8?cSyeE}C z0<>#D8>iD2Te7{NuS-*Q4d_1w-L2=ZvGT71eLQ^qnojSrMKDnLzXg2{=(7P$+k2K} z5ChSl2fhaKPuKJNE%|ZKqiN`rE#s2%G7$xb-15bl2wxQoROj^9aZgv*`NI)EkD{@w z`tdIB*>m!6Q_pg2Bov?=uvt7mwDhP4eH`euI=x4gnSTxFt3by&J1hSjnV)o^7$Ev) zz+RitX(&BVPdy%%d@j&l#osVG`b>QHX?#~&x_phYyx=(%Jf@rrE&6eY&vMYqbb5~x z!9YGg8}!RSFHfLPG3h@9eLd*dp|#{gPYN5#e;MdoKqtG*^5$p0Gj=RrqpTKP+@{Kxge`$s`9(&-V4em3Z@fbJA?KLq`4 z(5LG8VgF?i;R5b5(0k)&9yK<4j~`fNtOL(f@T9VF81g+qbqM1~C;6Q&#s} z-m8bGZQ}a?d>g=b1M%IL#CJ87NOh8h`P{Mim4#DzBjK~bpbI&Ro#oJc^XfEmVvy4e zy3o5YNsg&E#Xww@cT5tW(3|Xg7JRc_XT82bkDKTR(>|K7J^?-_`})D(kAp7sdOAst zsaFB`PQpA`_~FhZKC`?9nD4FzUm!y)z#}_O%CFu=NQqwiF)}h z2obQtrm-P7SNwdBY!vO&!(aG!$A8C z=uYL21pOhsAq{;C z=uf37e>dolrYXN4);ya)cN)9s*CP~TPWqLBp04~b=;_K|&E-4EUl00YY0BRM`o=W$ z-Jm~`hE8ies$Zx28wvV-pgYxH8R!qDksk*Afi(2hEZ=GVwjT65kRMAeD}Fpyd~E@p z*5LT%oJBv+qVEQMHR!4O5Ndz@um&aiSUrEem477YcY=PpPS@sGWuR{W-Kl>FgPzVm zt2w`8{eyl#7W1_@1WQ*JGk6fc!HO^j&H4mx2CC z8hRM?&q1G>Ab)}>e>Lbsu@~Z`|9a3*O+()T`Y6zy^xqBo+%)<7VGrQKH1v_6SEZ3( z2Kx1B@`pjcDGhx!=(m9GyLNzgAQQzACs;2 zH4=o`py%oIkVP*8eG%xX)|b?N!l17J-Kl-A2L0+Z^!1=Or=f2F{R+^X`k&pPUkSR? z_}&lGrp0OKBS83zaI+zOPc&6 zL8t!1Nxw1>9!WzFgZ^94Q>_KW{1@~m)8tN^PcZ}xpH2&@IjlW7#|OBp+Z-U81&W@!!wTk zVE_}VjLd6#Wc-^-{#lPh@91GX+yl266fbn4ba`{|--8}F=z)VCIOu`@FFepL9%|s3 z-2!P&GpCht&70)i{g+DkHp78_#c^O^`>I5FYSeiJdp2~b^ar-|r?zwk_BiNbfCaNm zW9cuhewRro=192R`?yRGk-)$bNnBzN4R?Lm4>Z4`3tOn-`s~}T2;FSJa|gOGX3C2T z>dnaom~GK@s079$R*cm)=(#Ih;u)8`75}nWW8;$&iN`nM+QsyTDwo?_!4K3>Pr0XV}be4a0Q|H!y5xxRv1!hMf!zw=6k}p^ITI z!(xWz3>Pr0XV}be4a0Q|H!y5xxRv1!hMf$JJT9N1i(xLqVus}m7ci`6*vxPZ!*vWd zFl=YImEjJCoeYh9E}x-`VJ^dBhUE+wFsx_T%y13EbqqH!Y-hNY;SPqK4DI#)ebar~ z6D3S3DH-P)F?(TSU98dNcIOo2EZkw%|p+~DHw(%6mr4C6?n zUo&B?{{K)ol<jywYYu+yvoZc0iJNczszz2qx5*gzk3_k@94qnSyb^B91zzIxe?!2N zp8mxWtL7aqGQKV*@uzU66ELt7{|R*xF_iIZ8Gjke5qpH7y#xGE%rdfgyd1&&dzt_F zphPI!7xt0zFI*_`r!)U=7(b2usp{+RjK78BM9IlR2bX0GF$TA@00v`$C;c-UB|_Qt zBgWTooG80qWc+Jvm(u6>LsQq+B;ZND+ZJzhPM7fa+wiY5eir*v*>x-?XvE(sW;KSP z@CzBgo7>SqwrdXN1uocYv@?RimEc!C(J6NUKY%FB5{obv*CQ}r{w%J?n^IYTg? zBt65ml2O(Ba>if7^{e#%BjYD<`%?ZNh@IUE*r}M_7=RVw=@1o_Veki zlq*ZhDXWo)8yG*G@o%y})o}YW#<$zz_HE#)ANX{f6hP+&>3UD#<-Pvzhyk+PPOirs zCN2j)l|S!d{xa5Q81o+w=2ZM%AjxkpT_hQ=XZ}XU+vDvi#y`G5GV-#*cm?Vw? zy>qiOu4nvs_8Z(KuE&5^c^M_ZU-MGqUuQY?{^V2)+*EEqZja}(u=^N)Rg*-hvQEZO zLHwuM+Dn-6zhS(p-@AZUd38O%VSal*b`CZyNdESi;W|DU;Dl%F%8@BpkYirb|e6>moYpGyBR4)|+NGah67 z3EYp}!1#|Gygd$da5} z7fD7{?s&!Vo1bCeJ>uvfEg+oaHl}jae9%s6a@wae&DVe$c=U2FbiiNgfZxXQ zS93pe9t+EZ;Z&|YPA*~m{p<(MX50aMDt(@Cz<077`+VbUR1lRrgX2WS?Ujt*c8OF_ zwd+@bPgU-)5mL?>w)(x6@k`kM{n_5X1D{Gx@oA~?^BwR_4*2T`-vg6V#Fu{E2t3(a zY>Sg4Fkz#4$l!LJ8)w&nC;m4YBpz-Q*Am9>w)y9E#;>ZEe5&2`IYY{^uY<~fr}b3v zCdm)Ch${qqD*rsea=f;FIDe#+e;)Urs@!_uQ_1;>1Ae0ee!BzyGvLWT5nH*rqf(c< z9C+%_o7<(GSs4=kOyK3c{_l&xll+j~pE(kLHutNWITL0x%FhEebS1?}r^Loawh)O9c9(p6ri2qNvc2o&Gty_(Dy;Q^SGk?T3jz7lu_iX*r z0Ca%p9gJ|C(yaSi}5& z#$U+tl^u_pC^w^ZSOe__zQt2dzY}BV_47Y8GmC; zB2;|tagftz9NV>2@=s+s7c<`8FFgo+s(N{z`Ae2b0gC6Gvr_X{0#Euseu?B){nBHM zx3|+Tfp@WPIv991`+@cE&%`L>Gq`;z`40fEbkpTL=YZc1JdJBLn>9~zZuW$zb^A91O~ zA0>e?<6J5KnPWspI502mEN@Q`t2Oc&e9T z)?ev)JLB#4{?kED=0qvSz7JUoJo&l(8EJ!>KP+eduX%vFo=x1q_-E@S;uyxi2Yf31 z3rl3VEw=X927D_1dzt@TE?3$6jsrj5A{C9fb0UV%=U*7|#r%s5AR1zg)zvuxqbe8; zF2ZY|f>B?r))xpz>Vgg6tcv&+g(D07VP91YZ=Le_8=H(kq_#dB#LJd)a#M5oszY@l zpFbM)FZ1C&I?-iDb<|%Q^i?(1)-FR9E#(7eEP++NHiTa6loP10H+(;+h?N!je4z+& zA~|EM&$pmhFExPuqha5jD;d~ zc}v`7`3=FvX{h!7s55mesJ_O!v=RzUdYOA{>5Pg>-{cASr{aPcCB9jn36r2$0%79p zvT2G?0Am{Hy*2^iln`ExatTtAI-kF;3T8IMqIks>rE^0e!#AaD=EMnQ$kMpbSL*Zq zpb)Py3e+rtdneDDFx}(x%$Vf!AvI;jY@f%=NbjUshHugZGbT(gEipyl&1w~%N?+xK ziDe$-_`!_SZ(=ENd&;K;Bpnq}6^!{qVN?dcRjv@#5{#9W)#iF;Rm8?HwsdM}#@H(L z=D4CsO<_+o8i{)7niQB>nX3wP`zjj|RlX2nO9+`-u4OC~?w`!Rd{_}ELd9M z_CeGbyk4dz=&$n3npROLL|TQ8manHNjm@a=X$lmIm)CgUFhp3sQhmQloi&YIl^+v| zL!DX#d+FRtAM`8~$kM5k>R}=jEQl?u50Z9cNjvxl@7G)Ek5-k=f`}B}bQk*O_@g0z z6{4ZB4watHkuxiOB*r~D8mx^h3HqYJYP9CM0K7BC$3;`UO-pu4DA6fnLk$&;0eW3u zl4D|8b!cw%h?2*0b^H8b^dNNz+a*EvgcS)kUsQ}16_rA%CEuY`H=O67yvMW8zSCp6 z#z9=6cz2?3okJs-=c^C<189x(E=ZpYYPd?Y*20Sy)`VizZe~>YO9LJBiZzs?skjTPLe(6uKG8@j@_qDw zEN^ra9bcr@=NAF#vod4k61`N4c4PKJuzp`^Oz(xPfyF(>=c^O%;xbDuEr$WQXo`y) zgTDG$v{Va$G`&+k8h=G>^paozW|t!5;WGS-9s%7cx`Q#Y{{yl+(na4l$t^;vat=hK zdVv;oHwDx$l)@w3Fd>h$ote6SK?Ta5ZDvK0w4^&FkQs2t%!&fpIf~v`^RLz2Osx_9 z{Va5NdhK%0BBKJK=fzOq!$cy8k;I*9)X;=;<$1~$y7h5H=n|~*$Nb+NYqk;3sdvz0 zZJMtBMVdO6tsYNap81+q-$G2KngYRkngDR$T;!WXlY+>yZpNml*d>^q$RJGWk_yo! z(QEb8i|afWiKcD!zPb&LMyKgu3sv-xi&3E|+OxaBHydw0#cLR8Iuxuby-3v4XpHyh z5oKQrUv?v`Dd;HB*qLc!t|B(JHc}U%xmobL!cQB`WMI*>taOr8G|9uGK~&xqZv`a2 z(wUeBR-p@SYzS6yNTXY$u7^4vIYZJSCQa`n8djcaSV?&b@p`|RGMcD)=lSM{Zc4VC zZaI}gih4NItjCf^b2{#&W^})@>bbg8#);YQ+uEYjtVM5&2xA{DC){p#p8nFpQW^{0 zff`KvdAtgcaRFhP_Bul`zImu2716Mj%TqgZ=nzXSZI(z5Z2772t^w_vqH9}ilErvA%#h7I>^fl+Vm|IAqIWKK4iC-90iycdPJ|t z`(R?)?c7AuEcEtqy%4jl7bcybgYB^#Y*sck`RehFhX_qwV#|C>u$hgOa;h%mqNz^R z=-7p%7NoBvrebeK>p%2$UmB^Y7hk$u2bt+My09N1SMj2q?RSH6=c60ITG59Y+M-yE zr#88D!5xSl*%yT)i?FFFc3;s6>AY$-84Ws(jr3rF8>_Xi=rqT-?ZzGG<;_^R2K80e zW4sX~I`*~2wwTszZOex|JbsupEg(BR?MViW^PvVGy%!zNFhcZIkWy@#c@~z_RM8g> zEez1JfoRM}GaczO>;vZ0W*be!^gg`1^GI!-Xg*=7!{!~tAIN<#5273UyQyY2az5s% zm?DQSE7oCRs=a~aJ#wc|*ynxT)a|xrs?+vHnx-ncHs`J*P1m;nW147!6oYt9hh;>{ z0j`^?18b6=vJ!aV?KJ*5`EB3EKa9z#>V33)Hc}Tun}|gHi?Gdy)sSN+U4(`zJ~W`_ zDJf*K8MIxPu(Rm27SZ<)_D@SEk0ffDIu29z?U+G^9sGRstJpmM7EGz(kNy*reg zZhLA1jYz(l5FYERowtPLlYC}?CxT{IpQ=MX(0zXu2UK!();Lus4R7Y-lkSW}`kq#( z$y1APGN1RN(@3$JLff-Pr+msRcIWozaj$&Zdmt^y$Nq+Bl;$&?6b9_W(;BD!9{WRb zvsoR`dQeY6(Q_WKx-DqH^8t2^7Nz-Ee3EqBg@rj2#`w$RfJw{W>(dJ}U?RwLOes^0Q_R4ds zf@;SLHjnm|!$R2#yD`CYT8QoQ1dpCscgq^;;Kj3UbRbpX@+Gy}rd`fhF%OS~8$9Kc zXH`Thdv1MeVfCNI#PA-$c== zjKql|?OW|{6`8ZM6#HQ2*oe;4X}f1SUXkV z+hq02eDtUfEjWNs#53^}vpFjUWv4%y9-vlNi)km8=Qw~EsJYlzjdOA4)U&{d1rV(%X z?8V(d%37Wp`>xXv@UvUn?z8W_CwA*RkIIJ=>$}DD6q}qhF%&WjtHX^AHK~^AsV2-$PjL73G*QU*bU-`-$P*`-ah^Dg=L6y_ za*8!ps>gx6N}Psgz4T}?aqdB_YAWUuc+5yW35@;jPS>GHJ4N!oHEA(pFpp>NLwB2Q zQX_VGdEsp>xXLw~M7)vRy6 zi{tBBZz=n$R7Vg+u;>p^&6$^Y)@*I*Y0~r&w)>MX9QVVFgeE(QGZG5OZjOuAa>03! zg2E|JBRkmV9L(ku(#D2xFj#LhLU*H6+;!h~qL-?yfTL18E2dydHDtJHZvbmK@gs%? z5B+)r0|7k|GtV^Bd5}esD2{TbJ1adViJzX>il_Zr!8}Ihh8pB1u6D#vAB|~6nfUxo z&NQG|ienVcEgO1*JIfi||aC22`Iz*?W?q! zo(W>nCgqn~2G zdOWk7JI8{=l|ncA1X^auCoQFw*jq4v;ERnP`gw09t+J^t|4+8#Iw3OYz)KL2E zgV}XU@$)ku{d%)DSd6zAUQ*OvUKPORCKbtZd^#X*X{?Y6YXfW)j418A~ju|=P znOF{Pa~4IAq??9dz{tUAE+oa_-<)WK4we}?!5V&#NKF-_sT7I{;Bc|{nFxM&M>8BD zRlZ_G%24+thtz;bp|Za=gtDY`tbvUjyy%2p>A>%8`MzKX1Z3c^2MeP2%+sadaWZNC zOT7AZG7^X2PvtM-{0idrZ3$POgcQD~inN};hVv_^-b1Fc^-?zsOKthp_Z}2<+4$}G z&j-nlM}jVf z>U%a4DP^d8g7jJ^`c{m}uf7kX;6`Rv`YS#KuK=CCJ)_d<`!fn|;QXXN>8bQ*w9$qe zGFjzU-@{Q*eGf;KuktJVZ|3|HS)ls9j)LlYJe2=f{Mqy01|0cS$v59`$$>}ltzY)~ zzZFD1zl-xJxP#+S$;jdx1@E%uH}IZ2aWPbUjK!Ov{X zrSvaumz2Srf2|^LhGK>a8IvSb|Eh9Tn%-TPD*sS^5!_f?23ykje}DgiCIA2c diff --git a/c++/example/dynamixel_api/basic_test.cpp b/c++/example/dynamixel_api/basic_test.cpp deleted file mode 100644 index aa09d7379..000000000 --- a/c++/example/dynamixel_api/basic_test.cpp +++ /dev/null @@ -1,367 +0,0 @@ -// Copyright 2025 ROBOTIS CO., LTD. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Author: Hyungyu Kim - -#include -#include -#include -#include - -#include -#include - -#include "dynamixel_sdk/dynamixel_api.hpp" - - -int getch() -{ - struct termios oldt, newt; - int ch; - tcgetattr(STDIN_FILENO, &oldt); - newt = oldt; - newt.c_lflag &= ~(ICANON | ECHO); - tcsetattr(STDIN_FILENO, TCSANOW, &newt); - ch = getchar(); - tcsetattr(STDIN_FILENO, TCSANOW, &oldt); - return ch; -} - -int kbhit(void) -{ - struct termios oldt, newt; - int ch; - int oldf; - - tcgetattr(STDIN_FILENO, &oldt); - newt = oldt; - newt.c_lflag &= ~(ICANON | ECHO); - tcsetattr(STDIN_FILENO, TCSANOW, &newt); - oldf = fcntl(STDIN_FILENO, F_GETFL, 0); - fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK); - - ch = getchar(); - - tcsetattr(STDIN_FILENO, TCSANOW, &oldt); - fcntl(STDIN_FILENO, F_SETFL, oldf); - - if (ch != EOF) { - ungetc(ch, stdin); - return 1; - } - - return 0; -} - -int main() -{ - int baudrate; - std::cout << "Dynamixel API Source Test Code" << std::endl; - std::cout << "┌─────[Test Process]────┐" << std::endl; - std::cout << "│ 1. Ping Test │" << std::endl; - std::cout << "│ 2. Torque ON/OFF Test │" << std::endl; - std::cout << "│ 3. Position Test │" << std::endl; - std::cout << "│ 4. Velocity Test │" << std::endl; - std::cout << "│ 5. LED Test │" << std::endl; - std::cout << "│ 6. Reverse Mode Test │" << std::endl; - std::cout << "└───────────────────────┘" << std::endl; - std::cout << "Enter the baudrate: "; - std::cin >> baudrate; - std::cout << "Baudrate set to: " << baudrate << std::endl; - std::cout << "Scanning all motors..." << std::endl; - - dynamixel::Connector connector("/dev/ttyUSB0", 2.0, baudrate); - std::vector> motors = connector.getAllMotors(); - - if (motors.size() == 0) { - std::cerr << "No motors found!" << std::endl; - return 1; - } - std::cout << "┌───────────[Motor List]──────────┐" << std::endl; - for (auto & motor : motors) { - std::cout << "│ ID: " << static_cast(motor->getID()) - << ", Model: " << motor->getModelName() << " │" << std::endl; - } - std::cout << "└─────────────────────────────────┘" << std::endl; - std::unique_ptr motor1 = std::move(motors[0]); - std::cout << "The test is conducted only on the motor with ID " - << static_cast(motor1->getID()) << std::endl; - - std::cout << "Press any key to continue! (or press ESC to quit!)" << std::endl; - if (getch() == 0x1B) { - return 0; - } - auto result_uint16_t = motor1->ping(); - if (!result_uint16_t.isSuccess()) { - std::cerr << dynamixel::getErrorMessage(result_uint16_t.error()) << std::endl; - return 1; - } - std::cout << "┌──────[Ping Test]─────┐" << std::endl; - std::cout << "│Ping result: " << result_uint16_t.value() << " │" << std::endl; - std::cout << "└──────────────────────┘" << std::endl; - - std::cout << "Press any key to continue! (or press ESC to quit!)" << std::endl; - if (getch() == 0x1B) { - return 0; - } - - auto result_void = motor1->enableTorque(); - if (!result_void.isSuccess()) { - std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; - return 1; - } - - auto result_uint8_t = motor1->isTorqueOn(); - if (!result_uint8_t.isSuccess()) { - std::cerr << dynamixel::getErrorMessage(result_uint8_t.error()) << std::endl; - return 1; - } - std::cout << "┌──[Torque ON/OFF Test]──┐" << std::endl; - std::cout << "│Torque ON result: " - << static_cast(result_uint8_t.value()) << " │" << std::endl; - - result_void = motor1->disableTorque(); - if (!result_void.isSuccess()) { - std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; - return 1; - } - - result_uint8_t = motor1->isTorqueOn(); - if (!result_uint8_t.isSuccess()) { - std::cerr << dynamixel::getErrorMessage(result_uint8_t.error()) << std::endl; - return 1; - } - - std::cout << "│Torque OFF result: " - << static_cast(result_uint8_t.value()) << " │" << std::endl; - std::cout << "└────────────────────────┘" << std::endl; - - std::cout << "Press any key to continue! (or press ESC to quit!)" << std::endl; - if (getch() == 0x1B) { - return 0; - } - result_void = motor1->setOperatingMode(dynamixel::Motor::OperatingMode::POSITION); - if (!result_void.isSuccess()) { - std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; - return 1; - } - std::cout << "───────────[Position Control Test]─────────────" << std::endl; - std::cout << "Position Control Mode set." << std::endl; - - - result_void = motor1->enableTorque(); - if (!result_void.isSuccess()) { - std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; - return 1; - } else { - std::cout << "Torque ON." << std::endl; - } - - int32_t current_pos; - int target_position = motor1->getMaxPositionLimit().value(); - - result_void = motor1->setGoalPosition(target_position); - if (!result_void.isSuccess()) { - std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; - return 1; - } - std::cout << "Target Position:" << target_position << "." << std::endl; - - auto result_int32_t = motor1->getPresentPosition(); - if (!result_int32_t.isSuccess()) { - std::cerr << dynamixel::getErrorMessage(result_int32_t.error()) << std::endl; - return 1; - } - current_pos = result_int32_t.value(); - - while (abs(target_position - current_pos) > 10) { - std::cout << "\rCurrent position: " << current_pos << std::flush; - result_int32_t = motor1->getPresentPosition(); - if (!result_int32_t.isSuccess()) { - std::cerr << dynamixel::getErrorMessage(result_int32_t.error()) << std::endl; - return 1; - } - current_pos = result_int32_t.value(); - } - std::cout << "\rTarget position reached: " << current_pos << std::endl; - - target_position = 0; - usleep(1000000); - - result_void = motor1->setGoalPosition(target_position); - if (!result_void.isSuccess()) { - std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; - return 1; - } else { - std::cout << "Target Position: " << target_position << "." << std::endl; - } - - result_int32_t = motor1->getPresentPosition(); - if (!result_int32_t.isSuccess()) { - std::cerr << dynamixel::getErrorMessage(result_int32_t.error()) << std::endl; - return 1; - } else { - current_pos = result_int32_t.value(); - } - - while (abs(target_position - current_pos) > 5) { - std::cout << "\rCurrent position: " << current_pos << " " << std::flush; - result_int32_t = motor1->getPresentPosition(); - if (!result_int32_t.isSuccess()) { - std::cerr << dynamixel::getErrorMessage(result_int32_t.error()) << std::endl; - return 1; - } - current_pos = result_int32_t.value(); - } - std::cout << "\rTarget position reached: " << current_pos << std::endl; - - result_void = motor1->disableTorque(); - if (!result_void.isSuccess()) { - std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; - return 1; - } - std::cout << "Torque OFF." << std::endl; - std::cout << "───────────────────────────────────────────────" << std::endl; - std::cout << "Press any key to continue! (or press ESC to quit!)" << std::endl; - if (getch() == 0x1B) { - return 0; - } - - std::cout << "───────────[Velocity Control Test]─────────────" << std::endl; - result_void = motor1->setOperatingMode(dynamixel::Motor::OperatingMode::VELOCITY); - if (!result_void.isSuccess()) { - std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; - return 1; - } - std::cout << "Velocity Control Mode set." << std::endl; - - result_void = motor1->enableTorque(); - if (!result_void.isSuccess()) { - std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; - return 1; - } - std::cout << "Torque ON." << std::endl; - - int32_t current_vel = motor1->getPresentVelocity().value(); - int target_velocity = motor1->getVelocityLimit().value(); - std::cout << "Target Velocity: " << target_velocity << std::endl; - result_void = motor1->setGoalVelocity(target_velocity); - if (!result_void.isSuccess()) { - std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; - return 1; - } - while (abs(target_velocity - current_vel) > 5) { - std::cout << "\rCurrent velocity: " << current_vel << std::flush; - result_int32_t = motor1->getPresentVelocity(); - if (!result_int32_t.isSuccess()) { - std::cerr << dynamixel::getErrorMessage(result_int32_t.error()) << std::endl; - return 1; - } - current_vel = result_int32_t.value(); - } - std::cout << "\rTarget velocity reached." << current_vel << std::endl; - std::cout << "Rotating 3 seconds" << std::endl; - usleep(3000000); - target_velocity = -target_velocity; - result_void = motor1->setGoalVelocity(target_velocity); - if (!result_void.isSuccess()) { - std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; - return 1; - } - std::cout << "Goal velocity set to " << target_velocity << "." << std::endl; - - while (abs(target_velocity - current_vel) > 5) { - std::cout << "\rCurrent velocity: " << current_vel << std::flush; - result_int32_t = motor1->getPresentVelocity(); - if (!result_int32_t.isSuccess()) { - std::cerr << dynamixel::getErrorMessage(result_int32_t.error()) << std::endl; - return 1; - } - current_vel = result_int32_t.value(); - } - - std::cout << "\rTarget velocity reached." << current_vel << std::endl; - std::cout << "Rotating 3 seconds" <disableTorque(); - if (!result_void.isSuccess()) { - std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; - return 1; - } - std::cout << "Torque OFF." << std::endl; - std::cout << "───────────────────────────────────────────────" << std::endl; - std::cout << "Press any key to continue! (or press ESC to quit!)" << std::endl; - if (getch() == 0x1B) { - return 0; - } - - std::cout << "───────────[Direction Test]─────────────" << std::endl; - result_void = motor1->setDirection(dynamixel::Motor::Direction::REVERSE); - if (!result_void.isSuccess()) { - std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; - return 1; - } - std::cout << "Reverse Direction set." << std::endl; - - target_velocity = motor1->getVelocityLimit().value(); - motor1->enableTorque(); - motor1->setGoalVelocity(target_velocity); - std::cout << "Rotating 3 seconds with Velocity Limit" << std::endl; - std::cout << "Set goal velocity :" << target_velocity << std::endl; - usleep(3000000); - motor1->setGoalVelocity(0); - motor1->disableTorque(); - result_void = motor1->setDirection(dynamixel::Motor::Direction::NORMAL); - if (!result_void.isSuccess()) { - std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; - return 1; - } - std::cout << "Normal Direction set." << std::endl; - - std::cout << "Press any key to continue! (or press ESC to quit!)" << std::endl; - if (getch() == 0x1B) { - return 0; - } - std::cout << "───────────[LED Test]─────────────" << std::endl; - result_void = motor1->LEDOn(); - if (!result_void.isSuccess()) { - std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; - return 1; - } - std::cout << "LED ON." << std::endl; - - result_uint8_t = motor1->isLEDOn(); - if (!result_uint8_t.isSuccess()) { - std::cerr << dynamixel::getErrorMessage(result_uint8_t.error()) << std::endl; - return 1; - } - std::cout << "LED ON result: " << static_cast(result_uint8_t.value()) << std::endl; - sleep(2); - result_void = motor1->LEDOff(); - if (!result_void.isSuccess()) { - std::cerr << dynamixel::getErrorMessage(result_void.error()) << std::endl; - return 1; - } else { - std::cout << "LED OFF." << std::endl; - } - - result_uint8_t = motor1->isLEDOn(); - if (!result_uint8_t.isSuccess()) { - std::cerr << dynamixel::getErrorMessage(result_uint8_t.error()) << std::endl; - return 1; - } - std::cout << "LED OFF result: " << static_cast(result_uint8_t.value()) << std::endl; - - return 0; -} From 7fa6d5377616ec52cd7d05db2e030e2d09b941aa Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Mon, 29 Sep 2025 11:43:40 +0900 Subject: [PATCH 22/37] add pid gain function Signed-off-by: Hyungyu Kim --- c++/include/dynamixel_sdk/dynamixel_api.hpp | 1 + .../dynamixel_sdk/dynamixel_api/connector.hpp | 5 +- .../dynamixel_sdk/dynamixel_api/motor.hpp | 10 ++ .../dynamixel_sdk/dynamixel_api/connector.cpp | 15 ++- c++/src/dynamixel_sdk/dynamixel_api/motor.cpp | 116 +++++++++++++++++- 5 files changed, 140 insertions(+), 7 deletions(-) diff --git a/c++/include/dynamixel_sdk/dynamixel_api.hpp b/c++/include/dynamixel_sdk/dynamixel_api.hpp index dac908d3c..b0bf4e800 100644 --- a/c++/include/dynamixel_sdk/dynamixel_api.hpp +++ b/c++/include/dynamixel_sdk/dynamixel_api.hpp @@ -21,5 +21,6 @@ #include "dynamixel_sdk/dynamixel_api/connector.hpp" #include "dynamixel_sdk/dynamixel_api/dynamixel_error.hpp" #include "dynamixel_sdk/dynamixel_api/motor.hpp" +#include "dynamixel_sdk/dynamixel_api/motor_group.hpp" #endif /* DYNAMIXEL_SDK_DYNAMIXEL_API_HPP_ */ diff --git a/c++/include/dynamixel_sdk/dynamixel_api/connector.hpp b/c++/include/dynamixel_sdk/dynamixel_api/connector.hpp index 15641cbb9..a02620273 100644 --- a/c++/include/dynamixel_sdk/dynamixel_api/connector.hpp +++ b/c++/include/dynamixel_sdk/dynamixel_api/connector.hpp @@ -39,6 +39,7 @@ class Connector std::unique_ptr getMotor(uint8_t id); std::vector> getAllMotors(int start_id = 0, int end_id = 252); std::unique_ptr getMotorGroup(); + Result write1ByteData(uint8_t id, uint16_t address, uint8_t value); Result write2ByteData(uint8_t id, uint16_t address, uint16_t value); Result write4ByteData(uint8_t id, uint16_t address, uint32_t value); @@ -52,11 +53,11 @@ class Connector Result factoryReset(uint8_t id, uint8_t option); PortHandler * getPortHandler() const {return port_handler_.get();} - PacketHandler * getPacketHandler() const {return packet_handler_.get();} + PacketHandler * getPacketHandler() const {return packet_handler_;} private: std::unique_ptr port_handler_; - std::unique_ptr packet_handler_; + PacketHandler * packet_handler_; }; } // namespace dynamixel #endif /* DYNAMIXEL_SDK_DYNAMIXEL_API_CONNECTOR_HPP_ */ diff --git a/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp b/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp index 8394eeb56..455709a20 100644 --- a/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp +++ b/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp @@ -76,6 +76,16 @@ class Motor Result setOperatingMode(OperatingMode mode); Result setProfileConfiguration(ProfileConfiguration config); Result setDirection(Direction direction); + //New + Result setPositionPGain(uint16_t p_gain); + Result setPositionIGain(uint16_t i_gain); + Result setPositionDGain(uint16_t d_gain); + Result setVelocityPGain(uint16_t p_gain); + Result setVelocityIGain(uint16_t i_gain); + Result setHomingOffset(int32_t offset); + Result setMaxPositionLimit(uint32_t limit); + Result setMinPositionLimit(uint32_t limit); + Result setVelocityLimit(uint32_t limit); Result reboot(); Result factoryResetAll(); diff --git a/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp b/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp index 026b849d7..d92129061 100644 --- a/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp +++ b/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp @@ -24,8 +24,7 @@ Connector::Connector(const std::string & port_name, float protocol_version, int throw DxlRuntimeError("Only Protocol 2.0 is supported in this Dynamixel API."); } port_handler_ = std::unique_ptr(PortHandler::getPortHandler(port_name.c_str())); - packet_handler_ = std::unique_ptr( - PacketHandler::getPacketHandler(protocol_version)); + packet_handler_ = PacketHandler::getPacketHandler(protocol_version); if (!port_handler_->openPort()) { throw DxlRuntimeError("Failed to open the port!"); @@ -193,8 +192,16 @@ Result Connector::reboot(uint8_t id) Result Connector::ping(uint8_t id) { - Result result = read2ByteData(id, 0); - return result; + uint8_t dxl_error = 0; + uint16_t data = 0; + int dxl_comm_result = packet_handler_->ping(port_handler_.get(), id, &data, &dxl_error); + if (dxl_comm_result != COMM_SUCCESS) { + return static_cast(dxl_comm_result); + } + if (dxl_error != 0) { + return static_cast(dxl_error); + } + return data; } Result Connector::factoryReset(uint8_t id, uint8_t option) diff --git a/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp b/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp index 15156cecd..0f9f31b33 100644 --- a/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp +++ b/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp @@ -103,7 +103,7 @@ Result Motor::LEDOff() Result Motor::ping() { - Result result = connector_->read2ByteData(id_, 0); + Result result = connector_->ping(id_); return result; } @@ -274,6 +274,120 @@ Result Motor::setDirection(Direction direction) return result; } +Result Motor::setPositionPGain(uint16_t p_gain) +{ + Result item_result = getControlTableItem("Position P Gain"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + if (item.size != 2) { + return DxlError::API_FUNCTION_NOT_SUPPORTED; + } + Result result = connector_->write2ByteData(id_, item.address, p_gain); + return result; +} + +Result Motor::setPositionIGain(uint16_t i_gain) +{ + Result item_result = getControlTableItem("Position I Gain"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + if (item.size != 2) { + return DxlError::API_FUNCTION_NOT_SUPPORTED; + } + Result result = connector_->write2ByteData(id_, item.address, i_gain); + return result; +} + +Result Motor::setPositionDGain(uint16_t d_gain) +{ + Result item_result = getControlTableItem("Position D Gain"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + if (item.size != 2) { + return DxlError::API_FUNCTION_NOT_SUPPORTED; + } + Result result = connector_->write2ByteData(id_, item.address, d_gain); + return result; +} + +Result Motor::setVelocityPGain(uint16_t p_gain) +{ + Result item_result = getControlTableItem("Velocity P Gain"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + if (item.size != 2) { + return DxlError::API_FUNCTION_NOT_SUPPORTED; + } + Result result = connector_->write2ByteData(id_, item.address, p_gain); + return result; +} + +Result Motor::setVelocityIGain(uint16_t i_gain) +{ + Result item_result = getControlTableItem("Velocity I Gain"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + if (item.size != 2) { + return DxlError::API_FUNCTION_NOT_SUPPORTED; + } + Result result = connector_->write2ByteData(id_, item.address, i_gain); + return result; +} + +Result Motor::setHomingOffset(int32_t offset) +{ + Result item_result = getControlTableItem("Homing Offset"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + Result result = connector_->write4ByteData(id_, item.address, static_cast(offset)); + return result; +} + +Result Motor::setMaxPositionLimit(uint32_t limit) +{ + Result item_result = getControlTableItem("Max Position Limit"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + Result result = connector_->write4ByteData(id_, item.address, limit); + return result; +} + +Result Motor::setMinPositionLimit(uint32_t limit) +{ + Result item_result = getControlTableItem("Min Position Limit"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + Result result = connector_->write4ByteData(id_, item.address, limit); + return result; +} + +Result Motor::setVelocityLimit(uint32_t limit) +{ + Result item_result = getControlTableItem("Velocity Limit"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + Result result = connector_->write4ByteData(id_, item.address, limit); + return result; +} + Result Motor::reboot() { Result result = connector_->reboot(id_); From cef960a0578484bce345650f53b99bf470c68eaa Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Mon, 29 Sep 2025 12:26:26 +0900 Subject: [PATCH 23/37] add current function Signed-off-by: Hyungyu Kim --- .../dynamixel_sdk/dynamixel_api/motor.hpp | 6 ++ c++/src/dynamixel_sdk/dynamixel_api/motor.cpp | 72 +++++++++++++++++++ 2 files changed, 78 insertions(+) diff --git a/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp b/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp index 455709a20..e1f3100b1 100644 --- a/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp +++ b/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp @@ -60,6 +60,8 @@ class Motor Result disableTorque(); Result setGoalPosition(uint32_t position); Result setGoalVelocity(uint32_t velocity); + Result setGoalCurrent(int16_t current); + Result setGoalPWM(int16_t pwm); Result LEDOn(); Result LEDOff(); @@ -71,6 +73,8 @@ class Motor Result getMaxPositionLimit(); Result getMinPositionLimit(); Result getVelocityLimit(); + Result getCurrentLimit(); + Result getPWMLimit(); Result changeID(uint8_t new_id); Result setOperatingMode(OperatingMode mode); @@ -86,6 +90,8 @@ class Motor Result setMaxPositionLimit(uint32_t limit); Result setMinPositionLimit(uint32_t limit); Result setVelocityLimit(uint32_t limit); + Result setCurrentLimit(uint16_t limit); + Result setPWMLimit(uint16_t limit); Result reboot(); Result factoryResetAll(); diff --git a/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp b/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp index 0f9f31b33..927277286 100644 --- a/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp +++ b/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp @@ -79,6 +79,34 @@ Result Motor::setGoalVelocity(uint32_t velocity) return result; } +Result Motor::setGoalCurrent(int16_t current) +{ + Result item_result = getControlTableItem("Goal Current"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + if (item.size != 2) { + return DxlError::API_FUNCTION_NOT_SUPPORTED; + } + Result result = connector_->write2ByteData(id_, item.address, current); + return result; +} + +Result Motor::setGoalPWM(int16_t pwm) +{ + Result item_result = getControlTableItem("Goal PWM"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + if (item.size != 2) { + return DxlError::API_FUNCTION_NOT_SUPPORTED; + } + Result result = connector_->write2ByteData(id_, item.address, pwm); + return result; +} + Result Motor::LEDOn() { Result item_result = getControlTableItem("LED"); @@ -199,6 +227,28 @@ Result Motor::getVelocityLimit() return result; } +Result Motor::getCurrentLimit() +{ + Result item_result = getControlTableItem("Current Limit"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + Result result = connector_->read2ByteData(id_, item.address); + return result; +} + +Result Motor::getPWMLimit() +{ + Result item_result = getControlTableItem("PWM Limit"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + Result result = connector_->read2ByteData(id_, item.address); + return result; +} + Result Motor::changeID(uint8_t new_id) { Result item_result = getControlTableItem("ID"); @@ -388,6 +438,28 @@ Result Motor::setVelocityLimit(uint32_t limit) return result; } +Result Motor::setCurrentLimit(uint16_t limit) +{ + Result item_result = getControlTableItem("Current Limit"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + Result result = connector_->write2ByteData(id_, item.address, limit); + return result; +} + +Result Motor::setPWMLimit(uint16_t limit) +{ + Result item_result = getControlTableItem("PWM Limit"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + Result result = connector_->write2ByteData(id_, item.address, limit); + return result; +} + Result Motor::reboot() { Result result = connector_->reboot(id_); From baad93ed763018642464a0ad1bd4680cf665878b Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Fri, 10 Oct 2025 11:53:58 +0900 Subject: [PATCH 24/37] unified motor object with motor proxy Signed-off-by: Hyungyu Kim --- c++/build/linux64/Makefile | 2 +- c++/include/dynamixel_sdk/dynamixel_api.hpp | 2 +- .../dynamixel_sdk/dynamixel_api/connector.hpp | 4 +- .../dynamixel_api/dynamixel_error.hpp | 5 +- .../dynamixel_api/group_executor.hpp | 68 +++ .../dynamixel_sdk/dynamixel_api/motor.hpp | 13 + .../dynamixel_api/motor_group.hpp | 127 ----- .../dynamixel_api/staged_command.hpp | 37 ++ .../dynamixel_sdk/dynamixel_api/connector.cpp | 8 +- .../dynamixel_api/dynamixel_error.cpp | 4 - .../dynamixel_api/group_executor.cpp | 224 +++++++++ c++/src/dynamixel_sdk/dynamixel_api/motor.cpp | 166 +++++++ .../dynamixel_api/motor_group.cpp | 434 ------------------ 13 files changed, 518 insertions(+), 576 deletions(-) create mode 100644 c++/include/dynamixel_sdk/dynamixel_api/group_executor.hpp delete mode 100644 c++/include/dynamixel_sdk/dynamixel_api/motor_group.hpp create mode 100644 c++/include/dynamixel_sdk/dynamixel_api/staged_command.hpp create mode 100644 c++/src/dynamixel_sdk/dynamixel_api/group_executor.cpp delete mode 100644 c++/src/dynamixel_sdk/dynamixel_api/motor_group.cpp diff --git a/c++/build/linux64/Makefile b/c++/build/linux64/Makefile index faa272984..0a9a5f1e3 100644 --- a/c++/build/linux64/Makefile +++ b/c++/build/linux64/Makefile @@ -66,7 +66,7 @@ SOURCES = src/dynamixel_sdk/group_bulk_read.cpp \ src/dynamixel_sdk/dynamixel_api/control_table.cpp \ src/dynamixel_sdk/dynamixel_api/motor.cpp \ src/dynamixel_sdk/dynamixel_api/dynamixel_error.cpp \ - src/dynamixel_sdk/dynamixel_api/motor_group.cpp + src/dynamixel_sdk/dynamixel_api/group_executor.cpp OBJECTS=$(addsuffix .o,$(addprefix $(DIR_OBJS)/,$(basename $(notdir $(SOURCES))))) diff --git a/c++/include/dynamixel_sdk/dynamixel_api.hpp b/c++/include/dynamixel_sdk/dynamixel_api.hpp index b0bf4e800..82aa615bb 100644 --- a/c++/include/dynamixel_sdk/dynamixel_api.hpp +++ b/c++/include/dynamixel_sdk/dynamixel_api.hpp @@ -21,6 +21,6 @@ #include "dynamixel_sdk/dynamixel_api/connector.hpp" #include "dynamixel_sdk/dynamixel_api/dynamixel_error.hpp" #include "dynamixel_sdk/dynamixel_api/motor.hpp" -#include "dynamixel_sdk/dynamixel_api/motor_group.hpp" +#include "dynamixel_sdk/dynamixel_api/group_executor.hpp" #endif /* DYNAMIXEL_SDK_DYNAMIXEL_API_HPP_ */ diff --git a/c++/include/dynamixel_sdk/dynamixel_api/connector.hpp b/c++/include/dynamixel_sdk/dynamixel_api/connector.hpp index a02620273..cde998979 100644 --- a/c++/include/dynamixel_sdk/dynamixel_api/connector.hpp +++ b/c++/include/dynamixel_sdk/dynamixel_api/connector.hpp @@ -24,7 +24,7 @@ #include "dynamixel_sdk/dynamixel_sdk.h" #include "dynamixel_sdk/dynamixel_api/dynamixel_error.hpp" #include "dynamixel_sdk/dynamixel_api/motor.hpp" -#include "dynamixel_sdk/dynamixel_api/motor_group.hpp" +// #include "dynamixel_sdk/dynamixel_api/motor_group.hpp" namespace dynamixel @@ -38,7 +38,7 @@ class Connector std::unique_ptr getMotor(uint8_t id); std::vector> getAllMotors(int start_id = 0, int end_id = 252); - std::unique_ptr getMotorGroup(); + // std::unique_ptr getMotorGroup(); Result write1ByteData(uint8_t id, uint16_t address, uint8_t value); Result write2ByteData(uint8_t id, uint16_t address, uint16_t value); diff --git a/c++/include/dynamixel_sdk/dynamixel_api/dynamixel_error.hpp b/c++/include/dynamixel_sdk/dynamixel_api/dynamixel_error.hpp index 912a3ced1..7a1b87f65 100644 --- a/c++/include/dynamixel_sdk/dynamixel_api/dynamixel_error.hpp +++ b/c++/include/dynamixel_sdk/dynamixel_api/dynamixel_error.hpp @@ -44,9 +44,8 @@ enum class DxlError API_FUNCTION_NOT_SUPPORTED = 11, // API does not support this function API_ADD_PARAM_FAIL = 21, // Failed to add parameter API_COMMAND_IS_EMPTY = 22, // No command to execute - API_INVALID_COMMAND_TYPE = 23, // Invalid command type in staged commands - API_DUPLICATE_ID = 24, // Duplicate ID in staged commands - API_FAIL_TO_GET_DATA = 25 // Failed to get data from motor + API_DUPLICATE_ID = 23, // Duplicate ID in staged commands + API_FAIL_TO_GET_DATA = 24 // Failed to get data from motor }; template diff --git a/c++/include/dynamixel_sdk/dynamixel_api/group_executor.hpp b/c++/include/dynamixel_sdk/dynamixel_api/group_executor.hpp new file mode 100644 index 000000000..a5b251822 --- /dev/null +++ b/c++/include/dynamixel_sdk/dynamixel_api/group_executor.hpp @@ -0,0 +1,68 @@ +// Copyright 2025 ROBOTIS CO., LTD. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Author: Hyungyu Kim + +#ifndef DYNAMIXEL_SDK_DYNAMIXEL_API_GROUP_EXECUTOR_HPP_ +#define DYNAMIXEL_SDK_DYNAMIXEL_API_GROUP_EXECUTOR_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "dynamixel_sdk/dynamixel_sdk.h" +#include "dynamixel_sdk/dynamixel_api/motor.hpp" +#include "dynamixel_sdk/dynamixel_api/control_table.hpp" +#include "dynamixel_sdk/dynamixel_api/dynamixel_error.hpp" +#include "dynamixel_sdk/dynamixel_api/staged_command.hpp" + +namespace dynamixel +{ +class Connector; + +class GroupExecutor +{ +public: + explicit GroupExecutor(Connector * connector); + virtual ~GroupExecutor() = default; + + void addCmd(StagedCommand command); + void addCmd(Result result); + Result executeWrite(); + Result>, DxlError> executeRead(); + +private: + Result executeSyncWrite(uint16_t address, uint16_t length); + Result executeBulkWrite(); + Result>, DxlError> executeSyncRead( + uint16_t address, + uint16_t length); + Result>, DxlError> executeBulkRead(); + + Connector * connector_; + PortHandler * port_handler_; + PacketHandler * packet_handler_; + + GroupBulkWrite group_bulk_write_; + GroupBulkRead group_bulk_read_; + std::vector staged_write_command_list_; + std::vector staged_read_command_list_; +}; + +} // namespace dynamixel + +#endif // DYNAMIXEL_SDK_DYNAMIXEL_API_GROUP_EXECUTOR_HPP_ diff --git a/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp b/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp index e1f3100b1..d4167f2d3 100644 --- a/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp +++ b/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp @@ -25,6 +25,7 @@ #include "dynamixel_sdk/dynamixel_api/control_table.hpp" #include "dynamixel_sdk/dynamixel_sdk.h" #include "dynamixel_sdk/dynamixel_api/dynamixel_error.hpp" +#include "dynamixel_sdk/dynamixel_api/staged_command.hpp" namespace dynamixel { @@ -98,6 +99,18 @@ class Motor Result factoryResetExceptID(); Result factoryResetExceptIDAndBaudRate(); + Result stageEnableTorque(); + Result stageDisableTorque(); + Result stageSetGoalPosition(uint32_t position); + Result stageSetGoalVelocity(uint32_t velocity); + Result stageLEDOn(); + Result stageLEDOff(); + + Result stageIsTorqueOn(); + Result stageIsLEDOn(); + Result stageGetPresentPosition(); + Result stageGetPresentVelocity(); + uint8_t getID() const {return id_;} uint16_t getModelNumber() const {return model_number_;} std::string getModelName() const {return model_name_;} diff --git a/c++/include/dynamixel_sdk/dynamixel_api/motor_group.hpp b/c++/include/dynamixel_sdk/dynamixel_api/motor_group.hpp deleted file mode 100644 index 33dffebd2..000000000 --- a/c++/include/dynamixel_sdk/dynamixel_api/motor_group.hpp +++ /dev/null @@ -1,127 +0,0 @@ -// Copyright 2025 ROBOTIS CO., LTD. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Author: Hyungyu Kim - -#ifndef DYNAMIXEL_SDK_DYNAMIXEL_API_MOTOR_GROUP_HPP_ -#define DYNAMIXEL_SDK_DYNAMIXEL_API_MOTOR_GROUP_HPP_ - -#include -#include -#include -#include -#include -#include - -#include "dynamixel_sdk/dynamixel_sdk.h" -#include "dynamixel_sdk/dynamixel_api/motor.hpp" -#include "dynamixel_sdk/dynamixel_api/control_table.hpp" -#include "dynamixel_sdk/dynamixel_api/dynamixel_error.hpp" - -namespace dynamixel -{ -class Connector; - -class MotorGroup -{ -public: - enum class CommandType - { - WRITE, - READ - }; - class MotorProxy - { - public: - using OperatingMode = Motor::OperatingMode; - using ProfileConfiguration = Motor::ProfileConfiguration; - using Direction = Motor::Direction; - - MotorProxy(uint8_t id, uint16_t model_number, Connector * connector, MotorGroup * motor_group); - ~MotorProxy() = default; - - Result stageEnableTorque(); - Result stageDisableTorque(); - Result stageSetGoalPosition(uint32_t position); - Result stageSetGoalVelocity(uint32_t velocity); - Result stageLEDOn(); - Result stageLEDOff(); - - Result stageIsTorqueOn(); - Result stageIsLEDOn(); - Result stageGetPresentPosition(); - Result stageGetPresentVelocity(); - - uint8_t getID() const {return id_;} - uint16_t getModelNumber() const {return model_number_;} - std::string getModelName() const {return model_name_;} - - private: - Result getControlTableItem(const std::string & item_name); - uint8_t id_; - uint16_t model_number_; - std::string model_name_; - MotorGroup * motor_group_; - - Connector * connector_; - const std::map & control_table_; - }; - - explicit MotorGroup(Connector * connector); - virtual ~MotorGroup() = default; - - MotorProxy * addMotor(uint8_t id, const std::string & motor_name); - Result executeWrite(); - Result>, DxlError> executeRead(); - -private: - struct StagedCommand - { - StagedCommand( - CommandType _command_type, - uint8_t _id, - uint16_t _address, - uint16_t _length, - const std::vector & _data) - : command_type(_command_type), id(_id), address(_address), length(_length), data(_data) {} - CommandType command_type; - uint8_t id; - uint16_t address; - uint16_t length; - std::vector data; - }; - - Result executeSyncWrite(uint16_t address, uint16_t length); - Result executeBulkWrite(); - Result>, DxlError> executeSyncRead( - uint16_t address, - uint16_t length); - Result>, DxlError> executeBulkRead(); - void stageCommand(StagedCommand && command, CommandType type); - - Connector * connector_; - PortHandler * port_handler_; - PacketHandler * packet_handler_; - - std::unordered_map> motors_list_; - GroupBulkWrite group_bulk_write_; - GroupBulkRead group_bulk_read_; - std::vector staged_write_command_list_; - std::vector staged_read_command_list_; - friend class MotorProxy; -}; - -} // namespace dynamixel - -#endif // DYNAMIXEL_SDK_DYNAMIXEL_API_MOTOR_GROUP_HPP_ diff --git a/c++/include/dynamixel_sdk/dynamixel_api/staged_command.hpp b/c++/include/dynamixel_sdk/dynamixel_api/staged_command.hpp new file mode 100644 index 000000000..8c8ac4e52 --- /dev/null +++ b/c++/include/dynamixel_sdk/dynamixel_api/staged_command.hpp @@ -0,0 +1,37 @@ +#ifndef DYNAMIXEL_SDK_DYNAMIXEL_API_STAGED_COMMAND_HPP_ +#define DYNAMIXEL_SDK_DYNAMIXEL_API_STAGED_COMMAND_HPP_ + +#include +#include + +namespace dynamixel +{ +enum class CommandType { + WRITE, + READ +}; + +struct StagedCommand +{ + StagedCommand( + CommandType _command_type, + uint8_t _id, + uint16_t _address, + uint16_t _length, + const std::vector& _data) + : command_type(_command_type), + id(_id), + address(_address), + length(_length), + data(_data) {} + + CommandType command_type; + uint8_t id; + uint16_t address; + uint16_t length; + std::vector data; +}; + +} // namespace dynamixel + +#endif // DYNAMIXEL_SDK_DYNAMIXEL_API_STAGED_COMMAND_HPP_ diff --git a/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp b/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp index d92129061..c13f9c829 100644 --- a/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp +++ b/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp @@ -68,10 +68,10 @@ std::vector> Connector::getAllMotors(int start_id, int en return motors; } -std::unique_ptr Connector::getMotorGroup() -{ - return std::make_unique(this); -} +// std::unique_ptr Connector::getMotorGroup() +// { +// return std::make_unique(this); +// } Result Connector::read1ByteData(uint8_t id, uint16_t address) { diff --git a/c++/src/dynamixel_sdk/dynamixel_api/dynamixel_error.cpp b/c++/src/dynamixel_sdk/dynamixel_api/dynamixel_error.cpp index 54965b063..f636af3f4 100644 --- a/c++/src/dynamixel_sdk/dynamixel_api/dynamixel_error.cpp +++ b/c++/src/dynamixel_sdk/dynamixel_api/dynamixel_error.cpp @@ -59,10 +59,6 @@ std::string getErrorMessage(DxlError error_code) return "[APIError] Failed to add parameter!"; case DxlError::API_COMMAND_IS_EMPTY: return "[APIError] No command to execute!"; - case DxlError::API_INVALID_COMMAND_TYPE: - return - "[APIError] Read commands can only be run with executeRead()," - "and Write commands with executeWrite()."; case DxlError::API_DUPLICATE_ID: return "[APIError] Duplicate ID found in staged commands."; default: return "Unknown Error"; diff --git a/c++/src/dynamixel_sdk/dynamixel_api/group_executor.cpp b/c++/src/dynamixel_sdk/dynamixel_api/group_executor.cpp new file mode 100644 index 000000000..591b74938 --- /dev/null +++ b/c++/src/dynamixel_sdk/dynamixel_api/group_executor.cpp @@ -0,0 +1,224 @@ +// Copyright 2025 ROBOTIS CO., LTD. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Author: Hyungyu Kim + +#include + +#include "dynamixel_api/group_executor.hpp" +#include "dynamixel_api/connector.hpp" + +namespace dynamixel +{ + +GroupExecutor::GroupExecutor(Connector * connector) +: connector_(connector), + port_handler_(connector->getPortHandler()), + packet_handler_(connector->getPacketHandler()), + group_bulk_write_(port_handler_, packet_handler_), + group_bulk_read_(port_handler_, packet_handler_) +{} + +void GroupExecutor::addCmd(StagedCommand command) +{ + if (command.command_type == CommandType::WRITE) { + staged_write_command_list_.push_back(std::move(command)); + } else if (command.command_type == CommandType::READ) { + staged_read_command_list_.push_back(std::move(command)); + } +} + +void GroupExecutor::addCmd(Result result) +{ + if (!result.isSuccess()) { + return; + } + + if (result.value().command_type == CommandType::WRITE) { + staged_write_command_list_.push_back(std::move(result.value())); + } else if (result.value().command_type == CommandType::READ) { + staged_read_command_list_.push_back(std::move(result.value())); + } +} + +Result GroupExecutor::executeWrite() +{ + if (staged_write_command_list_.empty()) { + return DxlError::API_COMMAND_IS_EMPTY; + } + const auto & reference_command = staged_write_command_list_.front(); + bool is_sync = true; + std::vector sorted_list = staged_write_command_list_; + std::sort( + sorted_list.begin(), + sorted_list.end(), + [](const StagedCommand & a, const StagedCommand & b) { + return a.id < b.id; + } + ); + + auto it = std::adjacent_find( + sorted_list.begin(), + sorted_list.end(), + [](const StagedCommand & a, const StagedCommand & b) { + return a.id == b.id; + } + ); + + if (it != sorted_list.end()) { + return DxlError::API_DUPLICATE_ID; + } + + for (size_t i = 1; i < staged_write_command_list_.size(); ++i) { + if (staged_write_command_list_[i].address != reference_command.address || + staged_write_command_list_[i].length != reference_command.length) + { + is_sync = false; + break; + } + } + + Result result; + if (is_sync) { + result = executeSyncWrite(reference_command.address, reference_command.length); + } else { + result = executeBulkWrite(); + } + + staged_write_command_list_.clear(); + return result; +} + +Result>, DxlError> GroupExecutor::executeRead() +{ + if (staged_read_command_list_.empty()) { + return DxlError::API_COMMAND_IS_EMPTY; + } + const auto & reference_command = staged_read_command_list_.front(); + bool is_sync = true; + for (size_t i = 1; i < staged_read_command_list_.size(); ++i) { + if (staged_read_command_list_[i].address != reference_command.address || + staged_read_command_list_[i].length != reference_command.length) + { + is_sync = false; + break; + } + } + + Result>, DxlError> result; + if (is_sync) { + result = executeSyncRead(reference_command.address, reference_command.length); + } else { + result = executeBulkRead(); + } + staged_read_command_list_.clear(); + return result; +} + + +Result GroupExecutor::executeSyncWrite(uint16_t address, uint16_t length) +{ + GroupSyncWrite group_sync_write(port_handler_, packet_handler_, address, length); + for (auto & command : staged_write_command_list_) { + if (!group_sync_write.addParam(command.id, command.data.data())) { + return DxlError::API_ADD_PARAM_FAIL; + } + } + int dxl_comm_result = group_sync_write.txPacket(); + if (dxl_comm_result != COMM_SUCCESS) { + return static_cast(dxl_comm_result); + } + return {}; +} + +Result GroupExecutor::executeBulkWrite() +{ + group_bulk_write_.clearParam(); + + for (auto & command : staged_write_command_list_) { + if (!group_bulk_write_.addParam( + command.id, + command.address, + command.length, + command.data.data())) + { + return DxlError::API_ADD_PARAM_FAIL; + } + } + + int dxl_comm_result = group_bulk_write_.txPacket(); + if (dxl_comm_result != COMM_SUCCESS) { + return static_cast(dxl_comm_result); + } + return {}; +} + +Result>, DxlError> GroupExecutor::executeSyncRead( + uint16_t address, + uint16_t length) +{ + GroupSyncRead group_sync_read(port_handler_, packet_handler_, address, length); + for (const auto & command : staged_read_command_list_) { + if (!group_sync_read.addParam(command.id)) { + return DxlError::API_ADD_PARAM_FAIL; + } + } + + int dxl_comm_result = group_sync_read.txRxPacket(); + if (dxl_comm_result != COMM_SUCCESS) { + return static_cast(dxl_comm_result); + } + std::vector> result_list; + for (const auto & command : staged_read_command_list_) { + if (!group_sync_read.isAvailable(command.id, address, length)) { + result_list.push_back(DxlError::API_FAIL_TO_GET_DATA); + continue; + } + int32_t value = group_sync_read.getData(command.id, address, length); + result_list.push_back(value); + } + + return result_list; +} + +Result>, DxlError> GroupExecutor::executeBulkRead() +{ + group_bulk_read_.clearParam(); + + for (const auto & command : staged_read_command_list_) { + if (!group_bulk_read_.addParam(command.id, command.address, command.length)) { + return DxlError::API_ADD_PARAM_FAIL; + } + } + + int dxl_comm_result = group_bulk_read_.txRxPacket(); + if (dxl_comm_result != COMM_SUCCESS) { + return static_cast(dxl_comm_result); + } + + std::vector> result_list; + for (const auto & command : staged_read_command_list_) { + if (!group_bulk_read_.isAvailable(command.id, command.address, command.length)) { + result_list.push_back(DxlError::API_FAIL_TO_GET_DATA); + continue; + } + + int32_t value = group_bulk_read_.getData(command.id, command.address, command.length); + result_list.push_back(value); + } + + return result_list; +} + +} // namespace dynamixel diff --git a/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp b/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp index 927277286..2eb8782bb 100644 --- a/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp +++ b/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp @@ -484,6 +484,172 @@ Result Motor::factoryResetExceptIDAndBaudRate() return result; } +Result Motor::stageEnableTorque() +{ + Result item_result = getControlTableItem("Torque Enable"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + StagedCommand cmd( + CommandType::WRITE, + id_, + item_result.value().address, + item_result.value().size, + {1}); + return cmd; +} + +Result Motor::stageDisableTorque() +{ + Result item_result = getControlTableItem("Torque Enable"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + StagedCommand cmd( + CommandType::WRITE, + id_, + item_result.value().address, + item_result.value().size, + {0}); + return cmd; +} + +Result Motor::stageSetGoalPosition(uint32_t position) +{ + Result item_result = getControlTableItem("Goal Position"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + std::vector data; + for (size_t i = 0; i < item_result.value().size; ++i) { + data.push_back((position >> (8 * i)) & 0xFF); + } + StagedCommand cmd( + CommandType::WRITE, + id_, + item_result.value().address, + item_result.value().size, + data + ); + return cmd; +} + +Result Motor::stageSetGoalVelocity(uint32_t velocity) +{ + Result item_result = getControlTableItem("Goal Velocity"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + std::vector data; + for (size_t i = 0; i < item_result.value().size; ++i) { + data.push_back((velocity >> (8 * i)) & 0xFF); + } + StagedCommand cmd( + CommandType::WRITE, + id_, + item_result.value().address, + item_result.value().size, + data + ); + return cmd; +} + +Result Motor::stageLEDOn() +{ + Result item_result = getControlTableItem("LED"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + StagedCommand cmd( + CommandType::WRITE, + id_, + item_result.value().address, + item_result.value().size, + {1} + ); + return cmd; +} + +Result Motor::stageLEDOff() +{ + Result item_result = getControlTableItem("LED"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + StagedCommand cmd( + CommandType::WRITE, + id_, + item_result.value().address, + item_result.value().size, + {0} + ); + return cmd; +} + +Result Motor::stageIsTorqueOn() +{ + Result item_result = getControlTableItem("Torque Enable"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + StagedCommand cmd( + CommandType::READ, + id_, + item_result.value().address, + item_result.value().size, + {} + ); + return cmd; +} + +Result Motor::stageIsLEDOn() +{ + Result item_result = getControlTableItem("LED"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + StagedCommand cmd( + CommandType::READ, + id_, + item_result.value().address, + item_result.value().size, + {} + ); + return cmd; +} + +Result Motor::stageGetPresentPosition() +{ + Result item_result = getControlTableItem("Present Position"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + StagedCommand cmd( + CommandType::READ, + id_, + item_result.value().address, + item_result.value().size, + {} + ); + return cmd; +} + +Result Motor::stageGetPresentVelocity() +{ + Result item_result = getControlTableItem("Present Velocity"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + StagedCommand cmd( + CommandType::READ, + id_, + item_result.value().address, + item_result.value().size, + {} + ); + return cmd; +} + Result Motor::getControlTableItem(const std::string & name) { auto it = control_table_.find(name); diff --git a/c++/src/dynamixel_sdk/dynamixel_api/motor_group.cpp b/c++/src/dynamixel_sdk/dynamixel_api/motor_group.cpp deleted file mode 100644 index 85a88ff18..000000000 --- a/c++/src/dynamixel_sdk/dynamixel_api/motor_group.cpp +++ /dev/null @@ -1,434 +0,0 @@ -// Copyright 2025 ROBOTIS CO., LTD. -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. -// -// Author: Hyungyu Kim - -#include - -#include "dynamixel_api/motor_group.hpp" -#include "dynamixel_api/connector.hpp" - -namespace dynamixel -{ -MotorGroup::MotorProxy::MotorProxy( - uint8_t id, - uint16_t model_number, - Connector * connector, - MotorGroup * motor_group) -: id_(id), - model_number_(model_number), - model_name_(ControlTable::getModelName(model_number)), - motor_group_(motor_group), - connector_(connector), - control_table_(ControlTable::getControlTable(model_number)) -{} - -Result MotorGroup::MotorProxy::stageEnableTorque() -{ - Result item_result = getControlTableItem("Torque Enable"); - if (!item_result.isSuccess()) { - return item_result.error(); - } - StagedCommand cmd( - CommandType::WRITE, - id_, - item_result.value().address, - item_result.value().size, - {1}); - motor_group_->stageCommand(std::move(cmd), CommandType::WRITE); - return {}; -} - -Result MotorGroup::MotorProxy::stageDisableTorque() -{ - Result item_result = getControlTableItem("Torque Enable"); - if (!item_result.isSuccess()) { - return item_result.error(); - } - StagedCommand cmd( - CommandType::WRITE, - id_, - item_result.value().address, - item_result.value().size, - {0}); - motor_group_->stageCommand(std::move(cmd), CommandType::WRITE); - return {}; -} - -Result MotorGroup::MotorProxy::stageSetGoalPosition(uint32_t position) -{ - Result item_result = getControlTableItem("Goal Position"); - if (!item_result.isSuccess()) { - return item_result.error(); - } - std::vector data; - for (size_t i = 0; i < item_result.value().size; ++i) { - data.push_back((position >> (8 * i)) & 0xFF); - } - StagedCommand cmd( - CommandType::WRITE, - id_, - item_result.value().address, - item_result.value().size, - data - ); - motor_group_->stageCommand(std::move(cmd), CommandType::WRITE); - return {}; -} - -Result MotorGroup::MotorProxy::stageSetGoalVelocity(uint32_t velocity) -{ - Result item_result = getControlTableItem("Goal Velocity"); - if (!item_result.isSuccess()) { - return item_result.error(); - } - std::vector data; - for (size_t i = 0; i < item_result.value().size; ++i) { - data.push_back((velocity >> (8 * i)) & 0xFF); - } - StagedCommand cmd( - CommandType::WRITE, - id_, - item_result.value().address, - item_result.value().size, - data - ); - motor_group_->stageCommand(std::move(cmd), CommandType::WRITE); - return {}; -} - -Result MotorGroup::MotorProxy::stageLEDOn() -{ - Result item_result = getControlTableItem("LED"); - if (!item_result.isSuccess()) { - return item_result.error(); - } - StagedCommand cmd( - CommandType::WRITE, - id_, - item_result.value().address, - item_result.value().size, - {1} - ); - motor_group_->stageCommand(std::move(cmd), CommandType::WRITE); - return {}; -} - -Result MotorGroup::MotorProxy::stageLEDOff() -{ - Result item_result = getControlTableItem("LED"); - if (!item_result.isSuccess()) { - return item_result.error(); - } - StagedCommand cmd( - CommandType::WRITE, - id_, - item_result.value().address, - item_result.value().size, - {0} - ); - motor_group_->stageCommand(std::move(cmd), CommandType::WRITE); - return {}; -} - -Result MotorGroup::MotorProxy::stageIsTorqueOn() -{ - Result item_result = getControlTableItem("Torque Enable"); - if (!item_result.isSuccess()) { - return item_result.error(); - } - StagedCommand cmd( - CommandType::READ, - id_, - item_result.value().address, - item_result.value().size, - {} - ); - motor_group_->stageCommand(std::move(cmd), CommandType::READ); - return {}; -} - -Result MotorGroup::MotorProxy::stageIsLEDOn() -{ - Result item_result = getControlTableItem("LED"); - if (!item_result.isSuccess()) { - return item_result.error(); - } - StagedCommand cmd( - CommandType::READ, - id_, - item_result.value().address, - item_result.value().size, - {} - ); - motor_group_->stageCommand(std::move(cmd), CommandType::READ); - return {}; -} - -Result MotorGroup::MotorProxy::stageGetPresentPosition() -{ - Result item_result = getControlTableItem("Present Position"); - if (!item_result.isSuccess()) { - return item_result.error(); - } - StagedCommand cmd( - CommandType::READ, - id_, - item_result.value().address, - item_result.value().size, - {} - ); - motor_group_->stageCommand(std::move(cmd), CommandType::READ); - return {}; -} - -Result MotorGroup::MotorProxy::stageGetPresentVelocity() -{ - Result item_result = getControlTableItem("Present Velocity"); - if (!item_result.isSuccess()) { - return item_result.error(); - } - StagedCommand cmd( - CommandType::READ, - id_, - item_result.value().address, - item_result.value().size, - {} - ); - motor_group_->stageCommand(std::move(cmd), CommandType::READ); - return {}; -} - -Result MotorGroup::MotorProxy::getControlTableItem( - const std::string & name) -{ - auto it = control_table_.find(name); - if (it == control_table_.end()) { - return DxlError::API_FUNCTION_NOT_SUPPORTED; - } - return it->second; -} - -MotorGroup::MotorGroup(Connector * connector) -: connector_(connector), - port_handler_(connector->getPortHandler()), - packet_handler_(connector->getPacketHandler()), - group_bulk_write_(port_handler_, packet_handler_), - group_bulk_read_(port_handler_, packet_handler_) -{} - -MotorGroup::MotorProxy * MotorGroup::addMotor(uint8_t id, const std::string & motor_name) -{ - if (motors_list_.find(motor_name) != motors_list_.end()) { - throw DxlRuntimeError("Motor with name '" + motor_name + "' already exists in the group."); - } - - Result result = connector_->ping(id); - if (!result.isSuccess()) { - throw DxlRuntimeError(getErrorMessage(result.error())); - } - - auto [it, success] = motors_list_.emplace( - motor_name, - std::make_unique(id, result.value(), connector_, this)); - if (!success) { - throw std::runtime_error("Failed to emplace motor '" + motor_name + "'"); - } - return it->second.get(); -} - -Result MotorGroup::executeWrite() -{ - if (staged_write_command_list_.empty()) { - return DxlError::API_COMMAND_IS_EMPTY; - } - const auto & reference_command = staged_write_command_list_.front(); - bool is_sync = true; - std::vector sorted_list = staged_write_command_list_; - std::sort( - sorted_list.begin(), - sorted_list.end(), - [](const StagedCommand & a, const StagedCommand & b) { - return a.id < b.id; - } - ); - - auto it = std::adjacent_find( - sorted_list.begin(), - sorted_list.end(), - [](const StagedCommand & a, const StagedCommand & b) { - return a.id == b.id; - } - ); - - if (it != sorted_list.end()) { - return DxlError::API_DUPLICATE_ID; - } - - for (size_t i = 1; i < staged_write_command_list_.size(); ++i) { - if (staged_write_command_list_[i].command_type != CommandType::WRITE) { - return DxlError::API_INVALID_COMMAND_TYPE; - } - if (staged_write_command_list_[i].address != reference_command.address || - staged_write_command_list_[i].length != reference_command.length) - { - is_sync = false; - break; - } - } - - Result result; - if (is_sync) { - result = executeSyncWrite(reference_command.address, reference_command.length); - } else { - result = executeBulkWrite(); - } - - staged_write_command_list_.clear(); - return result; -} - -Result>, DxlError> MotorGroup::executeRead() -{ - if (staged_read_command_list_.empty()) { - return DxlError::API_COMMAND_IS_EMPTY; - } - const auto & reference_command = staged_read_command_list_.front(); - bool is_sync = true; - for (size_t i = 1; i < staged_read_command_list_.size(); ++i) { - if (staged_read_command_list_[i].command_type != CommandType::READ) { - return DxlError::API_INVALID_COMMAND_TYPE; - } - if (staged_read_command_list_[i].address != reference_command.address || - staged_read_command_list_[i].length != reference_command.length) - { - is_sync = false; - break; - } - } - - Result>, DxlError> result; - if (is_sync) { - result = executeSyncRead(reference_command.address, reference_command.length); - } else { - result = executeBulkRead(); - } - staged_read_command_list_.clear(); - return result; -} - - -Result MotorGroup::executeSyncWrite(uint16_t address, uint16_t length) -{ - GroupSyncWrite group_sync_write(port_handler_, packet_handler_, address, length); - for (auto & command : staged_write_command_list_) { - if (!group_sync_write.addParam(command.id, command.data.data())) { - return DxlError::API_ADD_PARAM_FAIL; - } - } - int dxl_comm_result = group_sync_write.txPacket(); - if (dxl_comm_result != COMM_SUCCESS) { - return static_cast(dxl_comm_result); - } - return {}; -} - -Result MotorGroup::executeBulkWrite() -{ - group_bulk_write_.clearParam(); - - for (auto & command : staged_write_command_list_) { - if (!group_bulk_write_.addParam( - command.id, - command.address, - command.length, - command.data.data())) - { - return DxlError::API_ADD_PARAM_FAIL; - } - } - - int dxl_comm_result = group_bulk_write_.txPacket(); - if (dxl_comm_result != COMM_SUCCESS) { - return static_cast(dxl_comm_result); - } - return {}; -} - -Result>, DxlError> MotorGroup::executeSyncRead( - uint16_t address, - uint16_t length) -{ - GroupSyncRead group_sync_read(port_handler_, packet_handler_, address, length); - for (const auto & command : staged_read_command_list_) { - if (!group_sync_read.addParam(command.id)) { - return DxlError::API_ADD_PARAM_FAIL; - } - } - - int dxl_comm_result = group_sync_read.txRxPacket(); - if (dxl_comm_result != COMM_SUCCESS) { - return static_cast(dxl_comm_result); - } - std::vector> result_list; - for (const auto & command : staged_read_command_list_) { - if (!group_sync_read.isAvailable(command.id, address, length)) { - result_list.push_back(DxlError::API_FAIL_TO_GET_DATA); - continue; - } - int32_t value = group_sync_read.getData(command.id, address, length); - result_list.push_back(value); - } - - return result_list; -} - -Result>, DxlError> MotorGroup::executeBulkRead() -{ - group_bulk_read_.clearParam(); - - for (const auto & command : staged_read_command_list_) { - if (!group_bulk_read_.addParam(command.id, command.address, command.length)) { - return DxlError::API_ADD_PARAM_FAIL; - } - } - - int dxl_comm_result = group_bulk_read_.txRxPacket(); - if (dxl_comm_result != COMM_SUCCESS) { - return static_cast(dxl_comm_result); - } - - std::vector> result_list; - for (const auto & command : staged_read_command_list_) { - if (!group_bulk_read_.isAvailable(command.id, command.address, command.length)) { - result_list.push_back(DxlError::API_FAIL_TO_GET_DATA); - continue; - } - - int32_t value = group_bulk_read_.getData(command.id, command.address, command.length); - result_list.push_back(value); - } - - return result_list; -} - -void MotorGroup::stageCommand(StagedCommand && command, CommandType type) -{ - if (type == CommandType::WRITE) { - staged_write_command_list_.push_back(std::move(command)); - } else if (type == CommandType::READ) { - staged_read_command_list_.push_back(std::move(command)); - } -} -} // namespace dynamixel From bfb62ef4d8bec3b4c58826f273c683b2c154fe81 Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Fri, 10 Oct 2025 12:17:58 +0900 Subject: [PATCH 25/37] change function name which is named getMotor to createMotor Signed-off-by: Hyungyu Kim --- .../dynamixel_sdk/dynamixel_api/connector.hpp | 9 ++++----- c++/src/dynamixel_sdk/dynamixel_api/connector.cpp | 14 +++++++------- 2 files changed, 11 insertions(+), 12 deletions(-) diff --git a/c++/include/dynamixel_sdk/dynamixel_api/connector.hpp b/c++/include/dynamixel_sdk/dynamixel_api/connector.hpp index cde998979..8fd82caf1 100644 --- a/c++/include/dynamixel_sdk/dynamixel_api/connector.hpp +++ b/c++/include/dynamixel_sdk/dynamixel_api/connector.hpp @@ -24,8 +24,7 @@ #include "dynamixel_sdk/dynamixel_sdk.h" #include "dynamixel_sdk/dynamixel_api/dynamixel_error.hpp" #include "dynamixel_sdk/dynamixel_api/motor.hpp" -// #include "dynamixel_sdk/dynamixel_api/motor_group.hpp" - +#include "dynamixel_sdk/dynamixel_api/group_executor.hpp" namespace dynamixel { @@ -36,9 +35,9 @@ class Connector virtual ~Connector(); - std::unique_ptr getMotor(uint8_t id); - std::vector> getAllMotors(int start_id = 0, int end_id = 252); - // std::unique_ptr getMotorGroup(); + std::unique_ptr createMotor(uint8_t id); + std::vector> createAllMotors(int start_id = 0, int end_id = 252); + std::unique_ptr createGroupExecutor(); Result write1ByteData(uint8_t id, uint16_t address, uint8_t value); Result write2ByteData(uint8_t id, uint16_t address, uint16_t value); diff --git a/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp b/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp index c13f9c829..d61ef6ada 100644 --- a/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp +++ b/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp @@ -40,7 +40,7 @@ Connector::~Connector() port_handler_->closePort(); } -std::unique_ptr Connector::getMotor(uint8_t id) +std::unique_ptr Connector::createMotor(uint8_t id) { Result result = ping(id); if (!result.isSuccess()) { @@ -49,7 +49,7 @@ std::unique_ptr Connector::getMotor(uint8_t id) return std::make_unique(id, result.value(), this); } -std::vector> Connector::getAllMotors(int start_id, int end_id) +std::vector> Connector::createAllMotors(int start_id, int end_id) { if (start_id < 0 || start_id > 252 || end_id < 0 || end_id > 252 || start_id > end_id) { throw DxlRuntimeError("Invalid ID range. ID should be in the range of 0 to 252."); @@ -62,16 +62,16 @@ std::vector> Connector::getAllMotors(int start_id, int en } for (auto & id : ids) { if (id >= start_id && id <= end_id) { - motors.push_back(getMotor(id)); + motors.push_back(createMotor(id)); } } return motors; } -// std::unique_ptr Connector::getMotorGroup() -// { -// return std::make_unique(this); -// } +std::unique_ptr Connector::createGroupExecutor() +{ + return std::make_unique(this); +} Result Connector::read1ByteData(uint8_t id, uint16_t address) { From 3ef96ec257740e0b3be0bf18e50881d319fde078 Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Fri, 10 Oct 2025 16:31:49 +0900 Subject: [PATCH 26/37] remove motor header in group_executor.hpp Signed-off-by: Hyungyu Kim --- c++/include/dynamixel_sdk/dynamixel_api/group_executor.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/c++/include/dynamixel_sdk/dynamixel_api/group_executor.hpp b/c++/include/dynamixel_sdk/dynamixel_api/group_executor.hpp index a5b251822..abb12ff89 100644 --- a/c++/include/dynamixel_sdk/dynamixel_api/group_executor.hpp +++ b/c++/include/dynamixel_sdk/dynamixel_api/group_executor.hpp @@ -25,7 +25,6 @@ #include #include "dynamixel_sdk/dynamixel_sdk.h" -#include "dynamixel_sdk/dynamixel_api/motor.hpp" #include "dynamixel_sdk/dynamixel_api/control_table.hpp" #include "dynamixel_sdk/dynamixel_api/dynamixel_error.hpp" #include "dynamixel_sdk/dynamixel_api/staged_command.hpp" From 0af6a8a476c36894d3a3aedbafe7acafe1adc6be Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Tue, 14 Oct 2025 11:18:49 +0900 Subject: [PATCH 27/37] lint Signed-off-by: Hyungyu Kim --- .../dynamixel_sdk/dynamixel_api/motor.hpp | 1 - .../dynamixel_api/staged_command.hpp | 35 ++++++++++++++----- c++/src/dynamixel_sdk/dynamixel_api/motor.cpp | 5 ++- 3 files changed, 30 insertions(+), 11 deletions(-) diff --git a/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp b/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp index d4167f2d3..778708f86 100644 --- a/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp +++ b/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp @@ -81,7 +81,6 @@ class Motor Result setOperatingMode(OperatingMode mode); Result setProfileConfiguration(ProfileConfiguration config); Result setDirection(Direction direction); - //New Result setPositionPGain(uint16_t p_gain); Result setPositionIGain(uint16_t i_gain); Result setPositionDGain(uint16_t d_gain); diff --git a/c++/include/dynamixel_sdk/dynamixel_api/staged_command.hpp b/c++/include/dynamixel_sdk/dynamixel_api/staged_command.hpp index 8c8ac4e52..f7b9ea723 100644 --- a/c++/include/dynamixel_sdk/dynamixel_api/staged_command.hpp +++ b/c++/include/dynamixel_sdk/dynamixel_api/staged_command.hpp @@ -1,3 +1,19 @@ +// Copyright 2025 ROBOTIS CO., LTD. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// +// Author: Hyungyu Kim + #ifndef DYNAMIXEL_SDK_DYNAMIXEL_API_STAGED_COMMAND_HPP_ #define DYNAMIXEL_SDK_DYNAMIXEL_API_STAGED_COMMAND_HPP_ @@ -6,9 +22,10 @@ namespace dynamixel { -enum class CommandType { - WRITE, - READ +enum class CommandType +{ + WRITE, + READ }; struct StagedCommand @@ -18,12 +35,12 @@ struct StagedCommand uint8_t _id, uint16_t _address, uint16_t _length, - const std::vector& _data) - : command_type(_command_type), - id(_id), - address(_address), - length(_length), - data(_data) {} + const std::vector & _data) + : command_type(_command_type), + id(_id), + address(_address), + length(_length), + data(_data) {} CommandType command_type; uint8_t id; diff --git a/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp b/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp index 2eb8782bb..ec20c1fbe 100644 --- a/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp +++ b/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp @@ -401,7 +401,10 @@ Result Motor::setHomingOffset(int32_t offset) return item_result.error(); } const ControlTableItem & item = item_result.value(); - Result result = connector_->write4ByteData(id_, item.address, static_cast(offset)); + Result result = connector_->write4ByteData( + id_, + item.address, + static_cast(offset)); return result; } From 970dddf9c4639125a43e28ceac4a62a6861a84d1 Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Tue, 14 Oct 2025 11:23:18 +0900 Subject: [PATCH 28/37] bump Signed-off-by: Hyungyu Kim --- ReleaseNote.md | 2 +- ros/dynamixel_sdk/CHANGELOG.rst | 2 +- ros/dynamixel_sdk/package.xml | 2 +- ros/dynamixel_sdk_custom_interfaces/CHANGELOG.rst | 2 +- ros/dynamixel_sdk_custom_interfaces/package.xml | 2 +- ros/dynamixel_sdk_examples/CHANGELOG.rst | 2 +- ros/dynamixel_sdk_examples/package.xml | 2 +- 7 files changed, 7 insertions(+), 7 deletions(-) diff --git a/ReleaseNote.md b/ReleaseNote.md index 226dde1cb..3feb4e670 100644 --- a/ReleaseNote.md +++ b/ReleaseNote.md @@ -1,6 +1,6 @@ # Dynamixel SDK Release Notes -3.9.0 (2025-09-09) +4.0.0 (2025-10-14) ------------------ * Update Dynamixel API in C++ Linux 64bit * Contributors: Hyungyu Kim diff --git a/ros/dynamixel_sdk/CHANGELOG.rst b/ros/dynamixel_sdk/CHANGELOG.rst index 7b8853c5a..c0f8666e9 100644 --- a/ros/dynamixel_sdk/CHANGELOG.rst +++ b/ros/dynamixel_sdk/CHANGELOG.rst @@ -2,7 +2,7 @@ Changelog for package dynamixel_sdk ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -3.9.0 (2025-09-09) +4.0.0 (2025-10-14) ------------------ * None diff --git a/ros/dynamixel_sdk/package.xml b/ros/dynamixel_sdk/package.xml index 06f20cd93..49bd86cca 100644 --- a/ros/dynamixel_sdk/package.xml +++ b/ros/dynamixel_sdk/package.xml @@ -2,7 +2,7 @@ dynamixel_sdk - 3.9.0 + 4.0.0 This package is wrapping version of ROBOTIS Dynamixel SDK for ROS 2. The ROBOTIS Dynamixel SDK, or SDK, is a software development library that provides Dynamixel control functions for packet communication. The API is designed for Dynamixel actuators and Dynamixel-based platforms. diff --git a/ros/dynamixel_sdk_custom_interfaces/CHANGELOG.rst b/ros/dynamixel_sdk_custom_interfaces/CHANGELOG.rst index 3fb297df7..860d100e3 100644 --- a/ros/dynamixel_sdk_custom_interfaces/CHANGELOG.rst +++ b/ros/dynamixel_sdk_custom_interfaces/CHANGELOG.rst @@ -2,7 +2,7 @@ Changelog for package dynamixel_sdk_custom_interfaces ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -3.9.0 (2025-09-09) +4.0.0 (2025-10-14) ------------------ * None diff --git a/ros/dynamixel_sdk_custom_interfaces/package.xml b/ros/dynamixel_sdk_custom_interfaces/package.xml index 764f679cd..365f7332d 100644 --- a/ros/dynamixel_sdk_custom_interfaces/package.xml +++ b/ros/dynamixel_sdk_custom_interfaces/package.xml @@ -2,7 +2,7 @@ dynamixel_sdk_custom_interfaces - 3.9.0 + 4.0.0 ROS 2 custom interface examples using ROBOTIS DYNAMIXEL SDK Pyo Apache 2.0 diff --git a/ros/dynamixel_sdk_examples/CHANGELOG.rst b/ros/dynamixel_sdk_examples/CHANGELOG.rst index 6ce6ab90c..667f676c8 100644 --- a/ros/dynamixel_sdk_examples/CHANGELOG.rst +++ b/ros/dynamixel_sdk_examples/CHANGELOG.rst @@ -2,7 +2,7 @@ Changelog for package dynamixel_sdk_examples ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -3.9.0 (2025-09-09) +4.0.0 (2025-10-14) ------------------ * None diff --git a/ros/dynamixel_sdk_examples/package.xml b/ros/dynamixel_sdk_examples/package.xml index c946b18ab..630a2ce9f 100644 --- a/ros/dynamixel_sdk_examples/package.xml +++ b/ros/dynamixel_sdk_examples/package.xml @@ -1,7 +1,7 @@ dynamixel_sdk_examples - 3.9.0 + 4.0.0 ROS 2 examples using ROBOTIS DYNAMIXEL SDK Apache 2.0 Pyo From 5a4733fa9e3b2059cfaa6370f23f3eecf8132a91 Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Wed, 15 Oct 2025 11:52:41 +0900 Subject: [PATCH 29/37] bump Signed-off-by: Hyungyu Kim --- python/setup.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/setup.py b/python/setup.py index 5e846243b..d0ea4045c 100644 --- a/python/setup.py +++ b/python/setup.py @@ -13,7 +13,7 @@ setup( name='dynamixel_sdk', - version='3.9.0', + version='4.0.0', packages=['dynamixel_sdk'], package_dir={'': 'src'}, license='Apache 2.0', From aaff0f118d52c66ab76e7299caf606a24fa1c191 Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Wed, 15 Oct 2025 14:26:55 +0900 Subject: [PATCH 30/37] remove protocol arg in connector Signed-off-by: Hyungyu Kim --- c++/include/dynamixel_sdk/dynamixel_api/connector.hpp | 2 +- c++/src/dynamixel_sdk/dynamixel_api/connector.cpp | 7 ++----- 2 files changed, 3 insertions(+), 6 deletions(-) diff --git a/c++/include/dynamixel_sdk/dynamixel_api/connector.hpp b/c++/include/dynamixel_sdk/dynamixel_api/connector.hpp index 8fd82caf1..0681cbc50 100644 --- a/c++/include/dynamixel_sdk/dynamixel_api/connector.hpp +++ b/c++/include/dynamixel_sdk/dynamixel_api/connector.hpp @@ -31,7 +31,7 @@ namespace dynamixel class Connector { public: - Connector(const std::string & port_name, float protocol_version, int baud_rate); + Connector(const std::string & port_name, int baud_rate); virtual ~Connector(); diff --git a/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp b/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp index d61ef6ada..7ec53a6e4 100644 --- a/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp +++ b/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp @@ -18,13 +18,10 @@ namespace dynamixel { -Connector::Connector(const std::string & port_name, float protocol_version, int baud_rate) +Connector::Connector(const std::string & port_name, int baud_rate) { - if (protocol_version != 2.0f) { - throw DxlRuntimeError("Only Protocol 2.0 is supported in this Dynamixel API."); - } port_handler_ = std::unique_ptr(PortHandler::getPortHandler(port_name.c_str())); - packet_handler_ = PacketHandler::getPacketHandler(protocol_version); + packet_handler_ = PacketHandler::getPacketHandler(2.0); if (!port_handler_->openPort()) { throw DxlRuntimeError("Failed to open the port!"); From b87a605d80bc759066c22e511deac31624629c9b Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Thu, 16 Oct 2025 10:01:33 +0900 Subject: [PATCH 31/37] add error checking Signed-off-by: Hyungyu Kim --- .../dynamixel_api/dynamixel_error.hpp | 2 + .../dynamixel_api/group_executor.hpp | 4 ++ .../dynamixel_sdk/dynamixel_api/motor.hpp | 9 +++- .../dynamixel_api/group_executor.cpp | 4 -- c++/src/dynamixel_sdk/dynamixel_api/motor.cpp | 49 ++++++++++++++++++- 5 files changed, 61 insertions(+), 7 deletions(-) diff --git a/c++/include/dynamixel_sdk/dynamixel_api/dynamixel_error.hpp b/c++/include/dynamixel_sdk/dynamixel_api/dynamixel_error.hpp index 7a1b87f65..02f9fc100 100644 --- a/c++/include/dynamixel_sdk/dynamixel_api/dynamixel_error.hpp +++ b/c++/include/dynamixel_sdk/dynamixel_api/dynamixel_error.hpp @@ -42,6 +42,8 @@ enum class DxlError SDK_ERRNUM_DATA_LIMIT = 6, // Data limit error SDK_ERRNUM_ACCESS = 7, // Access error API_FUNCTION_NOT_SUPPORTED = 11, // API does not support this function + API_MOTOR_TORQUE_OFF = 12, // Motor torque is off + API_OPERATING_MODE_MISMATCH = 13, // Operating mode is not appropriate for this function API_ADD_PARAM_FAIL = 21, // Failed to add parameter API_COMMAND_IS_EMPTY = 22, // No command to execute API_DUPLICATE_ID = 23, // Duplicate ID in staged commands diff --git a/c++/include/dynamixel_sdk/dynamixel_api/group_executor.hpp b/c++/include/dynamixel_sdk/dynamixel_api/group_executor.hpp index abb12ff89..03a239028 100644 --- a/c++/include/dynamixel_sdk/dynamixel_api/group_executor.hpp +++ b/c++/include/dynamixel_sdk/dynamixel_api/group_executor.hpp @@ -43,6 +43,10 @@ class GroupExecutor void addCmd(Result result); Result executeWrite(); Result>, DxlError> executeRead(); + std::vector getStagedWriteCommands() const { return staged_write_command_list_; } + std::vector getStagedReadCommands() const { return staged_read_command_list_; } + void clearStagedWriteCommands() { staged_write_command_list_.clear(); } + void clearStagedReadCommands() { staged_read_command_list_.clear(); } private: Result executeSyncWrite(uint16_t address, uint16_t length); diff --git a/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp b/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp index 778708f86..6cf25bf94 100644 --- a/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp +++ b/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp @@ -36,9 +36,11 @@ class Motor public: enum class OperatingMode { - POSITION = 3, + CURRENT = 0, VELOCITY = 1, - CURRENT = 0 + POSITION = 3, + EXTENDED_POSITION = 4, + PWM = 16 }; enum class ProfileConfiguration @@ -76,6 +78,7 @@ class Motor Result getVelocityLimit(); Result getCurrentLimit(); Result getPWMLimit(); + Result getOperatingMode(); Result changeID(uint8_t new_id); Result setOperatingMode(OperatingMode mode); @@ -119,6 +122,8 @@ class Motor uint8_t id_; uint16_t model_number_; std::string model_name_; + uint8_t torque_; + uint8_t operating_mode_; Connector * connector_; const std::map & control_table_; diff --git a/c++/src/dynamixel_sdk/dynamixel_api/group_executor.cpp b/c++/src/dynamixel_sdk/dynamixel_api/group_executor.cpp index 591b74938..e624a2bae 100644 --- a/c++/src/dynamixel_sdk/dynamixel_api/group_executor.cpp +++ b/c++/src/dynamixel_sdk/dynamixel_api/group_executor.cpp @@ -95,8 +95,6 @@ Result GroupExecutor::executeWrite() } else { result = executeBulkWrite(); } - - staged_write_command_list_.clear(); return result; } @@ -122,11 +120,9 @@ Result>, DxlError> GroupExecutor::executeR } else { result = executeBulkRead(); } - staged_read_command_list_.clear(); return result; } - Result GroupExecutor::executeSyncWrite(uint16_t address, uint16_t length) { GroupSyncWrite group_sync_write(port_handler_, packet_handler_, address, length); diff --git a/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp b/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp index ec20c1fbe..4ce5e9581 100644 --- a/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp +++ b/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp @@ -25,7 +25,19 @@ Motor::Motor(uint8_t id, uint16_t model_number, Connector * connector) model_name_(ControlTable::getModelName(model_number)), connector_(connector), control_table_(ControlTable::getControlTable(model_number)) -{} +{ + Result torque_result = this->isTorqueOn(); + if (!torque_result.isSuccess()) { + throw DxlRuntimeError("Failed to get torque status: " + getErrorMessage(torque_result.error())); + } + torque_ = torque_result.value(); + + Result mode_result = this->getOperatingMode(); + if (!mode_result.isSuccess()) { + throw DxlRuntimeError("Failed to get operating mode: " + getErrorMessage(mode_result.error())); + } + operating_mode_ = mode_result.value(); +} Motor::~Motor() {} @@ -53,6 +65,12 @@ Result Motor::disableTorque() Result Motor::setGoalPosition(uint32_t position) { + if(torque_ == 0) { + return DxlError::API_MOTOR_TORQUE_OFF; + } + if(operating_mode_ != static_cast(OperatingMode::POSITION)) { + return DxlError::API_OPERATING_MODE_MISMATCH; + } Result item_result = getControlTableItem("Goal Position"); if (!item_result.isSuccess()) { return item_result.error(); @@ -67,6 +85,12 @@ Result Motor::setGoalPosition(uint32_t position) Result Motor::setGoalVelocity(uint32_t velocity) { + if(torque_ == 0) { + return DxlError::API_MOTOR_TORQUE_OFF; + } + if(operating_mode_ != static_cast(OperatingMode::VELOCITY)) { + return DxlError::API_OPERATING_MODE_MISMATCH; + } Result item_result = getControlTableItem("Goal Velocity"); if (!item_result.isSuccess()) { return item_result.error(); @@ -81,6 +105,12 @@ Result Motor::setGoalVelocity(uint32_t velocity) Result Motor::setGoalCurrent(int16_t current) { + if(torque_ == 0) { + return DxlError::API_MOTOR_TORQUE_OFF; + } + if(operating_mode_ != static_cast(OperatingMode::CURRENT)) { + return DxlError::API_OPERATING_MODE_MISMATCH; + } Result item_result = getControlTableItem("Goal Current"); if (!item_result.isSuccess()) { return item_result.error(); @@ -95,6 +125,12 @@ Result Motor::setGoalCurrent(int16_t current) Result Motor::setGoalPWM(int16_t pwm) { + if(torque_ == 0) { + return DxlError::API_MOTOR_TORQUE_OFF; + } + if(operating_mode_ != static_cast(OperatingMode::PWM)) { + return DxlError::API_OPERATING_MODE_MISMATCH; + } Result item_result = getControlTableItem("Goal PWM"); if (!item_result.isSuccess()) { return item_result.error(); @@ -249,6 +285,17 @@ Result Motor::getPWMLimit() return result; } +Result Motor::getOperatingMode() +{ + Result item_result = getControlTableItem("Operating Mode"); + if (!item_result.isSuccess()) { + return item_result.error(); + } + const ControlTableItem & item = item_result.value(); + Result result = connector_->read1ByteData(id_, item.address); + return result; +} + Result Motor::changeID(uint8_t new_id) { Result item_result = getControlTableItem("ID"); From 90d55c66d5876796e1c5aed9b72722bc7d397927 Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Thu, 16 Oct 2025 11:38:50 +0900 Subject: [PATCH 32/37] change file structure Signed-off-by: Hyungyu Kim --- c++/build/linux64/Makefile | 13 ++++++++----- .../dynamixel_api/connector.hpp | 12 ++++++------ .../dynamixel_api/control_table.hpp | 8 ++++---- .../dynamixel_api.hpp | 14 +++++++------- .../dynamixel_api/dynamixel_error.hpp | 6 +++--- .../dynamixel_api/group_executor.hpp | 12 ++++++------ .../{dynamixel_sdk => }/dynamixel_api/motor.hpp | 12 ++++++------ .../dynamixel_api/staged_command.hpp | 6 +++--- .../dynamixel_api/connector.cpp | 2 +- .../dynamixel_api/control_table.cpp | 0 .../dynamixel_api/dynamixel_error.cpp | 2 +- .../dynamixel_api/group_executor.cpp | 0 .../{dynamixel_sdk => }/dynamixel_api/motor.cpp | 4 ++-- 13 files changed, 47 insertions(+), 44 deletions(-) rename c++/include/{dynamixel_sdk => }/dynamixel_api/connector.hpp (85%) rename c++/include/{dynamixel_sdk => }/dynamixel_api/control_table.hpp (82%) rename c++/include/{dynamixel_sdk => dynamixel_api}/dynamixel_api.hpp (64%) rename c++/include/{dynamixel_sdk => }/dynamixel_api/dynamixel_error.hpp (95%) rename c++/include/{dynamixel_sdk => }/dynamixel_api/group_executor.hpp (86%) rename c++/include/{dynamixel_sdk => }/dynamixel_api/motor.hpp (92%) rename c++/include/{dynamixel_sdk => }/dynamixel_api/staged_command.hpp (85%) rename c++/src/{dynamixel_sdk => }/dynamixel_api/connector.cpp (99%) rename c++/src/{dynamixel_sdk => }/dynamixel_api/control_table.cpp (100%) rename c++/src/{dynamixel_sdk => }/dynamixel_api/dynamixel_error.cpp (97%) rename c++/src/{dynamixel_sdk => }/dynamixel_api/group_executor.cpp (100%) rename c++/src/{dynamixel_sdk => }/dynamixel_api/motor.cpp (99%) diff --git a/c++/build/linux64/Makefile b/c++/build/linux64/Makefile index 0a9a5f1e3..479a7a937 100644 --- a/c++/build/linux64/Makefile +++ b/c++/build/linux64/Makefile @@ -62,11 +62,11 @@ SOURCES = src/dynamixel_sdk/group_bulk_read.cpp \ src/dynamixel_sdk/protocol1_packet_handler.cpp \ src/dynamixel_sdk/protocol2_packet_handler.cpp \ src/dynamixel_sdk/port_handler_linux.cpp \ - src/dynamixel_sdk/dynamixel_api/connector.cpp \ - src/dynamixel_sdk/dynamixel_api/control_table.cpp \ - src/dynamixel_sdk/dynamixel_api/motor.cpp \ - src/dynamixel_sdk/dynamixel_api/dynamixel_error.cpp \ - src/dynamixel_sdk/dynamixel_api/group_executor.cpp + src/dynamixel_api/connector.cpp \ + src/dynamixel_api/control_table.cpp \ + src/dynamixel_api/motor.cpp \ + src/dynamixel_api/dynamixel_error.cpp \ + src/dynamixel_api/group_executor.cpp OBJECTS=$(addsuffix .o,$(addprefix $(DIR_OBJS)/,$(basename $(notdir $(SOURCES))))) @@ -96,6 +96,9 @@ install: $(TARGET) @$(CHK_DIR_EXISTS) $(INSTALL_ROOT)/include/dynamixel_sdk || $(MKDIR) $(INSTALL_ROOT)/include/dynamixel_sdk $(CP_ALL) $(DIR_DXL)/include/dynamixel_sdk/* $(INSTALL_ROOT)/include/dynamixel_sdk + @$(CHK_DIR_EXISTS) $(INSTALL_ROOT)/include/dynamixel_api || $(MKDIR) $(INSTALL_ROOT)/include/dynamixel_api + $(CP_ALL) $(DIR_DXL)/include/dynamixel_api/* $(INSTALL_ROOT)/include/dynamixel_api + # copy the control_table directory into the share directory @$(CHK_DIR_EXISTS) $(INSTALL_ROOT)/share/dynamixel_sdk || $(MKDIR) -p $(INSTALL_ROOT)/share/dynamixel_sdk $(CP_ALL) $(DIR_DXL)/../control_table $(INSTALL_ROOT)/share/dynamixel_sdk/ diff --git a/c++/include/dynamixel_sdk/dynamixel_api/connector.hpp b/c++/include/dynamixel_api/connector.hpp similarity index 85% rename from c++/include/dynamixel_sdk/dynamixel_api/connector.hpp rename to c++/include/dynamixel_api/connector.hpp index 0681cbc50..759eb0bb6 100644 --- a/c++/include/dynamixel_sdk/dynamixel_api/connector.hpp +++ b/c++/include/dynamixel_api/connector.hpp @@ -14,17 +14,17 @@ // // Author: Hyungyu Kim -#ifndef DYNAMIXEL_SDK_DYNAMIXEL_API_CONNECTOR_HPP_ -#define DYNAMIXEL_SDK_DYNAMIXEL_API_CONNECTOR_HPP_ +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_CONNECTOR_HPP_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_CONNECTOR_HPP_ #include #include #include #include "dynamixel_sdk/dynamixel_sdk.h" -#include "dynamixel_sdk/dynamixel_api/dynamixel_error.hpp" -#include "dynamixel_sdk/dynamixel_api/motor.hpp" -#include "dynamixel_sdk/dynamixel_api/group_executor.hpp" +#include "dynamixel_api/dynamixel_error.hpp" +#include "dynamixel_api/motor.hpp" +#include "dynamixel_api/group_executor.hpp" namespace dynamixel { @@ -59,4 +59,4 @@ class Connector PacketHandler * packet_handler_; }; } // namespace dynamixel -#endif /* DYNAMIXEL_SDK_DYNAMIXEL_API_CONNECTOR_HPP_ */ +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_CONNECTOR_HPP_ */ diff --git a/c++/include/dynamixel_sdk/dynamixel_api/control_table.hpp b/c++/include/dynamixel_api/control_table.hpp similarity index 82% rename from c++/include/dynamixel_sdk/dynamixel_api/control_table.hpp rename to c++/include/dynamixel_api/control_table.hpp index 3a1ecd837..d38fe7bcb 100644 --- a/c++/include/dynamixel_sdk/dynamixel_api/control_table.hpp +++ b/c++/include/dynamixel_api/control_table.hpp @@ -14,8 +14,8 @@ // // Author: Hyungyu Kim -#ifndef DYNAMIXEL_SDK_DYNAMIXEL_API_CONTROL_TABLE_HPP_ -#define DYNAMIXEL_SDK_DYNAMIXEL_API_CONTROL_TABLE_HPP_ +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_CONTROL_TABLE_HPP_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_CONTROL_TABLE_HPP_ #include #include @@ -24,7 +24,7 @@ #include #include -#include "dynamixel_sdk/dynamixel_api/dynamixel_error.hpp" +#include "dynamixel_api/dynamixel_error.hpp" namespace dynamixel { @@ -42,4 +42,4 @@ class ControlTable static const std::map & getControlTable(uint16_t model_number); }; } // namespace dynamixel -#endif /* DYNAMIXEL_SDK_DYNAMIXEL_API_CONTROL_TABLE_HPP_ */ +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_CONTROL_TABLE_HPP_ */ diff --git a/c++/include/dynamixel_sdk/dynamixel_api.hpp b/c++/include/dynamixel_api/dynamixel_api.hpp similarity index 64% rename from c++/include/dynamixel_sdk/dynamixel_api.hpp rename to c++/include/dynamixel_api/dynamixel_api.hpp index 82aa615bb..f9ad6b85b 100644 --- a/c++/include/dynamixel_sdk/dynamixel_api.hpp +++ b/c++/include/dynamixel_api/dynamixel_api.hpp @@ -14,13 +14,13 @@ // // Author: Hyungyu Kim -#ifndef DYNAMIXEL_SDK_DYNAMIXEL_API_HPP_ -#define DYNAMIXEL_SDK_DYNAMIXEL_API_HPP_ +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_DYNAMIXEL_API_HPP_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_DYNAMIXEL_API_HPP_ -#include "dynamixel_sdk/dynamixel_api/connector.hpp" -#include "dynamixel_sdk/dynamixel_api/dynamixel_error.hpp" -#include "dynamixel_sdk/dynamixel_api/motor.hpp" -#include "dynamixel_sdk/dynamixel_api/group_executor.hpp" +#include "dynamixel_api/connector.hpp" +#include "dynamixel_api/dynamixel_error.hpp" +#include "dynamixel_api/motor.hpp" +#include "dynamixel_api/group_executor.hpp" -#endif /* DYNAMIXEL_SDK_DYNAMIXEL_API_HPP_ */ +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_HPP_ */ diff --git a/c++/include/dynamixel_sdk/dynamixel_api/dynamixel_error.hpp b/c++/include/dynamixel_api/dynamixel_error.hpp similarity index 95% rename from c++/include/dynamixel_sdk/dynamixel_api/dynamixel_error.hpp rename to c++/include/dynamixel_api/dynamixel_error.hpp index 02f9fc100..10b419148 100644 --- a/c++/include/dynamixel_sdk/dynamixel_api/dynamixel_error.hpp +++ b/c++/include/dynamixel_api/dynamixel_error.hpp @@ -14,8 +14,8 @@ // // Author: Hyungyu Kim -#ifndef DYNAMIXEL_SDK_DYNAMIXEL_API_DYNAMIXEL_ERROR_HPP_ -#define DYNAMIXEL_SDK_DYNAMIXEL_API_DYNAMIXEL_ERROR_HPP_ +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_DYNAMIXEL_ERROR_HPP_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_DYNAMIXEL_ERROR_HPP_ #include #include @@ -142,4 +142,4 @@ class DxlRuntimeError : public std::runtime_error std::string getErrorMessage(DxlError error_code); } // namespace dynamixel -#endif /* DYNAMIXEL_SDK_DYNAMIXEL_API_DYNAMIXEL_ERROR_HPP_ */ +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_DYNAMIXEL_ERROR_HPP_ */ diff --git a/c++/include/dynamixel_sdk/dynamixel_api/group_executor.hpp b/c++/include/dynamixel_api/group_executor.hpp similarity index 86% rename from c++/include/dynamixel_sdk/dynamixel_api/group_executor.hpp rename to c++/include/dynamixel_api/group_executor.hpp index 03a239028..f610bf28c 100644 --- a/c++/include/dynamixel_sdk/dynamixel_api/group_executor.hpp +++ b/c++/include/dynamixel_api/group_executor.hpp @@ -14,8 +14,8 @@ // // Author: Hyungyu Kim -#ifndef DYNAMIXEL_SDK_DYNAMIXEL_API_GROUP_EXECUTOR_HPP_ -#define DYNAMIXEL_SDK_DYNAMIXEL_API_GROUP_EXECUTOR_HPP_ +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_GROUP_EXECUTOR_HPP_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_GROUP_EXECUTOR_HPP_ #include #include @@ -25,9 +25,9 @@ #include #include "dynamixel_sdk/dynamixel_sdk.h" -#include "dynamixel_sdk/dynamixel_api/control_table.hpp" -#include "dynamixel_sdk/dynamixel_api/dynamixel_error.hpp" -#include "dynamixel_sdk/dynamixel_api/staged_command.hpp" +#include "dynamixel_api/control_table.hpp" +#include "dynamixel_api/dynamixel_error.hpp" +#include "dynamixel_api/staged_command.hpp" namespace dynamixel { @@ -68,4 +68,4 @@ class GroupExecutor } // namespace dynamixel -#endif // DYNAMIXEL_SDK_DYNAMIXEL_API_GROUP_EXECUTOR_HPP_ +#endif // DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_GROUP_EXECUTOR_HPP_ diff --git a/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp b/c++/include/dynamixel_api/motor.hpp similarity index 92% rename from c++/include/dynamixel_sdk/dynamixel_api/motor.hpp rename to c++/include/dynamixel_api/motor.hpp index 6cf25bf94..e72fadb6c 100644 --- a/c++/include/dynamixel_sdk/dynamixel_api/motor.hpp +++ b/c++/include/dynamixel_api/motor.hpp @@ -14,18 +14,18 @@ // // Author: Hyungyu Kim -#ifndef DYNAMIXEL_SDK_DYNAMIXEL_API_MOTOR_HPP_ -#define DYNAMIXEL_SDK_DYNAMIXEL_API_MOTOR_HPP_ +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_MOTOR_HPP_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_MOTOR_HPP_ #include #include #include #include -#include "dynamixel_sdk/dynamixel_api/control_table.hpp" #include "dynamixel_sdk/dynamixel_sdk.h" -#include "dynamixel_sdk/dynamixel_api/dynamixel_error.hpp" -#include "dynamixel_sdk/dynamixel_api/staged_command.hpp" +#include "dynamixel_api/control_table.hpp" +#include "dynamixel_api/dynamixel_error.hpp" +#include "dynamixel_api/staged_command.hpp" namespace dynamixel { @@ -129,4 +129,4 @@ class Motor const std::map & control_table_; }; } // namespace dynamixel -#endif /* DYNAMIXEL_SDK_DYNAMIXEL_API_MOTOR_HPP_ */ +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_MOTOR_HPP_ */ diff --git a/c++/include/dynamixel_sdk/dynamixel_api/staged_command.hpp b/c++/include/dynamixel_api/staged_command.hpp similarity index 85% rename from c++/include/dynamixel_sdk/dynamixel_api/staged_command.hpp rename to c++/include/dynamixel_api/staged_command.hpp index f7b9ea723..296b927ef 100644 --- a/c++/include/dynamixel_sdk/dynamixel_api/staged_command.hpp +++ b/c++/include/dynamixel_api/staged_command.hpp @@ -14,8 +14,8 @@ // // Author: Hyungyu Kim -#ifndef DYNAMIXEL_SDK_DYNAMIXEL_API_STAGED_COMMAND_HPP_ -#define DYNAMIXEL_SDK_DYNAMIXEL_API_STAGED_COMMAND_HPP_ +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_STAGED_COMMAND_HPP_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_STAGED_COMMAND_HPP_ #include #include @@ -51,4 +51,4 @@ struct StagedCommand } // namespace dynamixel -#endif // DYNAMIXEL_SDK_DYNAMIXEL_API_STAGED_COMMAND_HPP_ +#endif // DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_STAGED_COMMAND_HPP_ diff --git a/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp b/c++/src/dynamixel_api/connector.cpp similarity index 99% rename from c++/src/dynamixel_sdk/dynamixel_api/connector.cpp rename to c++/src/dynamixel_api/connector.cpp index 7ec53a6e4..5d61c8f43 100644 --- a/c++/src/dynamixel_sdk/dynamixel_api/connector.cpp +++ b/c++/src/dynamixel_api/connector.cpp @@ -14,7 +14,7 @@ // // Author: Hyungyu Kim -#include "dynamixel_sdk/dynamixel_api/connector.hpp" +#include "dynamixel_api/connector.hpp" namespace dynamixel { diff --git a/c++/src/dynamixel_sdk/dynamixel_api/control_table.cpp b/c++/src/dynamixel_api/control_table.cpp similarity index 100% rename from c++/src/dynamixel_sdk/dynamixel_api/control_table.cpp rename to c++/src/dynamixel_api/control_table.cpp diff --git a/c++/src/dynamixel_sdk/dynamixel_api/dynamixel_error.cpp b/c++/src/dynamixel_api/dynamixel_error.cpp similarity index 97% rename from c++/src/dynamixel_sdk/dynamixel_api/dynamixel_error.cpp rename to c++/src/dynamixel_api/dynamixel_error.cpp index f636af3f4..29bb27dae 100644 --- a/c++/src/dynamixel_sdk/dynamixel_api/dynamixel_error.cpp +++ b/c++/src/dynamixel_api/dynamixel_error.cpp @@ -14,7 +14,7 @@ // // Author: Hyungyu Kim -#include "dynamixel_sdk/dynamixel_api/dynamixel_error.hpp" +#include "dynamixel_api/dynamixel_error.hpp" namespace dynamixel { diff --git a/c++/src/dynamixel_sdk/dynamixel_api/group_executor.cpp b/c++/src/dynamixel_api/group_executor.cpp similarity index 100% rename from c++/src/dynamixel_sdk/dynamixel_api/group_executor.cpp rename to c++/src/dynamixel_api/group_executor.cpp diff --git a/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp b/c++/src/dynamixel_api/motor.cpp similarity index 99% rename from c++/src/dynamixel_sdk/dynamixel_api/motor.cpp rename to c++/src/dynamixel_api/motor.cpp index 4ce5e9581..d349f0661 100644 --- a/c++/src/dynamixel_sdk/dynamixel_api/motor.cpp +++ b/c++/src/dynamixel_api/motor.cpp @@ -14,8 +14,8 @@ // // Author: Hyungyu Kim -#include "dynamixel_sdk/dynamixel_api/motor.hpp" -#include "dynamixel_sdk/dynamixel_api/connector.hpp" +#include "dynamixel_api/motor.hpp" +#include "dynamixel_api/connector.hpp" namespace dynamixel { From 1f812ea19c7c7174af3352b72149468aed55781f Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Thu, 16 Oct 2025 12:02:32 +0900 Subject: [PATCH 33/37] modified makefile Signed-off-by: Hyungyu Kim --- c++/build/linux64/Makefile | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/c++/build/linux64/Makefile b/c++/build/linux64/Makefile index 479a7a937..427eaa1de 100644 --- a/c++/build/linux64/Makefile +++ b/c++/build/linux64/Makefile @@ -113,6 +113,7 @@ uninstall: $(RM) $(INSTALL_ROOT)/include/dynamixel_sdk/dynamixel_sdk.h $(RM_ALL) $(INSTALL_ROOT)/include/dynamixel_sdk/* + $(RM_ALL) $(INSTALL_ROOT)/include/dynamixel_api/* $(RM_ALL) $(INSTALL_ROOT)/share/dynamixel_sdk/control_table reinstall: uninstall install @@ -134,7 +135,7 @@ $(DIR_OBJS)/%.o: $(DIR_DXL)/src/dynamixel_sdk/%.c $(DIR_OBJS)/%.o: $(DIR_DXL)/src/dynamixel_sdk/%.cpp $(CX) $(CXFLAGS) -c $? -o $@ -$(DIR_OBJS)/%.o: $(DIR_DXL)/src/dynamixel_sdk/dynamixel_api/%.cpp +$(DIR_OBJS)/%.o: $(DIR_DXL)/src/dynamixel_api/%.cpp $(CX) $(CXFLAGS) -c $? -o $@ #--------------------------------------------------------------------- From 0052c5c069135d96bcb7fdbcd8ce39bfd7cf88cc Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Thu, 16 Oct 2025 12:19:13 +0900 Subject: [PATCH 34/37] bump Signed-off-by: Hyungyu Kim --- Doxyfile | 2 +- c++/library.properties | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Doxyfile b/Doxyfile index 8fdd42e69..e68d41661 100644 --- a/Doxyfile +++ b/Doxyfile @@ -38,7 +38,7 @@ PROJECT_NAME = "DynamixelSDK c++" # could be handy for archiving the generated documentation or if some version # control system is used. -PROJECT_NUMBER = 3.9.0 +PROJECT_NUMBER = 4.0.0 # Using the PROJECT_BRIEF tag one can provide an optional one line description # for a project that appears at the top of each page and should give viewer a diff --git a/c++/library.properties b/c++/library.properties index fc2ea3cf7..e0bc01c96 100644 --- a/c++/library.properties +++ b/c++/library.properties @@ -1,5 +1,5 @@ name=DynamixelSDK -version=3.9.0 +version=4.0.0 author=ROBOTIS maintainer=ROBOTIS sentence=DynamixelSDK for Arduino From 160e3b988772a3bf3d74d33c8d1e583d69099b76 Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Thu, 16 Oct 2025 17:53:27 +0900 Subject: [PATCH 35/37] change name of package to Easy SDK Signed-off-by: Hyungyu Kim --- c++/build/linux64/Makefile | 18 ++++---- .../connector.hpp | 12 ++--- .../control_table.hpp | 8 ++-- .../dynamixel_easy_sdk.hpp} | 14 +++--- .../dynamixel_error.hpp | 20 ++++---- .../group_executor.hpp | 12 ++--- .../motor.hpp | 12 ++--- .../staged_command.hpp | 6 +-- .../connector.cpp | 2 +- .../control_table.cpp | 2 +- .../dynamixel_error.cpp | 10 ++-- .../group_executor.cpp | 22 ++++----- .../motor.cpp | 46 +++++++++---------- 13 files changed, 92 insertions(+), 92 deletions(-) rename c++/include/{dynamixel_api => dynamixel_easy_sdk}/connector.hpp (85%) rename c++/include/{dynamixel_api => dynamixel_easy_sdk}/control_table.hpp (81%) rename c++/include/{dynamixel_api/dynamixel_api.hpp => dynamixel_easy_sdk/dynamixel_easy_sdk.hpp} (61%) rename c++/include/{dynamixel_api => dynamixel_easy_sdk}/dynamixel_error.hpp (82%) rename c++/include/{dynamixel_api => dynamixel_easy_sdk}/group_executor.hpp (85%) rename c++/include/{dynamixel_api => dynamixel_easy_sdk}/motor.hpp (92%) rename c++/include/{dynamixel_api => dynamixel_easy_sdk}/staged_command.hpp (84%) rename c++/src/{dynamixel_api => dynamixel_easy_sdk}/connector.cpp (99%) rename c++/src/{dynamixel_api => dynamixel_easy_sdk}/control_table.cpp (98%) rename c++/src/{dynamixel_api => dynamixel_easy_sdk}/dynamixel_error.cpp (92%) rename c++/src/{dynamixel_api => dynamixel_easy_sdk}/group_executor.cpp (91%) rename c++/src/{dynamixel_api => dynamixel_easy_sdk}/motor.cpp (94%) diff --git a/c++/build/linux64/Makefile b/c++/build/linux64/Makefile index 427eaa1de..c81d39933 100644 --- a/c++/build/linux64/Makefile +++ b/c++/build/linux64/Makefile @@ -62,11 +62,11 @@ SOURCES = src/dynamixel_sdk/group_bulk_read.cpp \ src/dynamixel_sdk/protocol1_packet_handler.cpp \ src/dynamixel_sdk/protocol2_packet_handler.cpp \ src/dynamixel_sdk/port_handler_linux.cpp \ - src/dynamixel_api/connector.cpp \ - src/dynamixel_api/control_table.cpp \ - src/dynamixel_api/motor.cpp \ - src/dynamixel_api/dynamixel_error.cpp \ - src/dynamixel_api/group_executor.cpp + src/dynamixel_easy_sdk/connector.cpp \ + src/dynamixel_easy_sdk/control_table.cpp \ + src/dynamixel_easy_sdk/motor.cpp \ + src/dynamixel_easy_sdk/dynamixel_error.cpp \ + src/dynamixel_easy_sdk/group_executor.cpp OBJECTS=$(addsuffix .o,$(addprefix $(DIR_OBJS)/,$(basename $(notdir $(SOURCES))))) @@ -96,8 +96,8 @@ install: $(TARGET) @$(CHK_DIR_EXISTS) $(INSTALL_ROOT)/include/dynamixel_sdk || $(MKDIR) $(INSTALL_ROOT)/include/dynamixel_sdk $(CP_ALL) $(DIR_DXL)/include/dynamixel_sdk/* $(INSTALL_ROOT)/include/dynamixel_sdk - @$(CHK_DIR_EXISTS) $(INSTALL_ROOT)/include/dynamixel_api || $(MKDIR) $(INSTALL_ROOT)/include/dynamixel_api - $(CP_ALL) $(DIR_DXL)/include/dynamixel_api/* $(INSTALL_ROOT)/include/dynamixel_api + @$(CHK_DIR_EXISTS) $(INSTALL_ROOT)/include/dynamixel_easy_sdk || $(MKDIR) $(INSTALL_ROOT)/include/dynamixel_easy_sdk + $(CP_ALL) $(DIR_DXL)/include/dynamixel_easy_sdk/* $(INSTALL_ROOT)/include/dynamixel_easy_sdk # copy the control_table directory into the share directory @$(CHK_DIR_EXISTS) $(INSTALL_ROOT)/share/dynamixel_sdk || $(MKDIR) -p $(INSTALL_ROOT)/share/dynamixel_sdk @@ -113,7 +113,7 @@ uninstall: $(RM) $(INSTALL_ROOT)/include/dynamixel_sdk/dynamixel_sdk.h $(RM_ALL) $(INSTALL_ROOT)/include/dynamixel_sdk/* - $(RM_ALL) $(INSTALL_ROOT)/include/dynamixel_api/* + $(RM_ALL) $(INSTALL_ROOT)/include/dynamixel_easy_sdk/* $(RM_ALL) $(INSTALL_ROOT)/share/dynamixel_sdk/control_table reinstall: uninstall install @@ -135,7 +135,7 @@ $(DIR_OBJS)/%.o: $(DIR_DXL)/src/dynamixel_sdk/%.c $(DIR_OBJS)/%.o: $(DIR_DXL)/src/dynamixel_sdk/%.cpp $(CX) $(CXFLAGS) -c $? -o $@ -$(DIR_OBJS)/%.o: $(DIR_DXL)/src/dynamixel_api/%.cpp +$(DIR_OBJS)/%.o: $(DIR_DXL)/src/dynamixel_easy_sdk/%.cpp $(CX) $(CXFLAGS) -c $? -o $@ #--------------------------------------------------------------------- diff --git a/c++/include/dynamixel_api/connector.hpp b/c++/include/dynamixel_easy_sdk/connector.hpp similarity index 85% rename from c++/include/dynamixel_api/connector.hpp rename to c++/include/dynamixel_easy_sdk/connector.hpp index 759eb0bb6..7bc28f0ae 100644 --- a/c++/include/dynamixel_api/connector.hpp +++ b/c++/include/dynamixel_easy_sdk/connector.hpp @@ -14,17 +14,17 @@ // // Author: Hyungyu Kim -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_CONNECTOR_HPP_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_CONNECTOR_HPP_ +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_EASY_SDK_CONNECTOR_HPP_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_EASY_SDK_CONNECTOR_HPP_ #include #include #include #include "dynamixel_sdk/dynamixel_sdk.h" -#include "dynamixel_api/dynamixel_error.hpp" -#include "dynamixel_api/motor.hpp" -#include "dynamixel_api/group_executor.hpp" +#include "dynamixel_easy_sdk/dynamixel_error.hpp" +#include "dynamixel_easy_sdk/motor.hpp" +#include "dynamixel_easy_sdk/group_executor.hpp" namespace dynamixel { @@ -59,4 +59,4 @@ class Connector PacketHandler * packet_handler_; }; } // namespace dynamixel -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_CONNECTOR_HPP_ */ +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_EASY_SDK_CONNECTOR_HPP_ */ diff --git a/c++/include/dynamixel_api/control_table.hpp b/c++/include/dynamixel_easy_sdk/control_table.hpp similarity index 81% rename from c++/include/dynamixel_api/control_table.hpp rename to c++/include/dynamixel_easy_sdk/control_table.hpp index d38fe7bcb..a9764e151 100644 --- a/c++/include/dynamixel_api/control_table.hpp +++ b/c++/include/dynamixel_easy_sdk/control_table.hpp @@ -14,8 +14,8 @@ // // Author: Hyungyu Kim -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_CONTROL_TABLE_HPP_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_CONTROL_TABLE_HPP_ +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_EASY_SDK_CONTROL_TABLE_HPP_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_EASY_SDK_CONTROL_TABLE_HPP_ #include #include @@ -24,7 +24,7 @@ #include #include -#include "dynamixel_api/dynamixel_error.hpp" +#include "dynamixel_easy_sdk/dynamixel_error.hpp" namespace dynamixel { @@ -42,4 +42,4 @@ class ControlTable static const std::map & getControlTable(uint16_t model_number); }; } // namespace dynamixel -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_CONTROL_TABLE_HPP_ */ +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_EASY_SDK_CONTROL_TABLE_HPP_ */ diff --git a/c++/include/dynamixel_api/dynamixel_api.hpp b/c++/include/dynamixel_easy_sdk/dynamixel_easy_sdk.hpp similarity index 61% rename from c++/include/dynamixel_api/dynamixel_api.hpp rename to c++/include/dynamixel_easy_sdk/dynamixel_easy_sdk.hpp index f9ad6b85b..6536ed707 100644 --- a/c++/include/dynamixel_api/dynamixel_api.hpp +++ b/c++/include/dynamixel_easy_sdk/dynamixel_easy_sdk.hpp @@ -14,13 +14,13 @@ // // Author: Hyungyu Kim -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_DYNAMIXEL_API_HPP_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_DYNAMIXEL_API_HPP_ +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_EASY_SDK_DYNAMIXEL_API_HPP_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_EASY_SDK_DYNAMIXEL_API_HPP_ -#include "dynamixel_api/connector.hpp" -#include "dynamixel_api/dynamixel_error.hpp" -#include "dynamixel_api/motor.hpp" -#include "dynamixel_api/group_executor.hpp" +#include "dynamixel_easy_sdk/connector.hpp" +#include "dynamixel_easy_sdk/dynamixel_error.hpp" +#include "dynamixel_easy_sdk/motor.hpp" +#include "dynamixel_easy_sdk/group_executor.hpp" -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_HPP_ */ +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_EASY_SDK_DYNAMIXEL_API_HPP_ */ diff --git a/c++/include/dynamixel_api/dynamixel_error.hpp b/c++/include/dynamixel_easy_sdk/dynamixel_error.hpp similarity index 82% rename from c++/include/dynamixel_api/dynamixel_error.hpp rename to c++/include/dynamixel_easy_sdk/dynamixel_error.hpp index 10b419148..b6665cbf2 100644 --- a/c++/include/dynamixel_api/dynamixel_error.hpp +++ b/c++/include/dynamixel_easy_sdk/dynamixel_error.hpp @@ -14,8 +14,8 @@ // // Author: Hyungyu Kim -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_DYNAMIXEL_ERROR_HPP_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_DYNAMIXEL_ERROR_HPP_ +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_EASY_SDK_DYNAMIXEL_ERROR_HPP_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_EASY_SDK_DYNAMIXEL_ERROR_HPP_ #include #include @@ -41,13 +41,13 @@ enum class DxlError SDK_ERRNUM_DATA_LENGTH = 5, // Data length error SDK_ERRNUM_DATA_LIMIT = 6, // Data limit error SDK_ERRNUM_ACCESS = 7, // Access error - API_FUNCTION_NOT_SUPPORTED = 11, // API does not support this function - API_MOTOR_TORQUE_OFF = 12, // Motor torque is off - API_OPERATING_MODE_MISMATCH = 13, // Operating mode is not appropriate for this function - API_ADD_PARAM_FAIL = 21, // Failed to add parameter - API_COMMAND_IS_EMPTY = 22, // No command to execute - API_DUPLICATE_ID = 23, // Duplicate ID in staged commands - API_FAIL_TO_GET_DATA = 24 // Failed to get data from motor + EASY_SDK_FUNCTION_NOT_SUPPORTED = 11, // API does not support this function + EASY_SDK_MOTOR_TORQUE_OFF = 12, // Motor torque is off + EASY_SDK_OPERATING_MODE_MISMATCH = 13, // Operating mode is not appropriate for this function + EASY_SDK_ADD_PARAM_FAIL = 21, // Failed to add parameter + EASY_SDK_COMMAND_IS_EMPTY = 22, // No command to execute + EASY_SDK_DUPLICATE_ID = 23, // Duplicate ID in staged commands + EASY_SDK_FAIL_TO_GET_DATA = 24 // Failed to get data from motor }; template @@ -142,4 +142,4 @@ class DxlRuntimeError : public std::runtime_error std::string getErrorMessage(DxlError error_code); } // namespace dynamixel -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_DYNAMIXEL_ERROR_HPP_ */ +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_EASY_SDK_DYNAMIXEL_ERROR_HPP_ */ diff --git a/c++/include/dynamixel_api/group_executor.hpp b/c++/include/dynamixel_easy_sdk/group_executor.hpp similarity index 85% rename from c++/include/dynamixel_api/group_executor.hpp rename to c++/include/dynamixel_easy_sdk/group_executor.hpp index f610bf28c..fe1aebf85 100644 --- a/c++/include/dynamixel_api/group_executor.hpp +++ b/c++/include/dynamixel_easy_sdk/group_executor.hpp @@ -14,8 +14,8 @@ // // Author: Hyungyu Kim -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_GROUP_EXECUTOR_HPP_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_GROUP_EXECUTOR_HPP_ +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_EASY_SDK_GROUP_EXECUTOR_HPP_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_EASY_SDK_GROUP_EXECUTOR_HPP_ #include #include @@ -25,9 +25,9 @@ #include #include "dynamixel_sdk/dynamixel_sdk.h" -#include "dynamixel_api/control_table.hpp" -#include "dynamixel_api/dynamixel_error.hpp" -#include "dynamixel_api/staged_command.hpp" +#include "dynamixel_easy_sdk/control_table.hpp" +#include "dynamixel_easy_sdk/dynamixel_error.hpp" +#include "dynamixel_easy_sdk/staged_command.hpp" namespace dynamixel { @@ -68,4 +68,4 @@ class GroupExecutor } // namespace dynamixel -#endif // DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_GROUP_EXECUTOR_HPP_ +#endif // DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_EASY_SDK_GROUP_EXECUTOR_HPP_ diff --git a/c++/include/dynamixel_api/motor.hpp b/c++/include/dynamixel_easy_sdk/motor.hpp similarity index 92% rename from c++/include/dynamixel_api/motor.hpp rename to c++/include/dynamixel_easy_sdk/motor.hpp index e72fadb6c..12b3d7f20 100644 --- a/c++/include/dynamixel_api/motor.hpp +++ b/c++/include/dynamixel_easy_sdk/motor.hpp @@ -14,8 +14,8 @@ // // Author: Hyungyu Kim -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_MOTOR_HPP_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_MOTOR_HPP_ +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_EASY_SDK_MOTOR_HPP_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_EASY_SDK_MOTOR_HPP_ #include #include @@ -23,9 +23,9 @@ #include #include "dynamixel_sdk/dynamixel_sdk.h" -#include "dynamixel_api/control_table.hpp" -#include "dynamixel_api/dynamixel_error.hpp" -#include "dynamixel_api/staged_command.hpp" +#include "dynamixel_easy_sdk/control_table.hpp" +#include "dynamixel_easy_sdk/dynamixel_error.hpp" +#include "dynamixel_easy_sdk/staged_command.hpp" namespace dynamixel { @@ -129,4 +129,4 @@ class Motor const std::map & control_table_; }; } // namespace dynamixel -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_MOTOR_HPP_ */ +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_EASY_SDK_MOTOR_HPP_ */ diff --git a/c++/include/dynamixel_api/staged_command.hpp b/c++/include/dynamixel_easy_sdk/staged_command.hpp similarity index 84% rename from c++/include/dynamixel_api/staged_command.hpp rename to c++/include/dynamixel_easy_sdk/staged_command.hpp index 296b927ef..43a990495 100644 --- a/c++/include/dynamixel_api/staged_command.hpp +++ b/c++/include/dynamixel_easy_sdk/staged_command.hpp @@ -14,8 +14,8 @@ // // Author: Hyungyu Kim -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_STAGED_COMMAND_HPP_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_STAGED_COMMAND_HPP_ +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_EASY_SDK_STAGED_COMMAND_HPP_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_EASY_SDK_STAGED_COMMAND_HPP_ #include #include @@ -51,4 +51,4 @@ struct StagedCommand } // namespace dynamixel -#endif // DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_API_STAGED_COMMAND_HPP_ +#endif // DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_EASY_SDK_STAGED_COMMAND_HPP_ diff --git a/c++/src/dynamixel_api/connector.cpp b/c++/src/dynamixel_easy_sdk/connector.cpp similarity index 99% rename from c++/src/dynamixel_api/connector.cpp rename to c++/src/dynamixel_easy_sdk/connector.cpp index 5d61c8f43..637cd6640 100644 --- a/c++/src/dynamixel_api/connector.cpp +++ b/c++/src/dynamixel_easy_sdk/connector.cpp @@ -14,7 +14,7 @@ // // Author: Hyungyu Kim -#include "dynamixel_api/connector.hpp" +#include "dynamixel_easy_sdk/connector.hpp" namespace dynamixel { diff --git a/c++/src/dynamixel_api/control_table.cpp b/c++/src/dynamixel_easy_sdk/control_table.cpp similarity index 98% rename from c++/src/dynamixel_api/control_table.cpp rename to c++/src/dynamixel_easy_sdk/control_table.cpp index 0439bcdf7..c94890fed 100644 --- a/c++/src/dynamixel_api/control_table.cpp +++ b/c++/src/dynamixel_easy_sdk/control_table.cpp @@ -16,7 +16,7 @@ #include -#include "dynamixel_api/control_table.hpp" +#include "dynamixel_easy_sdk/control_table.hpp" namespace dynamixel { diff --git a/c++/src/dynamixel_api/dynamixel_error.cpp b/c++/src/dynamixel_easy_sdk/dynamixel_error.cpp similarity index 92% rename from c++/src/dynamixel_api/dynamixel_error.cpp rename to c++/src/dynamixel_easy_sdk/dynamixel_error.cpp index 29bb27dae..ce213207f 100644 --- a/c++/src/dynamixel_api/dynamixel_error.cpp +++ b/c++/src/dynamixel_easy_sdk/dynamixel_error.cpp @@ -14,7 +14,7 @@ // // Author: Hyungyu Kim -#include "dynamixel_api/dynamixel_error.hpp" +#include "dynamixel_easy_sdk/dynamixel_error.hpp" namespace dynamixel { @@ -53,13 +53,13 @@ std::string getErrorMessage(DxlError error_code) return "[RxPacketError] The data value exceeds the limit value!"; case DxlError::SDK_ERRNUM_ACCESS: return "[RxPacketError] Writing or Reading is not available to target address!"; - case DxlError::API_FUNCTION_NOT_SUPPORTED: + case DxlError::EASY_SDK_FUNCTION_NOT_SUPPORTED: return "[APIError] API function is not supported on this model!"; - case DxlError::API_ADD_PARAM_FAIL: + case DxlError::EASY_SDK_ADD_PARAM_FAIL: return "[APIError] Failed to add parameter!"; - case DxlError::API_COMMAND_IS_EMPTY: + case DxlError::EASY_SDK_COMMAND_IS_EMPTY: return "[APIError] No command to execute!"; - case DxlError::API_DUPLICATE_ID: + case DxlError::EASY_SDK_DUPLICATE_ID: return "[APIError] Duplicate ID found in staged commands."; default: return "Unknown Error"; } diff --git a/c++/src/dynamixel_api/group_executor.cpp b/c++/src/dynamixel_easy_sdk/group_executor.cpp similarity index 91% rename from c++/src/dynamixel_api/group_executor.cpp rename to c++/src/dynamixel_easy_sdk/group_executor.cpp index e624a2bae..3f9dd7cad 100644 --- a/c++/src/dynamixel_api/group_executor.cpp +++ b/c++/src/dynamixel_easy_sdk/group_executor.cpp @@ -16,8 +16,8 @@ #include -#include "dynamixel_api/group_executor.hpp" -#include "dynamixel_api/connector.hpp" +#include "dynamixel_easy_sdk/group_executor.hpp" +#include "dynamixel_easy_sdk/connector.hpp" namespace dynamixel { @@ -55,7 +55,7 @@ void GroupExecutor::addCmd(Result result) Result GroupExecutor::executeWrite() { if (staged_write_command_list_.empty()) { - return DxlError::API_COMMAND_IS_EMPTY; + return DxlError::EASY_SDK_COMMAND_IS_EMPTY; } const auto & reference_command = staged_write_command_list_.front(); bool is_sync = true; @@ -77,7 +77,7 @@ Result GroupExecutor::executeWrite() ); if (it != sorted_list.end()) { - return DxlError::API_DUPLICATE_ID; + return DxlError::EASY_SDK_DUPLICATE_ID; } for (size_t i = 1; i < staged_write_command_list_.size(); ++i) { @@ -101,7 +101,7 @@ Result GroupExecutor::executeWrite() Result>, DxlError> GroupExecutor::executeRead() { if (staged_read_command_list_.empty()) { - return DxlError::API_COMMAND_IS_EMPTY; + return DxlError::EASY_SDK_COMMAND_IS_EMPTY; } const auto & reference_command = staged_read_command_list_.front(); bool is_sync = true; @@ -128,7 +128,7 @@ Result GroupExecutor::executeSyncWrite(uint16_t address, uint16_ GroupSyncWrite group_sync_write(port_handler_, packet_handler_, address, length); for (auto & command : staged_write_command_list_) { if (!group_sync_write.addParam(command.id, command.data.data())) { - return DxlError::API_ADD_PARAM_FAIL; + return DxlError::EASY_SDK_ADD_PARAM_FAIL; } } int dxl_comm_result = group_sync_write.txPacket(); @@ -149,7 +149,7 @@ Result GroupExecutor::executeBulkWrite() command.length, command.data.data())) { - return DxlError::API_ADD_PARAM_FAIL; + return DxlError::EASY_SDK_ADD_PARAM_FAIL; } } @@ -167,7 +167,7 @@ Result>, DxlError> GroupExecutor::executeS GroupSyncRead group_sync_read(port_handler_, packet_handler_, address, length); for (const auto & command : staged_read_command_list_) { if (!group_sync_read.addParam(command.id)) { - return DxlError::API_ADD_PARAM_FAIL; + return DxlError::EASY_SDK_ADD_PARAM_FAIL; } } @@ -178,7 +178,7 @@ Result>, DxlError> GroupExecutor::executeS std::vector> result_list; for (const auto & command : staged_read_command_list_) { if (!group_sync_read.isAvailable(command.id, address, length)) { - result_list.push_back(DxlError::API_FAIL_TO_GET_DATA); + result_list.push_back(DxlError::EASY_SDK_FAIL_TO_GET_DATA); continue; } int32_t value = group_sync_read.getData(command.id, address, length); @@ -194,7 +194,7 @@ Result>, DxlError> GroupExecutor::executeB for (const auto & command : staged_read_command_list_) { if (!group_bulk_read_.addParam(command.id, command.address, command.length)) { - return DxlError::API_ADD_PARAM_FAIL; + return DxlError::EASY_SDK_ADD_PARAM_FAIL; } } @@ -206,7 +206,7 @@ Result>, DxlError> GroupExecutor::executeB std::vector> result_list; for (const auto & command : staged_read_command_list_) { if (!group_bulk_read_.isAvailable(command.id, command.address, command.length)) { - result_list.push_back(DxlError::API_FAIL_TO_GET_DATA); + result_list.push_back(DxlError::EASY_SDK_FAIL_TO_GET_DATA); continue; } diff --git a/c++/src/dynamixel_api/motor.cpp b/c++/src/dynamixel_easy_sdk/motor.cpp similarity index 94% rename from c++/src/dynamixel_api/motor.cpp rename to c++/src/dynamixel_easy_sdk/motor.cpp index d349f0661..e169a0171 100644 --- a/c++/src/dynamixel_api/motor.cpp +++ b/c++/src/dynamixel_easy_sdk/motor.cpp @@ -14,8 +14,8 @@ // // Author: Hyungyu Kim -#include "dynamixel_api/motor.hpp" -#include "dynamixel_api/connector.hpp" +#include "dynamixel_easy_sdk/motor.hpp" +#include "dynamixel_easy_sdk/connector.hpp" namespace dynamixel { @@ -66,10 +66,10 @@ Result Motor::disableTorque() Result Motor::setGoalPosition(uint32_t position) { if(torque_ == 0) { - return DxlError::API_MOTOR_TORQUE_OFF; + return DxlError::EASY_SDK_MOTOR_TORQUE_OFF; } if(operating_mode_ != static_cast(OperatingMode::POSITION)) { - return DxlError::API_OPERATING_MODE_MISMATCH; + return DxlError::EASY_SDK_OPERATING_MODE_MISMATCH; } Result item_result = getControlTableItem("Goal Position"); if (!item_result.isSuccess()) { @@ -77,7 +77,7 @@ Result Motor::setGoalPosition(uint32_t position) } const ControlTableItem & item = item_result.value(); if (item.size != 4) { - return DxlError::API_FUNCTION_NOT_SUPPORTED; + return DxlError::EASY_SDK_FUNCTION_NOT_SUPPORTED; } Result result = connector_->write4ByteData(id_, item.address, position); return result; @@ -86,10 +86,10 @@ Result Motor::setGoalPosition(uint32_t position) Result Motor::setGoalVelocity(uint32_t velocity) { if(torque_ == 0) { - return DxlError::API_MOTOR_TORQUE_OFF; + return DxlError::EASY_SDK_MOTOR_TORQUE_OFF; } if(operating_mode_ != static_cast(OperatingMode::VELOCITY)) { - return DxlError::API_OPERATING_MODE_MISMATCH; + return DxlError::EASY_SDK_OPERATING_MODE_MISMATCH; } Result item_result = getControlTableItem("Goal Velocity"); if (!item_result.isSuccess()) { @@ -97,7 +97,7 @@ Result Motor::setGoalVelocity(uint32_t velocity) } const ControlTableItem & item = item_result.value(); if (item.size != 4) { - return DxlError::API_FUNCTION_NOT_SUPPORTED; + return DxlError::EASY_SDK_FUNCTION_NOT_SUPPORTED; } Result result = connector_->write4ByteData(id_, item.address, velocity); return result; @@ -106,10 +106,10 @@ Result Motor::setGoalVelocity(uint32_t velocity) Result Motor::setGoalCurrent(int16_t current) { if(torque_ == 0) { - return DxlError::API_MOTOR_TORQUE_OFF; + return DxlError::EASY_SDK_MOTOR_TORQUE_OFF; } if(operating_mode_ != static_cast(OperatingMode::CURRENT)) { - return DxlError::API_OPERATING_MODE_MISMATCH; + return DxlError::EASY_SDK_OPERATING_MODE_MISMATCH; } Result item_result = getControlTableItem("Goal Current"); if (!item_result.isSuccess()) { @@ -117,7 +117,7 @@ Result Motor::setGoalCurrent(int16_t current) } const ControlTableItem & item = item_result.value(); if (item.size != 2) { - return DxlError::API_FUNCTION_NOT_SUPPORTED; + return DxlError::EASY_SDK_FUNCTION_NOT_SUPPORTED; } Result result = connector_->write2ByteData(id_, item.address, current); return result; @@ -126,10 +126,10 @@ Result Motor::setGoalCurrent(int16_t current) Result Motor::setGoalPWM(int16_t pwm) { if(torque_ == 0) { - return DxlError::API_MOTOR_TORQUE_OFF; + return DxlError::EASY_SDK_MOTOR_TORQUE_OFF; } if(operating_mode_ != static_cast(OperatingMode::PWM)) { - return DxlError::API_OPERATING_MODE_MISMATCH; + return DxlError::EASY_SDK_OPERATING_MODE_MISMATCH; } Result item_result = getControlTableItem("Goal PWM"); if (!item_result.isSuccess()) { @@ -137,7 +137,7 @@ Result Motor::setGoalPWM(int16_t pwm) } const ControlTableItem & item = item_result.value(); if (item.size != 2) { - return DxlError::API_FUNCTION_NOT_SUPPORTED; + return DxlError::EASY_SDK_FUNCTION_NOT_SUPPORTED; } Result result = connector_->write2ByteData(id_, item.address, pwm); return result; @@ -229,7 +229,7 @@ Result Motor::getMaxPositionLimit() } const ControlTableItem & item = item_result.value(); if (item.size != 4) { - return DxlError::API_FUNCTION_NOT_SUPPORTED; + return DxlError::EASY_SDK_FUNCTION_NOT_SUPPORTED; } Result result = connector_->read4ByteData(id_, item.address); return result; @@ -243,7 +243,7 @@ Result Motor::getMinPositionLimit() } const ControlTableItem & item = item_result.value(); if (item.size != 4) { - return DxlError::API_FUNCTION_NOT_SUPPORTED; + return DxlError::EASY_SDK_FUNCTION_NOT_SUPPORTED; } Result result = connector_->read4ByteData(id_, item.address); return result; @@ -257,7 +257,7 @@ Result Motor::getVelocityLimit() } const ControlTableItem & item = item_result.value(); if (item.size != 4) { - return DxlError::API_FUNCTION_NOT_SUPPORTED; + return DxlError::EASY_SDK_FUNCTION_NOT_SUPPORTED; } Result result = connector_->read4ByteData(id_, item.address); return result; @@ -379,7 +379,7 @@ Result Motor::setPositionPGain(uint16_t p_gain) } const ControlTableItem & item = item_result.value(); if (item.size != 2) { - return DxlError::API_FUNCTION_NOT_SUPPORTED; + return DxlError::EASY_SDK_FUNCTION_NOT_SUPPORTED; } Result result = connector_->write2ByteData(id_, item.address, p_gain); return result; @@ -393,7 +393,7 @@ Result Motor::setPositionIGain(uint16_t i_gain) } const ControlTableItem & item = item_result.value(); if (item.size != 2) { - return DxlError::API_FUNCTION_NOT_SUPPORTED; + return DxlError::EASY_SDK_FUNCTION_NOT_SUPPORTED; } Result result = connector_->write2ByteData(id_, item.address, i_gain); return result; @@ -407,7 +407,7 @@ Result Motor::setPositionDGain(uint16_t d_gain) } const ControlTableItem & item = item_result.value(); if (item.size != 2) { - return DxlError::API_FUNCTION_NOT_SUPPORTED; + return DxlError::EASY_SDK_FUNCTION_NOT_SUPPORTED; } Result result = connector_->write2ByteData(id_, item.address, d_gain); return result; @@ -421,7 +421,7 @@ Result Motor::setVelocityPGain(uint16_t p_gain) } const ControlTableItem & item = item_result.value(); if (item.size != 2) { - return DxlError::API_FUNCTION_NOT_SUPPORTED; + return DxlError::EASY_SDK_FUNCTION_NOT_SUPPORTED; } Result result = connector_->write2ByteData(id_, item.address, p_gain); return result; @@ -435,7 +435,7 @@ Result Motor::setVelocityIGain(uint16_t i_gain) } const ControlTableItem & item = item_result.value(); if (item.size != 2) { - return DxlError::API_FUNCTION_NOT_SUPPORTED; + return DxlError::EASY_SDK_FUNCTION_NOT_SUPPORTED; } Result result = connector_->write2ByteData(id_, item.address, i_gain); return result; @@ -704,7 +704,7 @@ Result Motor::getControlTableItem(const std::string { auto it = control_table_.find(name); if (it == control_table_.end()) { - return DxlError::API_FUNCTION_NOT_SUPPORTED; + return DxlError::EASY_SDK_FUNCTION_NOT_SUPPORTED; } return it->second; } From 0a4f97146e52b7722284c3a45d8fd917c4193cee Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Tue, 21 Oct 2025 09:34:58 +0900 Subject: [PATCH 36/37] reflect review Signed-off-by: Hyungyu Kim --- c++/src/dynamixel_easy_sdk/connector.cpp | 4 +++- c++/src/dynamixel_easy_sdk/dynamixel_error.cpp | 10 +++++----- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/c++/src/dynamixel_easy_sdk/connector.cpp b/c++/src/dynamixel_easy_sdk/connector.cpp index 637cd6640..6c15b1b8b 100644 --- a/c++/src/dynamixel_easy_sdk/connector.cpp +++ b/c++/src/dynamixel_easy_sdk/connector.cpp @@ -16,12 +16,14 @@ #include "dynamixel_easy_sdk/connector.hpp" +static constexpr float PROTOCOL_VERSION = 2.0f; + namespace dynamixel { Connector::Connector(const std::string & port_name, int baud_rate) { port_handler_ = std::unique_ptr(PortHandler::getPortHandler(port_name.c_str())); - packet_handler_ = PacketHandler::getPacketHandler(2.0); + packet_handler_ = PacketHandler::getPacketHandler(PROTOCOL_VERSION); if (!port_handler_->openPort()) { throw DxlRuntimeError("Failed to open the port!"); diff --git a/c++/src/dynamixel_easy_sdk/dynamixel_error.cpp b/c++/src/dynamixel_easy_sdk/dynamixel_error.cpp index ce213207f..246e40e90 100644 --- a/c++/src/dynamixel_easy_sdk/dynamixel_error.cpp +++ b/c++/src/dynamixel_easy_sdk/dynamixel_error.cpp @@ -38,7 +38,7 @@ std::string getErrorMessage(DxlError error_code) case DxlError::SDK_COMM_RX_CORRUPT: return "[TxRxResult] Incorrect status packet!"; case DxlError::SDK_COMM_NOT_AVAILABLE: - return "[TxRxResult] Protocol does not support This function!"; + return "[TxRxResult] Protocol does not support this function!"; case DxlError::SDK_ERRNUM_RESULT_FAIL: return "[RxPacketError] Failed to process the instruction packet!"; case DxlError::SDK_ERRNUM_INSTRUCTION: @@ -54,13 +54,13 @@ std::string getErrorMessage(DxlError error_code) case DxlError::SDK_ERRNUM_ACCESS: return "[RxPacketError] Writing or Reading is not available to target address!"; case DxlError::EASY_SDK_FUNCTION_NOT_SUPPORTED: - return "[APIError] API function is not supported on this model!"; + return "[EasySDKError] Easy SDK function is not supported on this model!"; case DxlError::EASY_SDK_ADD_PARAM_FAIL: - return "[APIError] Failed to add parameter!"; + return "[EasySDKError] Failed to add parameter!"; case DxlError::EASY_SDK_COMMAND_IS_EMPTY: - return "[APIError] No command to execute!"; + return "[EasySDKError] No command to execute!"; case DxlError::EASY_SDK_DUPLICATE_ID: - return "[APIError] Duplicate ID found in staged commands."; + return "[EasySDKError] Duplicate ID found in staged commands."; default: return "Unknown Error"; } } From 693076c7710ea10885beea1929f698190e097171 Mon Sep 17 00:00:00 2001 From: Hyungyu Kim Date: Tue, 21 Oct 2025 10:27:10 +0900 Subject: [PATCH 37/37] reflect review Signed-off-by: Hyungyu Kim --- ReleaseNote.md | 2 +- c++/include/dynamixel_easy_sdk/dynamixel_easy_sdk.hpp | 6 +++--- c++/src/dynamixel_easy_sdk/dynamixel_error.cpp | 8 +++++++- 3 files changed, 11 insertions(+), 5 deletions(-) diff --git a/ReleaseNote.md b/ReleaseNote.md index 3feb4e670..69a89f8d7 100644 --- a/ReleaseNote.md +++ b/ReleaseNote.md @@ -2,7 +2,7 @@ 4.0.0 (2025-10-14) ------------------ -* Update Dynamixel API in C++ Linux 64bit +* Update Dynamixel Easy SDK in C++ Linux 64bit * Contributors: Hyungyu Kim 3.8.4 (2025-05-28) diff --git a/c++/include/dynamixel_easy_sdk/dynamixel_easy_sdk.hpp b/c++/include/dynamixel_easy_sdk/dynamixel_easy_sdk.hpp index 6536ed707..38b9a4f36 100644 --- a/c++/include/dynamixel_easy_sdk/dynamixel_easy_sdk.hpp +++ b/c++/include/dynamixel_easy_sdk/dynamixel_easy_sdk.hpp @@ -14,8 +14,8 @@ // // Author: Hyungyu Kim -#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_EASY_SDK_DYNAMIXEL_API_HPP_ -#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_EASY_SDK_DYNAMIXEL_API_HPP_ +#ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_EASY_SDK_DYNAMIXEL_EASY_SDK_HPP_ +#define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_EASY_SDK_DYNAMIXEL_EASY_SDK_HPP_ #include "dynamixel_easy_sdk/connector.hpp" @@ -23,4 +23,4 @@ #include "dynamixel_easy_sdk/motor.hpp" #include "dynamixel_easy_sdk/group_executor.hpp" -#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_EASY_SDK_DYNAMIXEL_API_HPP_ */ +#endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_EASY_SDK_DYNAMIXEL_EASY_SDK_HPP_ */ diff --git a/c++/src/dynamixel_easy_sdk/dynamixel_error.cpp b/c++/src/dynamixel_easy_sdk/dynamixel_error.cpp index 246e40e90..d90ed6f68 100644 --- a/c++/src/dynamixel_easy_sdk/dynamixel_error.cpp +++ b/c++/src/dynamixel_easy_sdk/dynamixel_error.cpp @@ -32,7 +32,7 @@ std::string getErrorMessage(DxlError error_code) case DxlError::SDK_COMM_TX_ERROR: return "[TxRxResult] Incorrect instruction packet!"; case DxlError::SDK_COMM_RX_WAITING: - return "[TxRxResult] Now recieving status packet!"; + return "[TxRxResult] Now receiving status packet!"; case DxlError::SDK_COMM_RX_TIMEOUT: return "[TxRxResult] There is no status packet!"; case DxlError::SDK_COMM_RX_CORRUPT: @@ -55,12 +55,18 @@ std::string getErrorMessage(DxlError error_code) return "[RxPacketError] Writing or Reading is not available to target address!"; case DxlError::EASY_SDK_FUNCTION_NOT_SUPPORTED: return "[EasySDKError] Easy SDK function is not supported on this model!"; + case DxlError::EASY_SDK_MOTOR_TORQUE_OFF: + return "[EasySDKError] Motor torque is off!"; + case DxlError::EASY_SDK_OPERATING_MODE_MISMATCH: + return "[EasySDKError] Operating mode is not appropriate for this function!"; case DxlError::EASY_SDK_ADD_PARAM_FAIL: return "[EasySDKError] Failed to add parameter!"; case DxlError::EASY_SDK_COMMAND_IS_EMPTY: return "[EasySDKError] No command to execute!"; case DxlError::EASY_SDK_DUPLICATE_ID: return "[EasySDKError] Duplicate ID found in staged commands."; + case DxlError::EASY_SDK_FAIL_TO_GET_DATA: + return "[EasySDKError] Failed to get data from motor."; default: return "Unknown Error"; } }