From 628493626bd8d2c5dfb195b4a10e58de7c9bbb34 Mon Sep 17 00:00:00 2001 From: Tatsuro Sakaguchi Date: Tue, 10 Dec 2024 19:10:21 +0900 Subject: [PATCH 1/4] Add reg_multi_turn_goal_position_angle function --- src/dynamixel/servos/base_servo.hpp | 5 +++++ src/dynamixel/servos/servo.hpp | 18 ++++++++++++++++++ 2 files changed, 23 insertions(+) diff --git a/src/dynamixel/servos/base_servo.hpp b/src/dynamixel/servos/base_servo.hpp index cb5b3ce..836a606 100644 --- a/src/dynamixel/servos/base_servo.hpp +++ b/src/dynamixel/servos/base_servo.hpp @@ -188,6 +188,11 @@ namespace dynamixel { throw errors::Error("reg_goal_position_angle not implemented in model"); } + virtual InstructionPacket reg_multi_turn_goal_position_angle(double rad) const + { + throw errors::Error("reg_multi_turn_goal_position_angle not implemented in model"); + } + virtual InstructionPacket get_present_position_angle() const { throw errors::Error("get_present_position_angle not implemented in model"); diff --git a/src/dynamixel/servos/servo.hpp b/src/dynamixel/servos/servo.hpp index cd12d26..c526c8c 100644 --- a/src/dynamixel/servos/servo.hpp +++ b/src/dynamixel/servos/servo.hpp @@ -172,6 +172,20 @@ namespace dynamixel { return reg_goal_position(id, pos); } + static inline InstructionPacket reg_multi_turn_goal_position_angle(typename Servo::protocol_t::id_t id, double rad) + { + double deg = rad * 57.2958; + if (!(deg >= -92160 && deg <= 92160)) // 256 * 360 + throw errors::ServoLimitError( + id, + -92160.0 * 0.01745, + 92160.0 * 0.01745, + rad + ); + typename ct_t::goal_position_t pos = deg * 11.37778; // * 360 / 4096 + return reg_goal_position(id, pos); + } + InstructionPacket set_goal_position_angle(double rad) const override { return Model::set_goal_position_angle(this->_id, rad); @@ -182,6 +196,10 @@ namespace dynamixel { return Model::reg_goal_position_angle(this->_id, rad); } + InstructionPacket reg_multi_turn_goal_position_angle(double rad) const override { + return Model::reg_multi_turn_goal_position_angle(this->_id, rad); + } + static InstructionPacket::protocol_t> get_present_position_angle(typename Servo::protocol_t::id_t id) { return get_present_position(id); From 31efb4afa7fd5f0e1b881cfa429b2e4cd5ff54d4 Mon Sep 17 00:00:00 2001 From: Tatsuro Sakaguchi Date: Wed, 15 Jan 2025 18:37:20 +0900 Subject: [PATCH 2/4] Add XH540W150 --- src/dynamixel/auto_detect.hpp | 2 + src/dynamixel/servos.hpp | 1 + src/dynamixel/servos/xh540_w150.hpp | 209 ++++++++++++++++++++++++++++ 3 files changed, 212 insertions(+) create mode 100644 src/dynamixel/servos/xh540_w150.hpp diff --git a/src/dynamixel/auto_detect.hpp b/src/dynamixel/auto_detect.hpp index 6da701c..fffc674 100644 --- a/src/dynamixel/auto_detect.hpp +++ b/src/dynamixel/auto_detect.hpp @@ -83,6 +83,8 @@ namespace dynamixel { return std::make_shared(id); case servos::Xh430V350::ct_t::model_number_value: return std::make_shared(id); + case servos::Xh540W150::ct_t::model_number_value: + return std::make_shared(id); default: throw errors::Error("Unrecognized model number"); } diff --git a/src/dynamixel/servos.hpp b/src/dynamixel/servos.hpp index 0923784..b2272a7 100644 --- a/src/dynamixel/servos.hpp +++ b/src/dynamixel/servos.hpp @@ -22,6 +22,7 @@ #include "servos/xh430_w350.hpp" #include "servos/xh430_v210.hpp" #include "servos/xh430_v350.hpp" +#include "servos/xh540_w150.hpp" #include "servos/pro_h54_200_s500.hpp" #include "servos/pro_h54_100_s500.hpp" #include "servos/pro_h42_20_s300.hpp" diff --git a/src/dynamixel/servos/xh540_w150.hpp b/src/dynamixel/servos/xh540_w150.hpp new file mode 100644 index 0000000..260a229 --- /dev/null +++ b/src/dynamixel/servos/xh540_w150.hpp @@ -0,0 +1,209 @@ +/********************************************************************* + * Copyright (c) 2025 SoftBank Corp. + * + * 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. + * + ********************************************************************/ + +#ifndef DYNAMIXEL_SERVOS_XH540W150_HPP_ +#define DYNAMIXEL_SERVOS_XH540W150_HPP_ + +#include + +#include "servo.hpp" +#include "../protocols/protocol2.hpp" + +namespace dynamixel { + namespace servos { + class Xh540W150; + + template <> + struct ModelTraits { + typedef protocols::Protocol2 protocol_t; + + struct CT { + static const protocol_t::address_t model_number = 0; + typedef uint16_t model_number_t; + static const model_number_t model_number_value = 1110; + static const protocol_t::address_t model_info = 2; + typedef uint32_t model_info_t; + static const protocol_t::address_t firmware_version = 6; + typedef uint8_t firmware_version_t; + static const protocol_t::address_t id = 7; + typedef protocol_t::id_t id_t; + static const protocol_t::address_t baudrate = 8; + typedef uint8_t baudrate_t; + static const protocol_t::address_t return_delay_time = 9; + typedef uint8_t return_delay_time_t; + static const protocol_t::address_t drive_mode = 10; + typedef uint8_t drive_mode_t; + static const protocol_t::address_t operating_mode = 11; + typedef uint8_t operating_mode_t; + static const protocol_t::address_t secondary_id = 12; + typedef uint8_t secondary_id_t; + static const protocol_t::address_t protocol_version = 13; + typedef uint8_t protocol_version_t; + static const protocol_t::address_t homing_offset = 20; + typedef uint32_t homing_offset_t; + static const protocol_t::address_t moving_threshold = 24; + typedef uint32_t moving_threshold_t; + static const protocol_t::address_t highest_temperature_limit = 31; + typedef uint8_t highest_temperature_limit_t; + static const protocol_t::address_t highest_voltage_limit = 32; + typedef uint16_t highest_voltage_limit_t; + static const protocol_t::address_t lowest_voltage_limit = 34; + typedef uint16_t lowest_voltage_limit_t; + static const protocol_t::address_t pwm_limit = 36; + typedef uint16_t pwm_limit_t; + static const protocol_t::address_t current_limit = 38; + typedef uint16_t current_limit_t; + static const protocol_t::address_t acceleration_limit = 40; + typedef uint32_t acceleration_limit_t; + static const protocol_t::address_t speed_limit = 44; + typedef uint32_t speed_limit_t; + static const protocol_t::address_t max_position_limit = 48; + typedef int32_t max_position_limit_t; + static const protocol_t::address_t min_position_limit = 52; + typedef int32_t min_position_limit_t; + static const protocol_t::address_t alarm_shutdown = 63; + typedef uint8_t alarm_shutdown_t; + static const protocol_t::address_t torque_enable = 64; + typedef uint8_t torque_enable_t; + static const protocol_t::address_t led = 65; + typedef uint8_t led_t; + static const protocol_t::address_t status_return_level = 68; + typedef uint8_t status_return_level_t; + static const protocol_t::address_t registered = 69; + typedef uint8_t registered_t; + static const protocol_t::address_t hardware_error_status = 70; + typedef uint8_t hardware_error_status_t; + static const protocol_t::address_t velocity_i_gain = 76; + typedef uint16_t velocity_i_gain_t; + static const protocol_t::address_t velocity_p_gain = 78; + typedef uint16_t velocity_p_gain_t; + static const protocol_t::address_t position_d_gain = 80; + typedef uint16_t position_d_gain_t; + static const protocol_t::address_t position_i_gain = 82; + typedef uint16_t position_i_gain_t; + static const protocol_t::address_t position_p_gain = 84; + typedef uint16_t position_p_gain_t; + static const protocol_t::address_t feedforward_2nd_gain = 88; + typedef uint16_t feedforward_2nd_gain_t; + static const protocol_t::address_t feedforward_1st_gain = 90; + typedef uint16_t feedforward_1st_gain_t; + static const protocol_t::address_t bus_watchdog = 98; + typedef uint8_t bus_watchdog_t; + static const protocol_t::address_t goal_pwm = 100; + typedef uint16_t goal_pwm_t; + static const protocol_t::address_t goal_current = 102; + typedef uint16_t goal_current_t; + static const protocol_t::address_t moving_speed = 104; + typedef uint32_t moving_speed_t; + static const moving_speed_t min_goal_speed = INT32_MIN; + static const moving_speed_t max_goal_speed = INT32_MAX; + static constexpr double rpm_per_tick = 0.229; + static const bool speed_sign_bit = false; + static const protocol_t::address_t profile_acceleration = 108; + typedef uint32_t profile_acceleration_t; + static const protocol_t::address_t profile_speed = 112; + typedef uint32_t profile_speed_t; + static const protocol_t::address_t goal_position = 116; + typedef int32_t goal_position_t; + static const goal_position_t min_goal_position = 0; + static const goal_position_t max_goal_position = 4095; + static const uint32_t min_goal_angle_deg = 0; + static const uint32_t max_goal_angle_deg = 360; + static const protocol_t::address_t realtime_tick = 120; + typedef uint16_t realtime_tick_t; + static const protocol_t::address_t moving = 122; + typedef uint8_t moving_t; + static const protocol_t::address_t moving_status = 123; + typedef uint8_t moving_status_t; + static const protocol_t::address_t present_pwm = 124; + typedef uint16_t present_pwm_t; + static const protocol_t::address_t current = 126; + typedef uint16_t current_t; + static const protocol_t::address_t present_speed = 128; + typedef int32_t present_speed_t; + static const protocol_t::address_t present_position = 132; + typedef int32_t present_position_t; + static const protocol_t::address_t speed_trajectory = 136; + typedef int32_t speed_trajectory_t; + static const protocol_t::address_t position_trajectory = 140; + typedef int32_t position_trajectory_t; + static const protocol_t::address_t present_voltage = 144; + typedef uint16_t present_voltage_t; + static const protocol_t::address_t present_temperature = 146; + typedef uint8_t present_temperature_t; + }; + }; + + class Xh540W150 : public Servo { + public: + typedef Xh540W150 Model; + + Xh540W150(typename protocol_t::id_t id) : Servo(id){}; + + MODEL_NAME(Xh540W150); + + // Here we add the fields that are not common to all dynamixels + READ_FIELD(model_info); + READ_WRITE_FIELD(drive_mode); + READ_WRITE_FIELD(operating_mode); + READ_WRITE_FIELD(secondary_id); + READ_WRITE_FIELD(protocol_version); + READ_WRITE_FIELD(homing_offset); + READ_WRITE_FIELD(moving_threshold); + READ_WRITE_FIELD(pwm_limit); + READ_WRITE_FIELD(current_limit); + READ_WRITE_FIELD(acceleration_limit); + READ_WRITE_FIELD(speed_limit); + READ_WRITE_FIELD(max_position_limit); + READ_WRITE_FIELD(min_position_limit); + READ_WRITE_FIELD(led); + READ_FIELD(registered); + READ_FIELD(hardware_error_status); + READ_WRITE_FIELD(velocity_i_gain); + READ_WRITE_FIELD(velocity_p_gain); + READ_WRITE_FIELD(position_d_gain); + READ_WRITE_FIELD(position_i_gain); + READ_WRITE_FIELD(position_p_gain); + READ_WRITE_FIELD(feedforward_2nd_gain); + READ_WRITE_FIELD(feedforward_1st_gain); + READ_WRITE_FIELD(bus_watchdog); + READ_WRITE_FIELD(goal_pwm); + READ_WRITE_FIELD(goal_current); + READ_WRITE_FIELD(profile_acceleration); + READ_WRITE_FIELD(profile_speed); + READ_FIELD(realtime_tick); + READ_FIELD(moving_status); + READ_FIELD(present_pwm); + // READ_FIELD(current); + READ_FIELD(speed_trajectory); + READ_FIELD(position_trajectory); + + // rebooting functions only for Protocol2 + static inline InstructionPacket reboot(typename Servo::protocol_t::id_t id) + { + return reboot_t(id); + } + + InstructionPacket reboot() const override + { + return reboot_t(this->_id); + } + }; + } +} + +#endif From 38c70cef42d35ad5d1b61b1c11f8c076e4454dcb Mon Sep 17 00:00:00 2001 From: Tatsuro Sakaguchi Date: Thu, 16 Jan 2025 16:03:53 +0900 Subject: [PATCH 3/4] Add const variable --- src/dynamixel/servos/servo.hpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/dynamixel/servos/servo.hpp b/src/dynamixel/servos/servo.hpp index c526c8c..9b2a7d1 100644 --- a/src/dynamixel/servos/servo.hpp +++ b/src/dynamixel/servos/servo.hpp @@ -175,11 +175,12 @@ namespace dynamixel { static inline InstructionPacket reg_multi_turn_goal_position_angle(typename Servo::protocol_t::id_t id, double rad) { double deg = rad * 57.2958; - if (!(deg >= -92160 && deg <= 92160)) // 256 * 360 + const double limit = 92160.0; // 256 * 360 + if (!(deg >= -limit && deg <= limit)) // 256 * 360 throw errors::ServoLimitError( id, - -92160.0 * 0.01745, - 92160.0 * 0.01745, + -limit * 0.01745, + limit * 0.01745, rad ); typename ct_t::goal_position_t pos = deg * 11.37778; // * 360 / 4096 From e083ff7546f4dfa2c3410efc168eaff76da0acce Mon Sep 17 00:00:00 2001 From: Tatsuro Sakaguchi Date: Thu, 16 Jan 2025 16:06:32 +0900 Subject: [PATCH 4/4] Fix comment --- src/dynamixel/servos/servo.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/dynamixel/servos/servo.hpp b/src/dynamixel/servos/servo.hpp index 9b2a7d1..17c9f00 100644 --- a/src/dynamixel/servos/servo.hpp +++ b/src/dynamixel/servos/servo.hpp @@ -183,7 +183,7 @@ namespace dynamixel { limit * 0.01745, rad ); - typename ct_t::goal_position_t pos = deg * 11.37778; // * 360 / 4096 + typename ct_t::goal_position_t pos = deg * 11.37778; // * resolution [pulse/rev] / 360 deg return reg_goal_position(id, pos); }