Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add reg_multi_turn_goal_position_angle function #9

Merged
merged 4 commits into from
Jan 16, 2025
Merged

Add reg_multi_turn_goal_position_angle function #9

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
6284936
Add reg_multi_turn_goal_position_angle function
Tacha-S Dec 10, 2024
31efb4a
Add XH540W150
Tacha-S Jan 15, 2025
38c70ce
Add const variable
Tacha-S Jan 16, 2025
e083ff7
Fix comment
Tacha-S Jan 16, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions src/dynamixel/auto_detect.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -83,6 +83,8 @@ namespace dynamixel {
return std::make_shared<servos::Xh430V210>(id);
case servos::Xh430V350::ct_t::model_number_value:
return std::make_shared<servos::Xh430V350>(id);
case servos::Xh540W150::ct_t::model_number_value:
return std::make_shared<servos::Xh540W150>(id);
default:
throw errors::Error("Unrecognized model number");
}
Expand Down
1 change: 1 addition & 0 deletions src/dynamixel/servos.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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"
Expand Down
5 changes: 5 additions & 0 deletions src/dynamixel/servos/base_servo.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -188,6 +188,11 @@ namespace dynamixel {
throw errors::Error("reg_goal_position_angle not implemented in model");
}

virtual InstructionPacket<protocol_t> reg_multi_turn_goal_position_angle(double rad) const
{
throw errors::Error("reg_multi_turn_goal_position_angle not implemented in model");
}

virtual InstructionPacket<protocol_t> get_present_position_angle() const
{
throw errors::Error("get_present_position_angle not implemented in model");
Expand Down
19 changes: 19 additions & 0 deletions src/dynamixel/servos/servo.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -172,6 +172,21 @@ namespace dynamixel {
return reg_goal_position(id, pos);
}

static inline InstructionPacket<protocol_t> reg_multi_turn_goal_position_angle(typename Servo<Model>::protocol_t::id_t id, double rad)
{
double deg = rad * 57.2958;
const double limit = 92160.0; // 256 * 360
if (!(deg >= -limit && deg <= limit)) // 256 * 360
throw errors::ServoLimitError(
id,
-limit * 0.01745,
limit * 0.01745,
rad
);
typename ct_t::goal_position_t pos = deg * 11.37778; // * resolution [pulse/rev] / 360 deg
return reg_goal_position(id, pos);
}

InstructionPacket<protocol_t> set_goal_position_angle(double rad) const override
{
return Model::set_goal_position_angle(this->_id, rad);
Expand All @@ -182,6 +197,10 @@ namespace dynamixel {
return Model::reg_goal_position_angle(this->_id, rad);
}

InstructionPacket<protocol_t> reg_multi_turn_goal_position_angle(double rad) const override {
return Model::reg_multi_turn_goal_position_angle(this->_id, rad);
}

static InstructionPacket<typename Servo<Model>::protocol_t> get_present_position_angle(typename Servo<Model>::protocol_t::id_t id)
{
return get_present_position(id);
Expand Down
209 changes: 209 additions & 0 deletions src/dynamixel/servos/xh540_w150.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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 <stdint.h>

#include "servo.hpp"
#include "../protocols/protocol2.hpp"

namespace dynamixel {
namespace servos {
class Xh540W150;

template <>
struct ModelTraits<Xh540W150> {
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<Xh540W150> {
public:
typedef Xh540W150 Model;

Xh540W150(typename protocol_t::id_t id) : Servo<Xh540W150>(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<protocol_t> reboot(typename Servo<Model>::protocol_t::id_t id)
{
return reboot_t(id);
}

InstructionPacket<protocol_t> reboot() const override
{
return reboot_t(this->_id);
}
};
}
}

#endif