XH430、XH540、PH42クラスを追加 (#22)

* xh430, xh540クラスを追加

* ジョイントクラスにXHシリーズのインスタンス生成を追加

* PH42クラスを追加

* dynamixel_pのテストを追加

rt_manipulators_lib/include/dynamixel_p.hpp

@@ -0,0 +1,130 @@
+// Copyright 2022 RT Corporation
+//
+// 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 RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_P_HPP_
+#define RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_P_HPP_
+
+#include <string>
+#include <vector>
+
+#include "dynamixel_base.hpp"
+
+namespace dynamixel_p {
+
+class DynamixelP : public dynamixel_base::DynamixelBase  {
+ public:
+  explicit DynamixelP(const uint8_t id, const int home_position = 0);
+
+  bool read_operating_mode(const dynamixel_base::comm_t & comm, uint8_t & mode);
+  bool write_operating_mode(const dynamixel_base::comm_t & comm, const uint8_t mode);
+  bool read_current_limit(const dynamixel_base::comm_t & comm, double & limit_ampere);
+  bool read_max_position_limit(const dynamixel_base::comm_t & comm, double & limit_radian);
+  bool read_min_position_limit(const dynamixel_base::comm_t & comm, double & limit_radian);
+
+  bool write_torque_enable(const dynamixel_base::comm_t & comm, const bool enable);
+
+  bool write_velocity_i_gain(const dynamixel_base::comm_t & comm, const unsigned int gain);
+  bool write_velocity_p_gain(const dynamixel_base::comm_t & comm, const unsigned int gain);
+  bool write_position_d_gain(const dynamixel_base::comm_t & comm, const unsigned int gain);
+  bool write_position_i_gain(const dynamixel_base::comm_t & comm, const unsigned int gain);
+  bool write_position_p_gain(const dynamixel_base::comm_t & comm, const unsigned int gain);
+
+  bool write_profile_acceleration(
+    const dynamixel_base::comm_t & comm, const double acceleration_rpss);
+  bool write_profile_velocity(
+    const dynamixel_base::comm_t & comm, const double velocity_rps);
+
+  unsigned int to_profile_acceleration(const double acceleration_rpss);
+  unsigned int to_profile_velocity(const double velocity_rps);
+  double to_position_radian(const int position);
+  double to_velocity_rps(const int velocity);
+  double to_current_ampere(const int current);
+  double to_voltage_volt(const int voltage);
+  unsigned int from_position_radian(const double position_rad);
+  unsigned int from_velocity_rps(const double velocity_rps);
+  unsigned int from_current_ampere(const double current_ampere);
+
+  bool auto_set_indirect_address_of_present_position(const dynamixel_base::comm_t & comm);
+  bool auto_set_indirect_address_of_present_velocity(const dynamixel_base::comm_t & comm);
+  bool auto_set_indirect_address_of_present_current(const dynamixel_base::comm_t & comm);
+  bool auto_set_indirect_address_of_present_input_voltage(const dynamixel_base::comm_t & comm);
+  bool auto_set_indirect_address_of_present_temperature(const dynamixel_base::comm_t & comm);
+  bool auto_set_indirect_address_of_goal_position(const dynamixel_base::comm_t & comm);
+  bool auto_set_indirect_address_of_goal_velocity(const dynamixel_base::comm_t & comm);
+  bool auto_set_indirect_address_of_goal_current(const dynamixel_base::comm_t & comm);
+
+  unsigned int indirect_addr_of_present_position(void);
+  unsigned int indirect_addr_of_present_velocity(void);
+  unsigned int indirect_addr_of_present_current(void);
+  unsigned int indirect_addr_of_present_input_voltage(void);
+  unsigned int indirect_addr_of_present_temperature(void);
+  unsigned int indirect_addr_of_goal_position(void);
+  unsigned int indirect_addr_of_goal_velocity(void);
+  unsigned int indirect_addr_of_goal_current(void);
+
+  unsigned int start_address_for_indirect_read(void);
+  unsigned int length_of_indirect_data_read(void);
+  unsigned int next_indirect_addr_read(void) const;
+
+  unsigned int start_address_for_indirect_write(void);
+  unsigned int length_of_indirect_data_write(void);
+  unsigned int next_indirect_addr_write(void) const;
+
+  bool extract_present_position_from_sync_read(
+    const dynamixel_base::comm_t & comm, const std::string & group_name,
+    double & position_rad);
+  bool extract_present_velocity_from_sync_read(
+    const dynamixel_base::comm_t & comm, const std::string & group_name,
+    double & velocity_rps);
+  bool extract_present_current_from_sync_read(
+    const dynamixel_base::comm_t & comm, const std::string & group_name,
+    double & current_ampere);
+  bool extract_present_input_voltage_from_sync_read(
+    const dynamixel_base::comm_t & comm, const std::string & group_name,
+    double & voltage_volt);
+  bool extract_present_temperature_from_sync_read(
+    const dynamixel_base::comm_t & comm, const std::string & group_name,
+    int & temperature_deg);
+
+  void push_back_position_for_sync_write(
+    const double position_rad, std::vector<uint8_t> & write_data);
+  void push_back_velocity_for_sync_write(
+    const double velocity_rps, std::vector<uint8_t> & write_data);
+  void push_back_current_for_sync_write(
+    const double current_ampere, std::vector<uint8_t> & write_data);
+
+ protected:
+  int HOME_POSITION_;
+  unsigned int total_length_of_indirect_addr_read_;
+  unsigned int total_length_of_indirect_addr_write_;
+  uint16_t indirect_addr_of_present_position_;
+  uint16_t indirect_addr_of_present_velocity_;
+  uint16_t indirect_addr_of_present_current_;
+  uint16_t indirect_addr_of_present_input_voltage_;
+  uint16_t indirect_addr_of_present_temperature_;
+  uint16_t indirect_addr_of_goal_position_;
+  uint16_t indirect_addr_of_goal_velocity_;
+  uint16_t indirect_addr_of_goal_current_;
+
+  bool set_indirect_address_read(
+    const dynamixel_base::comm_t & comm, const uint16_t addr, const uint16_t len,
+    uint16_t & indirect_addr);
+  bool set_indirect_address_write(
+    const dynamixel_base::comm_t & comm, const uint16_t addr, const uint16_t len,
+    uint16_t & indirect_addr);
+};
+
+}  // namespace dynamixel_p
+
+#endif  // RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_P_HPP_

rt_manipulators_lib/include/dynamixel_ph42.hpp

@@ -0,0 +1,29 @@
+// Copyright 2022 RT Corporation
+//
+// 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 RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_PH42_HPP_
+#define RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_PH42_HPP_
+
+#include "dynamixel_p.hpp"
+
+namespace dynamixel_ph42 {
+
+class DynamixelPH42 : public dynamixel_p::DynamixelP {
+ public:
+  explicit DynamixelPH42(const uint8_t id);
+};
+
+}  // namespace dynamixel_ph42
+
+#endif  // RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_PH42_HPP_

rt_manipulators_lib/include/dynamixel_xh430.hpp

@@ -0,0 +1,31 @@
+// Copyright 2022 RT Corporation
+//
+// 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 RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_XH430_HPP_
+#define RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_XH430_HPP_
+
+#include "dynamixel_x.hpp"
+
+namespace dynamixel_xh430 {
+
+class DynamixelXH430 : public dynamixel_x::DynamixelX {
+ public:
+  explicit DynamixelXH430(const uint8_t id);
+  double to_current_ampere(const int current) override;
+  unsigned int from_current_ampere(const double current_ampere) override;
+};
+
+}  // namespace dynamixel_xh430
+
+#endif  // RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_XH430_HPP_

rt_manipulators_lib/include/dynamixel_xh540.hpp

@@ -0,0 +1,29 @@
+// Copyright 2022 RT Corporation
+//
+// 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 RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_XH540_HPP_
+#define RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_XH540_HPP_
+
+#include "dynamixel_x.hpp"
+
+namespace dynamixel_xh540 {
+
+class DynamixelXH540 : public dynamixel_x::DynamixelX {
+ public:
+  explicit DynamixelXH540(const uint8_t id);
+};
+
+}  // namespace dynamixel_xh540
+
+#endif  // RT_MANIPULATORS_LIB_INCLUDE_DYNAMIXEL_XH540_HPP_

rt_manipulators_lib/src/CMakeLists.txt

@@ -15,6 +15,10 @@ add_library(${library_name}
     dynamixel_x.cpp
     dynamixel_xm430.cpp
     dynamixel_xm540.cpp
+    dynamixel_xh430.cpp
+    dynamixel_xh540.cpp
+    dynamixel_p.cpp
+    dynamixel_ph42.cpp
 )
 set_target_properties(${library_name} PROPERTIES VERSION 1.1.0 SOVERSION 1)