From 4752c830b9ccb06e8c177c92bbc552603be890b9 Mon Sep 17 00:00:00 2001 From: ggh Date: Mon, 21 Aug 2023 23:06:17 +0900 Subject: [PATCH 1/6] nav2 bt --- witmotion_ros | 1 - .../launch/robot_description.launch.py | 32 ++--- .../yacyac_camera/qr_detector_node.hpp | 2 +- yacyac_core/CMakeLists.txt | 59 ++++++-- yacyac_core/bt_xml/bt_nav_mememan.xml | 26 ++++ yacyac_core/{bt_tree => bt_xml}/default.xml | 0 yacyac_core/include/yacyac_core/message.hpp | 3 +- yacyac_core/include/yacyac_core/qr_client.hpp | 8 +- yacyac_core/src/bt_ros2.cpp | 50 +++++++ yacyac_core/src/main.cpp | 12 +- yacyac_core/src/message.cpp | 6 +- yacyac_core/src/nav2_client.cpp | 136 ++++++++++++++++++ yacyac_core/src/pub.cpp | 44 ------ yacyac_core/src/qr_client.cpp | 11 +- yacyac_core/src/send_goal.cpp | 98 +++++++++++++ yacyac_core/src/sub.cpp | 31 ---- yacyac_io/yacyac_io/env/lib64 | 1 + yacyac_io/yacyac_io/env/pyvenv.cfg | 3 + 18 files changed, 395 insertions(+), 128 deletions(-) delete mode 160000 witmotion_ros create mode 100644 yacyac_core/bt_xml/bt_nav_mememan.xml rename yacyac_core/{bt_tree => bt_xml}/default.xml (100%) create mode 100644 yacyac_core/src/bt_ros2.cpp create mode 100644 yacyac_core/src/nav2_client.cpp delete mode 100644 yacyac_core/src/pub.cpp create mode 100644 yacyac_core/src/send_goal.cpp delete mode 100644 yacyac_core/src/sub.cpp create mode 120000 yacyac_io/yacyac_io/env/lib64 create mode 100644 yacyac_io/yacyac_io/env/pyvenv.cfg diff --git a/witmotion_ros b/witmotion_ros deleted file mode 160000 index 7252d44..0000000 --- a/witmotion_ros +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 7252d44be51062b3cfecb015c9e61365d04a5f00 diff --git a/yacyac_bringup/launch/robot_description.launch.py b/yacyac_bringup/launch/robot_description.launch.py index c7daa73..4fcff2d 100644 --- a/yacyac_bringup/launch/robot_description.launch.py +++ b/yacyac_bringup/launch/robot_description.launch.py @@ -11,10 +11,10 @@ def generate_launch_description(): # s2lidar package - # g2lidar_prefix = get_package_share_directory("ydlidar_ros2_driver") - # start_g2lidar_cmd = IncludeLaunchDescription( - # PythonLaunchDescriptionSource(os.path.join(g2lidar_prefix, "launch", "ydlidar_launch.py")) - # ) + g2lidar_prefix = get_package_share_directory("ydlidar_ros2_driver") + start_g2lidar_cmd = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(g2lidar_prefix, "launch", "ydlidar_launch.py")) + ) # tracer mini package yacyac_prefix = get_package_share_directory("md") @@ -22,16 +22,16 @@ def generate_launch_description(): PythonLaunchDescriptionSource(os.path.join(yacyac_prefix, "launch", "md.launch.py")) ) - # tracer mini package - yacyac_camera_prefix = get_package_share_directory("yacyac_camera") - start_yacyac_camera_cmd = IncludeLaunchDescription( - PythonLaunchDescriptionSource(os.path.join(yacyac_camera_prefix, "launch", "camera.launch.py")) - ) + # # tracer mini package + # yacyac_camera_prefix = get_package_share_directory("yacyac_camera") + # start_yacyac_camera_cmd = IncludeLaunchDescription( + # PythonLaunchDescriptionSource(os.path.join(yacyac_camera_prefix, "launch", "camera.launch.py")) + # ) - # rosbridge websocket cmd - start_web_bridge_cmd = ExecuteProcess( - cmd=["ros2", "launch", "rosbridge_server", "rosbridge_websocket_launch.xml"], output="screen" - ) + # # rosbridge websocket cmd + # start_web_bridge_cmd = ExecuteProcess( + # cmd=["ros2", "launch", "rosbridge_server", "rosbridge_websocket_launch.xml"], output="screen" + # ) # start_insta360_cmd = ExecuteProcess( # cmd=["ros2", "run", "insta360_node", "insta360_node"], output="screen" @@ -79,9 +79,9 @@ def generate_launch_description(): [ base_to_laser_publisher, base_to_imu_publisher, - # start_g2lidar_cmd, + start_g2lidar_cmd, start_yacyac_cmd, - start_web_bridge_cmd, - start_yacyac_camera_cmd + # start_web_bridge_cmd, + # start_yacyac_camera_cmd ] ) diff --git a/yacyac_camera/include/yacyac_camera/qr_detector_node.hpp b/yacyac_camera/include/yacyac_camera/qr_detector_node.hpp index ce3d2aa..777b17c 100644 --- a/yacyac_camera/include/yacyac_camera/qr_detector_node.hpp +++ b/yacyac_camera/include/yacyac_camera/qr_detector_node.hpp @@ -15,7 +15,7 @@ class QrDetectorNode final : public rclcpp::Node { RCLCPP_INFO(this->get_logger(), "QR DETECTOR NODE CREATE"); scanner_.set_config(zbar::ZBAR_NONE, zbar::ZBAR_CFG_ENABLE, 1); - sub_ = this->create_subscription(input_topic_name, rclcpp::SensorDataQoS(), std::bind(&QrDetectorNode::image_callback_, this, std::placeholders::_1)); + this->create_subscription(input_topic_name, rclcpp::SensorDataQoS(), std::bind(&QrDetectorNode::image_callback_, this, std::placeholders::_1)); publisher_ = this->create_publisher(output, rclcpp::QoS(1)); diff --git a/yacyac_core/CMakeLists.txt b/yacyac_core/CMakeLists.txt index b0f55a9..ff7d9f9 100644 --- a/yacyac_core/CMakeLists.txt +++ b/yacyac_core/CMakeLists.txt @@ -14,36 +14,69 @@ include_directories(include) find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) +find_package(rclcpp_action REQUIRED) find_package(std_msgs REQUIRED) find_package(behaviortree_cpp REQUIRED) +find_package(nav2_behavior_tree REQUIRED) +find_package(nav2_msgs REQUIRED) find_package(sensor_msgs REQUIRED) -find_package(std_msgs REQUIRED) find_package(geometry_msgs REQUIRED) find_package(tf2 REQUIRED) -find_package(nav_msgs REQUIRED) + find_package(yacyac_interface REQUIRED) find_package(tf2_ros REQUIRED) + +set(DEPENDENCIES + rclcpp + rclcpp_action + std_msgs + geometry_msgs + nav2_behavior_tree + nav2_msgs + behaviortree_cpp + ${OTHER_DEPS} +) + + add_library( ${PROJECT_NAME}_lib SHARED src/qr_client.cpp src/message.cpp + src/nav2_client.cpp ) -ament_target_dependencies(${PROJECT_NAME}_lib - rclcpp std_msgs behaviortree_cpp yacyac_interface +# send_goal +add_executable(send_goal + src/send_goal.cpp +) +ament_target_dependencies(send_goal + ${DEPENDENCIES} ) +# ----------------------------- -add_executable(talker src/pub.cpp) -add_executable(listener src/sub.cpp) +# bt_ros2 +add_executable(bt_ros2 + src/bt_ros2.cpp +) +# ament_target_dependencies(bt_ros2 +# ${DEPENDENCIES} +# ) +# ----------------------------- + + +# add_executable(send_goal src/send_goal.cpp) add_executable(${PROJECT_NAME} src/main.cpp) -ament_target_dependencies(talker rclcpp std_msgs) -ament_target_dependencies(listener rclcpp std_msgs) -ament_target_dependencies(${PROJECT_NAME} rclcpp std_msgs behaviortree_cpp yacyac_interface) +ament_target_dependencies(${PROJECT_NAME}_lib + rclcpp std_msgs behaviortree_cpp yacyac_interface nav2_behavior_tree nav2_msgs +) + +ament_target_dependencies(send_goal rclcpp std_msgs rclcpp_action geometry_msgs nav2_msgs) target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_lib) +target_link_libraries(bt_ros2 ${PROJECT_NAME}_lib) install(TARGETS ${PROJECT_NAME}_lib ARCHIVE DESTINATION lib @@ -52,14 +85,14 @@ install(TARGETS ${PROJECT_NAME}_lib ) install(DIRECTORY - bt_tree + bt_xml DESTINATION share/${PROJECT_NAME} ) install(TARGETS - talker - listener + send_goal + bt_ros2 ${PROJECT_NAME} DESTINATION lib/${PROJECT_NAME}) -ament_package() \ No newline at end of file +ament_package() diff --git a/yacyac_core/bt_xml/bt_nav_mememan.xml b/yacyac_core/bt_xml/bt_nav_mememan.xml new file mode 100644 index 0000000..4f3aef0 --- /dev/null +++ b/yacyac_core/bt_xml/bt_nav_mememan.xml @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/yacyac_core/bt_tree/default.xml b/yacyac_core/bt_xml/default.xml similarity index 100% rename from yacyac_core/bt_tree/default.xml rename to yacyac_core/bt_xml/default.xml diff --git a/yacyac_core/include/yacyac_core/message.hpp b/yacyac_core/include/yacyac_core/message.hpp index e96dd33..510eeb8 100644 --- a/yacyac_core/include/yacyac_core/message.hpp +++ b/yacyac_core/include/yacyac_core/message.hpp @@ -6,7 +6,7 @@ class Message : public BT::SyncActionNode { public: Message(const std::string& name, const BT::NodeConfig& config) : BT::SyncActionNode(name, config) {} - ~Message() {}; + ~Message(){}; static BT::PortsList providedPorts() { @@ -15,6 +15,7 @@ class Message : public BT::SyncActionNode { } BT::NodeStatus tick() override; + private: }; diff --git a/yacyac_core/include/yacyac_core/qr_client.hpp b/yacyac_core/include/yacyac_core/qr_client.hpp index 8b46eb0..05b566e 100644 --- a/yacyac_core/include/yacyac_core/qr_client.hpp +++ b/yacyac_core/include/yacyac_core/qr_client.hpp @@ -1,8 +1,8 @@ #ifndef QR_CLIENT_HPP_ #define QR_CLIENT_HPP_ -#include #include +#include #include "yacyac_interface/msg/qrcode.hpp" @@ -11,10 +11,7 @@ class QRClient : public BT::StatefulActionNode { QRClient(const std::string& name, const BT::NodeConfig& config); ~QRClient(); - static BT::PortsList providedPorts() - { - return {}; - } + static BT::PortsList providedPorts() { return {}; } // this function is invoked once at the beginning. BT::NodeStatus onStart() override; @@ -24,6 +21,7 @@ class QRClient : public BT::StatefulActionNode { BT::NodeStatus onRunning() override; void onHalted() override; + private: rclcpp::Node::SharedPtr node_; rclcpp::Subscription::SharedPtr qr_sub_; diff --git a/yacyac_core/src/bt_ros2.cpp b/yacyac_core/src/bt_ros2.cpp new file mode 100644 index 0000000..c99f88c --- /dev/null +++ b/yacyac_core/src/bt_ros2.cpp @@ -0,0 +1,50 @@ +#include "nav2_client.cpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include +#include +#include + +using namespace BT; + +int main(int argc, char** argv) +{ + setvbuf(stdout, NULL, _IONBF, BUFSIZ); + rclcpp::init(argc, argv); + + const std::string packagePath = ament_index_cpp::get_package_share_directory("yacyac_core"); + + auto nh = rclcpp::Node::make_shared("yacyac_core"); + nh->declare_parameter("bt_xml", "bt_nav_mememan.xml"); + std::string bt_xml; + nh->get_parameter("bt_xml", bt_xml); + bt_xml = packagePath + "/bt_xml/" + bt_xml; + RCLCPP_INFO(nh->get_logger(), "Loading XML : %s", bt_xml.c_str()); + // RCLCPP_INFO(nh->get_logger(), "Loading XML : %s", bt_xml.c_str()); + // std::cout << "start" << std::endl; + // We use the BehaviorTreeFactory to register our custom nodes + + BT::BehaviorTreeFactory factory; + + // RCLCPP_INFO(nh->get_logger(), "Loading XML : %s", bt_xml.c_str()); + factory.registerNodeType("Nav2Client"); + + // Trees are created at deployment-time (i.e. at run-time, but only once at + // the beginning). The currently supported format is XML. IMPORTANT: when the + // object "tree" goes out of scope, all the TreeNodes are destroyed + auto tree = factory.createTreeFromFile(bt_xml); + + // Create a logger + StdCoutLogger logger_cout(tree); + + NodeStatus status = NodeStatus::RUNNING; + // Keep on ticking until you get either a SUCCESS or FAILURE state + while (rclcpp::ok() && status == NodeStatus::RUNNING) { + status = tree.tickWhileRunning(); + // Sleep 100 milliseconds + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + + return 0; +} \ No newline at end of file diff --git a/yacyac_core/src/main.cpp b/yacyac_core/src/main.cpp index cee32e3..82b8951 100644 --- a/yacyac_core/src/main.cpp +++ b/yacyac_core/src/main.cpp @@ -1,13 +1,13 @@ #include +#include #include #include -#include -#include "yacyac_core/qr_client.hpp" #include "yacyac_core/message.hpp" +#include "yacyac_core/qr_client.hpp" -int main(int argc, char **argv) +int main(int argc, char** argv) { setvbuf(stdout, NULL, _IONBF, BUFSIZ); rclcpp::init(argc, argv); @@ -18,12 +18,12 @@ int main(int argc, char **argv) nh->declare_parameter("bt_xml", "default.xml"); std::string bt_xml; nh->get_parameter("bt_xml", bt_xml); - bt_xml = packagePath + "/bt_tree/" + bt_xml; + bt_xml = packagePath + "/bt_xml/" + bt_xml; RCLCPP_INFO(nh->get_logger(), "Loading XML : %s", bt_xml.c_str()); - + // We use the BehaviorTreeFactory to register our custom nodes BT::BehaviorTreeFactory factory; - + factory.registerNodeType("QRClient"); factory.registerNodeType("Message"); diff --git a/yacyac_core/src/message.cpp b/yacyac_core/src/message.cpp index b8b0e57..ae582d5 100644 --- a/yacyac_core/src/message.cpp +++ b/yacyac_core/src/message.cpp @@ -4,10 +4,8 @@ BT::NodeStatus Message::tick() { BT::Expected msg = getInput("message"); // Check if expected is valid. If not, throw its error - if (!msg) - { - throw BT::RuntimeError("missing required input [message]: ", - msg.error() ); + if (!msg) { + throw BT::RuntimeError("missing required input [message]: ", msg.error()); } // use the method value() to extract the valid message. std::cout << "Robot says: " << msg.value() << std::endl; diff --git a/yacyac_core/src/nav2_client.cpp b/yacyac_core/src/nav2_client.cpp new file mode 100644 index 0000000..a261552 --- /dev/null +++ b/yacyac_core/src/nav2_client.cpp @@ -0,0 +1,136 @@ +// // #include "yacyac_core/nav2_clinent.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include +#include +#include +#include + +#include "behaviortree_cpp/action_node.h" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/quaternion.hpp" +#include "nav2_msgs/action/navigate_to_pose.hpp" +#include "std_msgs/msg/header.hpp" +#include "tf2/LinearMath/Quaternion.h" +#include "tf2/transform_datatypes.h" +#include "tf2_geometry_msgs/tf2_geometry_msgs.h" + +// Custom type +struct Pose2D { + double x, y, quaternion_z, quaternion_w; +}; + +namespace BT { +template <> +inline Pose2D convertFromString(StringView key) +{ + // three real numbers separated by semicolons + auto parts = BT::splitString(key, ';'); + if (parts.size() != 4) { + throw BT::RuntimeError("invalid input)"); + } + else { + Pose\\\\\2D output; + + output.x = convertFromString(parts[0]); + output.y = convertFromString(parts[1]); + output.quaternion_z = convertFromString(parts[2]); + output.quaternion_w = convertFromString(parts[3]); + std::cout << "output.x: " << output.x << std::endl; + return output; + } +} +} // namespace BT +// SyncActionNode + +// end namespace BT + +class Nav2Client : public BT::SyncActionNode { +public: + Nav2Client(const std::string& name, const BT::NodeConfig& config) : BT::SyncActionNode(name, config) { std::cout << "nav2 client start" << std::endl; } + + static BT::PortsList providedPorts() { return { BT::InputPort("goal") }; } + + virtual BT::NodeStatus tick() override + { + std::cout << "nav2 client tick" << std::endl; + node_ = rclcpp::Node::make_shared("nav2_client"); + auto action_client = rclcpp_action::create_client(node_, "navigate_to_pose"); + // if no server is present, fail after 5 seconds + std::cout << "nav2 server wait" << std::endl; + if (!action_client->wait_for_action_server(std::chrono::seconds(20))) { + RCLCPP_ERROR(node_->get_logger(), "Action server not available after waiting"); + return BT::NodeStatus::FAILURE; + } + // Take the goal from the InputPort of the Node + Pose2 goal; + if (!getInput("goal", goal)) { + // if I can't get this, there is something wrong with your BT. + // For this reason throw an exception instead of returning FAILURE + throw BT::RuntimeError("missing required input [goal]"); + } + + _aborted = false; + + RCLCPP_INFO(node_->get_logger(), "Sending goal %f %f %f %f", goal.x, goal.y, goal.quaternion_z, goal.quaternion_w); + + nav2_msgs::action::NavigateToPose::Goal goal_msg; + goal_msg.pose.header.frame_id = "map"; + goal_msg.pose.header.stamp = node_->get_clock()->now(); + goal_msg.pose.pose.position.x = goal.x; + goal_msg.pose.pose.position.y = goal.y; + goal_msg.pose.pose.position.z = 0.0; + + goal_msg.pose.pose.orientation.x = 0; + goal_msg.pose.pose.orientation.y = 0; + goal_msg.pose.pose.orientation.z = goal.quaternion_z; + goal_msg.pose.pose.orientation.w = goal.quaternion_w; + + auto goal_handle_future = action_client->async_send_goal(goal_msg); + if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR(node_->get_logger(), "send goal call failed"); + return BT::NodeStatus::FAILURE; + } + + rclcpp_action::ClientGoalHandle::SharedPtr goal_handle = goal_handle_future.get(); + if (!goal_handle) { + RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); + return BT::NodeStatus::FAILURE; + } + + auto result_future = action_client->async_get_result(goal_handle); + + RCLCPP_INFO(node_->get_logger(), "Waiting for result"); + if (rclcpp::spin_until_future_complete(node_, result_future) != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR(node_->get_logger(), "get result call failed "); + return BT::NodeStatus::FAILURE; + } + + rclcpp_action::ClientGoalHandle::WrappedResult wrapped_result = result_future.get(); + + switch (wrapped_result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: break; + case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR(node_->get_logger(), "Goal was aborted"); return BT::NodeStatus::FAILURE; + case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR(node_->get_logger(), "Goal was canceled"); return BT::NodeStatus::FAILURE; + default: RCLCPP_ERROR(node_->get_logger(), "Unknown result code"); return BT::NodeStatus::FAILURE; + } + + if (_aborted) { + // this happens only if method halt() was invoked + //_client.cancelAllGoals(); + RCLCPP_INFO(node_->get_logger(), "MoveBase aborted"); + return BT::NodeStatus::FAILURE; + } + + // RCLCPP_INFO(node_->get_logger(), "result received"); + return BT::NodeStatus::SUCCESS; + } + +private: + bool _aborted; + + // auto node_ = std::make_shared("nav2_client"); + rclcpp::Node::SharedPtr node_; +}; \ No newline at end of file diff --git a/yacyac_core/src/pub.cpp b/yacyac_core/src/pub.cpp deleted file mode 100644 index 2157f70..0000000 --- a/yacyac_core/src/pub.cpp +++ /dev/null @@ -1,44 +0,0 @@ -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/string.hpp" - -using namespace std::chrono_literals; - -/* This example creates a subclass of Node and uses std::bind() to register a -* member function as a callback from the timer. */ - -class MinimalPublisher : public rclcpp::Node -{ - public: - MinimalPublisher() - : Node("minimal_publisher"), count_(0) - { - publisher_ = this->create_publisher("topic", 10); - timer_ = this->create_wall_timer( - 500ms, std::bind(&MinimalPublisher::timer_callback, this)); - } - - private: - void timer_callback() - { - auto message = std_msgs::msg::String(); - message.data = "Hello, world! " + std::to_string(count_++); - RCLCPP_INFO(this->get_logger(), "Publishing: '%s'", message.data.c_str()); - publisher_->publish(message); - } - rclcpp::TimerBase::SharedPtr timer_; - rclcpp::Publisher::SharedPtr publisher_; - size_t count_; -}; - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} \ No newline at end of file diff --git a/yacyac_core/src/qr_client.cpp b/yacyac_core/src/qr_client.cpp index 97b882f..d872fbd 100644 --- a/yacyac_core/src/qr_client.cpp +++ b/yacyac_core/src/qr_client.cpp @@ -1,15 +1,15 @@ #include "yacyac_core/qr_client.hpp" -QRClient::QRClient(const std::string& name, const BT::NodeConfig& config) : BT::StatefulActionNode(name, config) { +QRClient::QRClient(const std::string& name, const BT::NodeConfig& config) : BT::StatefulActionNode(name, config) +{ node_ = rclcpp::Node::make_shared("qr_client"); std::string QR_topic_name = "/qr_codes"; - - qr_sub_ = node_->create_subscription(QR_topic_name, rclcpp::QoS(1), std::bind(&QRClient::QR_callback_, this, std::placeholders::_1)); + node_->create_subscription(QR_topic_name, rclcpp::QoS(1), std::bind(&QRClient::QR_callback_, this, std::placeholders::_1)); } QRClient::~QRClient() { - RCLCPP_INFO(node_->get_logger(),"SHUTTING DOWN QR NODE"); + RCLCPP_INFO(node_->get_logger(), "SHUTTING DOWN QR NODE"); } BT::NodeStatus QRClient::onStart() @@ -42,10 +42,9 @@ BT::NodeStatus QRClient::onRunning() void QRClient::onHalted() { -// printf("[ QRClient: ABORTED ]"); + // printf("[ QRClient: ABORTED ]"); } - void QRClient::QR_callback_(const yacyac_interface::msg::Qrcode::ConstSharedPtr& msg) { detected_QR_ = msg->qr_infos; diff --git a/yacyac_core/src/send_goal.cpp b/yacyac_core/src/send_goal.cpp new file mode 100644 index 0000000..829c873 --- /dev/null +++ b/yacyac_core/src/send_goal.cpp @@ -0,0 +1,98 @@ +#include "geometry_msgs/msg/pose.hpp" +#include "nav2_msgs/action/navigate_to_pose.hpp" +#include "std_msgs/msg/header.hpp" +#include +#include +#include +#include + +class SendGoalClient { +public: + SendGoalClient(double waypoint_x, double waypoint_y, double waypoint_theta) : waypoint_x_(waypoint_x), waypoint_y_(waypoint_y), waypoint_theta_(waypoint_theta) + { + // Initialize the ROS node + rclcpp::init(0, nullptr); + node_ = std::make_shared("send_goal_client"); + action_client_ = rclcpp_action::create_client(node_, "navigate_to_pose"); + + // Wait for the action server to become available + if (!action_client_->wait_for_action_server(std::chrono::seconds(20))) { + RCLCPP_ERROR(node_->get_logger(), "Action server not available after waiting"); + rclcpp::shutdown(); + return; + } + + // Initialize the goal message + goal_msg_.pose.header.frame_id = "map"; + goal_msg_.pose.pose.position.x = waypoint_x_; + goal_msg_.pose.pose.position.y = waypoint_y_; + goal_msg_.pose.pose.position.z = 0.0; + goal_msg_.pose.pose.orientation.x = 0; + goal_msg_.pose.pose.orientation.y = 0; + goal_msg_.pose.pose.orientation.z = 0; + goal_msg_.pose.pose.orientation.w = waypoint_theta_; + } + void sendGoal() + { + RCLCPP_INFO(node_->get_logger(), "Sending goal"); + + auto goal_handle_future = action_client_->async_send_goal(goal_msg_); + + if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR(node_->get_logger(), "Failed to send goal"); + rclcpp::shutdown(); + return; + } + + rclcpp_action::ClientGoalHandle::SharedPtr goal_handle = goal_handle_future.get(); + if (!goal_handle) { + RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); + rclcpp::shutdown(); + return; + } + + auto result_future = action_client_->async_get_result(goal_handle); + + RCLCPP_INFO(node_->get_logger(), "Waiting for result"); + if (rclcpp::spin_until_future_complete(node_, result_future) != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR(node_->get_logger(), "Failed to receive result"); + rclcpp::shutdown(); + return; + } + + rclcpp_action::ClientGoalHandle::WrappedResult wrapped_result = result_future.get(); + + switch (wrapped_result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: RCLCPP_INFO(node_->get_logger(), "Goal reached successfully!"); break; + case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR(node_->get_logger(), "Goal was aborted"); break; + case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR(node_->get_logger(), "Goal was canceled"); break; + default: RCLCPP_ERROR(node_->get_logger(), "Unknown result code"); break; + } + + rclcpp::shutdown(); + } + +private: + rclcpp::Node::SharedPtr node_; + rclcpp_action::Client::SharedPtr action_client_; + nav2_msgs::action::NavigateToPose::Goal goal_msg_; + double waypoint_x_; + double waypoint_y_; + double waypoint_theta_; +}; + +int main(int argc, char** argv) +{ + if (argc != 4) { + std::cerr << "Usage: " << argv[0] << " " << std::endl; + return 1; + } + + double waypoint_x = std::stod(argv[1]); + double waypoint_y = std::stod(argv[2]); + double waypoint_theta = std::stod(argv[3]); + std::cout << "start" << std::endl; + SendGoalClient client(waypoint_x, waypoint_y, waypoint_theta); + client.sendGoal(); + return 0; +} diff --git a/yacyac_core/src/sub.cpp b/yacyac_core/src/sub.cpp deleted file mode 100644 index 83345b4..0000000 --- a/yacyac_core/src/sub.cpp +++ /dev/null @@ -1,31 +0,0 @@ -#include - -#include "rclcpp/rclcpp.hpp" -#include "std_msgs/msg/string.hpp" -using std::placeholders::_1; - -class MinimalSubscriber : public rclcpp::Node -{ - public: - MinimalSubscriber() - : Node("minimal_subscriber") - { - subscription_ = this->create_subscription( - "topic", 10, std::bind(&MinimalSubscriber::topic_callback, this, _1)); - } - - private: - void topic_callback(const std_msgs::msg::String::SharedPtr msg) const - { - RCLCPP_INFO(this->get_logger(), "I heard: '%s'", msg->data.c_str()); - } - rclcpp::Subscription::SharedPtr subscription_; -}; - -int main(int argc, char * argv[]) -{ - rclcpp::init(argc, argv); - rclcpp::spin(std::make_shared()); - rclcpp::shutdown(); - return 0; -} \ No newline at end of file diff --git a/yacyac_io/yacyac_io/env/lib64 b/yacyac_io/yacyac_io/env/lib64 new file mode 120000 index 0000000..7951405 --- /dev/null +++ b/yacyac_io/yacyac_io/env/lib64 @@ -0,0 +1 @@ +lib \ No newline at end of file diff --git a/yacyac_io/yacyac_io/env/pyvenv.cfg b/yacyac_io/yacyac_io/env/pyvenv.cfg new file mode 100644 index 0000000..853404e --- /dev/null +++ b/yacyac_io/yacyac_io/env/pyvenv.cfg @@ -0,0 +1,3 @@ +home = /usr/bin +include-system-site-packages = false +version = 3.8.10 From a8fdbebec2206088a96a38d30ae023294b0d3b65 Mon Sep 17 00:00:00 2001 From: ggh Date: Tue, 22 Aug 2023 01:28:09 +0900 Subject: [PATCH 2/6] yacyac_servo action update --- yacyac_core/src/nav2_client.cpp | 4 +- yacyac_interface/CMakeLists.txt | 1 + yacyac_interface/action/Supply.action | 5 + yacyac_servo/setup.py | 1 + .../yacyac_servo/action_servo_ctrl.py | 211 ++++++++++++++++++ yacyac_servo/yacyac_servo/servo_ctrl.py | 5 +- 6 files changed, 224 insertions(+), 3 deletions(-) create mode 100644 yacyac_interface/action/Supply.action create mode 100644 yacyac_servo/yacyac_servo/action_servo_ctrl.py diff --git a/yacyac_core/src/nav2_client.cpp b/yacyac_core/src/nav2_client.cpp index a261552..15b5215 100644 --- a/yacyac_core/src/nav2_client.cpp +++ b/yacyac_core/src/nav2_client.cpp @@ -32,7 +32,7 @@ inline Pose2D convertFromString(StringView key) throw BT::RuntimeError("invalid input)"); } else { - Pose\\\\\2D output; + Pose2D output; output.x = convertFromString(parts[0]); output.y = convertFromString(parts[1]); @@ -65,7 +65,7 @@ class Nav2Client : public BT::SyncActionNode { return BT::NodeStatus::FAILURE; } // Take the goal from the InputPort of the Node - Pose2 goal; + Pose2D goal; if (!getInput("goal", goal)) { // if I can't get this, there is something wrong with your BT. // For this reason throw an exception instead of returning FAILURE diff --git a/yacyac_interface/CMakeLists.txt b/yacyac_interface/CMakeLists.txt index 8fc4b43..04a9201 100644 --- a/yacyac_interface/CMakeLists.txt +++ b/yacyac_interface/CMakeLists.txt @@ -23,6 +23,7 @@ rosidl_generate_interfaces(${PROJECT_NAME}_msgs "msg/Qrcode.msg" "msg/Servo.msg" "srv/TTS.srv" + "action/Supply.action" DEPENDENCIES std_msgs LIBRARY_NAME ${PROJECT_NAME} ) diff --git a/yacyac_interface/action/Supply.action b/yacyac_interface/action/Supply.action new file mode 100644 index 0000000..7d00a07 --- /dev/null +++ b/yacyac_interface/action/Supply.action @@ -0,0 +1,5 @@ +uint8[8] supply_list +--- +uint8 sequence +--- +uint8[] partial_sequence \ No newline at end of file diff --git a/yacyac_servo/setup.py b/yacyac_servo/setup.py index d5fea2c..c580a7c 100644 --- a/yacyac_servo/setup.py +++ b/yacyac_servo/setup.py @@ -23,6 +23,7 @@ entry_points={ 'console_scripts': [ 'servo_ctrl = yacyac_servo.servo_ctrl:main', + 'action_servo_ctrl = yacyac_servo.action_servo_ctrl:main', ], }, ) \ No newline at end of file diff --git a/yacyac_servo/yacyac_servo/action_servo_ctrl.py b/yacyac_servo/yacyac_servo/action_servo_ctrl.py new file mode 100644 index 0000000..704b217 --- /dev/null +++ b/yacyac_servo/yacyac_servo/action_servo_ctrl.py @@ -0,0 +1,211 @@ +from typing import Optional +import rclpy as rp +import time as time +from rclpy.executors import Executor + +from rclpy.node import Node +from rclpy.action import ActionServer + +from yacyac_interface.msg import Servo +from yacyac_interface.action import Supply as SupplyAction +from dynamixel_sdk_custom_interfaces.msg import SetPosition +from dynamixel_sdk_custom_interfaces.srv import GetPosition + +# 목적 포지션을 받으면 그에 맞게 서보를 움직임 +# 목적 포지션에 도달하면 도착 메시지를 보냄 + + + +class ServoCtrl(Node): + def __init__(self): + super().__init__("servo_ctrl") + # 약 개수 입력받음 -> 이걸 서비스 콜로 받아야함 + self._action_server = ActionServer(self, SupplyAction, "yacyac/supply_action", self.execute_callback) + # 해당 포지션으로 다이나믹셀의 포지션을 이동시킴 + self.pub = self.create_publisher(SetPosition, "/set_position", 20) + # 현재 포지션을 서비스 클라이언트로 받아옴 + self.cli = self.create_client(GetPosition, "/get_position") + + self.cmd_pose_id = Servo() + self.cnt = 0 + # 목적 포지션 list + # 1023 to 360 degree + # 시계방향으로 돌아가는 포지션 리스트 + self.cw_position_set = [0, 164, 328, 492, 656, 820] + # 반 시계방향으로 돌아가는 포지션 리스트 + self.ccw_position_set = [915,751, 587,423, 259] + # 방향 flag + self.position_flag = [0, 0, 0, 0, 0, 0, 0, 0, 0] + # 현재 포지션 index + self.position_cnt = [0, 0, 0, 0, 0, 0, 0, 0, 0] + + + + # 현재 약 배출 개수 + self.yac_cnt = 0 + + print ("init positioning...") + # for i in range(8): + # # 근처 포지션으로 이동합니다. + # self.init_position(i) + # # 원점 포지션으로 이동합니다. + # # self.reset_position(i) + print ("init positioning done!!!") + + + def execute_callback(self, goal_handle): + print('제조 정보를 입력 받았습니다...') + + supply_list = list(goal_handle.request.supply_list) + print("약 제조 리스트") + print("list : ", supply_list) + + yac_sum = sum(supply_list) + for idx in range(len(supply_list)): + if supply_list[idx] != 0: + self.control_position(idx, supply_list[idx], goal_handle) + + goal_handle.succeed() + + result = SupplyAction.Result() + print(yac_sum) + result.sequence = int(yac_sum) + print('총 ', yac_sum, '개의 약을 제조했습니다.') + # print('Result: {0}'.format(result.sequence)) + + return result + + + + + def init_position(self, id): + req = GetPosition.Request() + req.id = id + while not self.cli.wait_for_service(timeout_sec=1.0): + self.get_logger().info("Service not available, waiting again...") + + future = self.cli.call_async(req) + rp.spin_until_future_complete(self, future) + + if future.result() is not None: + response = future.result() + # print("response") + closest_idx = 0 + + min_diff = 10000 + + + cmd_pose = SetPosition() + cmd_pose.id = id + flag = 0 + for i, val in enumerate(self.cw_position_set): + diff = abs(response.position - val) + if diff < min_diff: + min_diff = diff + closest_idx = i + cmd_pose.position = self.cw_position_set[closest_idx] + flag = 1 + for i, val in enumerate(self.ccw_position_set): + diff = abs(response.position - val) + if diff < min_diff: + min_diff = diff + closest_idx = i + cmd_pose.position = self.ccw_position_set[closest_idx] + flag = 0 + if flag == 0: + self.position_flag[id] = 1 + # print("cw") + else: + self.position_flag[id] = 0 + # print("ccw") + # 리스트의 요소값과 같은 값이 있으면 그 인덱스를 반환 + if flag: + for i, val in enumerate(self.cw_position_set): + if val == cmd_pose.position: + self.position_cnt[id] = i + break + else: + for i, val in enumerate(self.ccw_position_set): + if val == cmd_pose.position: + self.position_cnt[id] = i + break + self.pub.publish(cmd_pose) + time.sleep(0.1) + else: + self.get_logger().info(f"Service call for ID {id} failed") + + + + def control_position(self, id, servo, goal_handle): + # 제조할 약의 개수 + yac_num = servo + # 배출한 약의 개수 + yac_cnt = 0 + while yac_num != 0: + # 포지션이 끝에 도달하면 방향을 바꿈 + # 역방향 진행 + yac_num -= 1 + yac_cnt += 1 + # yac_cnt + print(id, "번 약 ", servo, "개중 ", yac_cnt, "개 배출중...") + feedback_msg = SupplyAction.Feedback() + self.yac_cnt += 1 + feedback_msg.partial_sequence.append(self.yac_cnt) + goal_handle.publish_feedback(feedback_msg) + # self.get_logger().info('Feedback: {0}'.format(feedback_msg.partial_sequence)) + if self.position_flag[id] == 1: + cmd_pose = SetPosition() + self.position_cnt[id] += 1 + if self.position_cnt[id] >= len(self.ccw_position_set): + self.position_cnt[id] = 0 + self.position_flag[id] = 0 + cmd_pose.position = self.cw_position_set[self.position_cnt[id]] + cmd_pose.id = id + self.pub.publish(cmd_pose) + else: + cmd_pose.position = self.ccw_position_set[self.position_cnt[id]] + cmd_pose.id = id + self.pub.publish(cmd_pose) + else : + cmd_pose = SetPosition() + self.position_cnt[id] += 1 + if self.position_cnt[id] >= len(self.cw_position_set): + self.position_cnt[id] = 0 + self.position_flag[id] = 1 + cmd_pose.position = self.ccw_position_set[self.position_cnt[id]] + cmd_pose.id = id + self.pub.publish(cmd_pose) + else: + cmd_pose.position = self.cw_position_set[self.position_cnt[id]] + cmd_pose.id = id + self.pub.publish(cmd_pose) + + + time.sleep(1) + + print(id, "번 약 배출이 완료되었습니다.") + + + def reset_position(self, id): + cmd_pose = SetPosition() + cmd_pose.id = id + cmd_pose.position = self.cw_position_set[0] + self.pub.publish(cmd_pose) + time.sleep(0.1) + + + + +def main(args=None): + rp.init(args=args) + + servo_node = ServoCtrl() + rp.spin(servo_node) + + servo_node.destroy_node() + rp.shutdown() + + +if __name__ == "__main__": + print("start servo_node") + main() diff --git a/yacyac_servo/yacyac_servo/servo_ctrl.py b/yacyac_servo/yacyac_servo/servo_ctrl.py index a09b079..75d6e01 100644 --- a/yacyac_servo/yacyac_servo/servo_ctrl.py +++ b/yacyac_servo/yacyac_servo/servo_ctrl.py @@ -12,11 +12,14 @@ -class ServoCtrl(Node): +class ServoCtrl(Node): def __init__(self): super().__init__("servo_ctrl") + # 약 개수 입력받음 -> 이걸 서비스 콜로 받아야함 self.sub = self.create_subscription(Servo, "/yacyac/servo", self.callback_yac, 10) + # 해당 포지션으로 다이나믹셀의 포지션을 이동시킴 self.pub = self.create_publisher(SetPosition, "/set_position", 20) + # 현재 포지션을 서비스 클라이언트로 받아옴 self.cli = self.create_client(GetPosition, "/get_position") self.cmd_pose_id = Servo() From fee58adaa59e21b3cfb7f4006eb2db2a41d2c8a1 Mon Sep 17 00:00:00 2001 From: ggh Date: Tue, 22 Aug 2023 10:18:16 +0900 Subject: [PATCH 3/6] clang update --- .../include/read_write_node.hpp | 24 +- .../src/read_write_node.cpp | 273 ++++++++---------- 2 files changed, 130 insertions(+), 167 deletions(-) diff --git a/DynamixelSDK/dynamixel_sdk_examples/include/read_write_node.hpp b/DynamixelSDK/dynamixel_sdk_examples/include/read_write_node.hpp index f97a9e9..1c1c56b 100644 --- a/DynamixelSDK/dynamixel_sdk_examples/include/read_write_node.hpp +++ b/DynamixelSDK/dynamixel_sdk_examples/include/read_write_node.hpp @@ -19,27 +19,25 @@ #include #include -#include "rclcpp/rclcpp.hpp" -#include "rcutils/cmdline_parser.h" #include "dynamixel_sdk/dynamixel_sdk.h" #include "dynamixel_sdk_custom_interfaces/msg/set_position.hpp" #include "dynamixel_sdk_custom_interfaces/srv/get_position.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rcutils/cmdline_parser.h" - -class ReadWriteNode : public rclcpp::Node -{ +class ReadWriteNode : public rclcpp::Node { public: - using SetPosition = dynamixel_sdk_custom_interfaces::msg::SetPosition; - using GetPosition = dynamixel_sdk_custom_interfaces::srv::GetPosition; + using SetPosition = dynamixel_sdk_custom_interfaces::msg::SetPosition; + using GetPosition = dynamixel_sdk_custom_interfaces::srv::GetPosition; - ReadWriteNode(); - virtual ~ReadWriteNode(); + ReadWriteNode(); + virtual ~ReadWriteNode(); private: - rclcpp::Subscription::SharedPtr set_position_subscriber_; - rclcpp::Service::SharedPtr get_position_server_; + rclcpp::Subscription::SharedPtr set_position_subscriber_; + rclcpp::Service::SharedPtr get_position_server_; - int present_position; + int present_position; }; -#endif // READ_WRITE_NODE_HPP_ +#endif // READ_WRITE_NODE_HPP_ diff --git a/DynamixelSDK/dynamixel_sdk_examples/src/read_write_node.cpp b/DynamixelSDK/dynamixel_sdk_examples/src/read_write_node.cpp index ecbf8ec..382b598 100644 --- a/DynamixelSDK/dynamixel_sdk_examples/src/read_write_node.cpp +++ b/DynamixelSDK/dynamixel_sdk_examples/src/read_write_node.cpp @@ -13,16 +13,20 @@ // limitations under the License. /******************************************************************************* -// This example is written for DYNAMIXEL X(excluding XL-320) and MX(2.0) series with U2D2. -// For other series, please refer to the product eManual and modify the Control Table addresses and other definitions. +// This example is written for DYNAMIXEL X(excluding XL-320) and MX(2.0) series +with U2D2. +// For other series, please refer to the product eManual and modify the Control +Table addresses and other definitions. // To test this example, please follow the commands below. // // Open terminal #1 // $ ros2 run dynamixel_sdk_examples read_write_node // // Open terminal #2 (run one of below commands at a time) -// $ ros2 topic pub -1 /set_position dynamixel_sdk_custom_interfaces/SetPosition "{id: 1, position: 1000}" -// $ ros2 service call /get_position dynamixel_sdk_custom_interfaces/srv/GetPosition "id: 1" +// $ ros2 topic pub -1 /set_position dynamixel_sdk_custom_interfaces/SetPosition +"{id: 1, position: 1000}" +// $ ros2 service call /get_position +dynamixel_sdk_custom_interfaces/srv/GetPosition "id: 1" // // Author: Will Son *******************************************************************************/ @@ -40,174 +44,135 @@ #include "read_write_node.hpp" // Control table address for X series (except XL-320) -#define ADDR_OPERATING_MODE 11 -#define ADDR_TORQUE_ENABLE 64 -#define ADDR_GOAL_POSITION 116 -#define ADDR_PRESENT_POSITION 132 +#define ADDR_OPERATING_MODE 1 +#define ADDR_TORQUE_ENABLE 24 +#define ADDR_GOAL_POSITION 30 +#define ADDR_PRESENT_POSITION 36 + +#define MOVING_SPEED 32 // Protocol version -#define PROTOCOL_VERSION 2.0 // Default Protocol version of DYNAMIXEL X series. +#define PROTOCOL_VERSION 1.0 // Default Protocol version of DYNAMIXEL X series. // Default setting -#define BAUDRATE 57600 // Default Baudrate of DYNAMIXEL X series -#define DEVICE_NAME "/dev/ttyUSB0" // [Linux]: "/dev/ttyUSB*", [Windows]: "COM*" +#define BAUDRATE 1000000 // Default Baudrate of DYNAMIXEL X series +#define DEVICE_NAME "/dev/yacyac/servo" // [Linux]: "/dev/ttyUSB*", [Windows]: "COM*" -dynamixel::PortHandler * portHandler; -dynamixel::PacketHandler * packetHandler; +dynamixel::PortHandler* portHandler; +dynamixel::PacketHandler* packetHandler; uint8_t dxl_error = 0; uint32_t goal_position = 0; int dxl_comm_result = COMM_TX_FAIL; -ReadWriteNode::ReadWriteNode() -: Node("read_write_node") +ReadWriteNode::ReadWriteNode() : Node("read_write_node") { - RCLCPP_INFO(this->get_logger(), "Run read write node"); - - this->declare_parameter("qos_depth", 10); - int8_t qos_depth = 0; - this->get_parameter("qos_depth", qos_depth); - - const auto QOS_RKL10V = - rclcpp::QoS(rclcpp::KeepLast(qos_depth)).reliable().durability_volatile(); - - set_position_subscriber_ = - this->create_subscription( - "set_position", - QOS_RKL10V, - [this](const SetPosition::SharedPtr msg) -> void - { - uint8_t dxl_error = 0; - - // Position Value of X series is 4 byte data. - // For AX & MX(1.0) use 2 byte data(uint16_t) for the Position Value. - uint32_t goal_position = (unsigned int)msg->position; // Convert int32 -> uint32 - - // Write Goal Position (length : 4 bytes) - // When writing 2 byte data to AX / MX(1.0), use write2ByteTxRx() instead. - dxl_comm_result = - packetHandler->write4ByteTxRx( - portHandler, - (uint8_t) msg->id, - ADDR_GOAL_POSITION, - goal_position, - &dxl_error - ); - - if (dxl_comm_result != COMM_SUCCESS) { - RCLCPP_INFO(this->get_logger(), "%s", packetHandler->getTxRxResult(dxl_comm_result)); - } else if (dxl_error != 0) { - RCLCPP_INFO(this->get_logger(), "%s", packetHandler->getRxPacketError(dxl_error)); - } else { - RCLCPP_INFO(this->get_logger(), "Set [ID: %d] [Goal Position: %d]", msg->id, msg->position); - } - } - ); - - auto get_present_position = - [this]( - const std::shared_ptr request, - std::shared_ptr response) -> void - { - // Read Present Position (length : 4 bytes) and Convert uint32 -> int32 - // When reading 2 byte data from AX / MX(1.0), use read2ByteTxRx() instead. - dxl_comm_result = packetHandler->read4ByteTxRx( - portHandler, - (uint8_t) request->id, - ADDR_PRESENT_POSITION, - reinterpret_cast(&present_position), - &dxl_error - ); - - RCLCPP_INFO( - this->get_logger(), - "Get [ID: %d] [Present Position: %d]", - request->id, - present_position - ); - - response->position = present_position; + RCLCPP_INFO(this->get_logger(), "Run read write node"); + + this->declare_parameter("qos_depth", 10); + int8_t qos_depth = 0; + this->get_parameter("qos_depth", qos_depth); + + const auto QOS_RKL10V = rclcpp::QoS(rclcpp::KeepLast(qos_depth)).reliable().durability_volatile(); + + set_position_subscriber_ = this->create_subscription("set_position", QOS_RKL10V, [this](const SetPosition::SharedPtr msg) -> void { + uint8_t dxl_error = 0; + + // Position Value of X series is 4 byte data. + // For AX & MX(1.0) use 2 byte data(uint16_t) for the Position Value. + uint16_t goal_position = static_cast(msg->position); // Convert int32 -> uint16_t + uint16_t moving_speed = static_cast(160); // Convert int32 -> uint16_t + // Write Goal Position (length: 2 bytes) for AX series motor + // For MX(1.0) series, use write2ByteTxRx() instead. + dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, msg->id, MOVING_SPEED, moving_speed, &dxl_error); + dxl_comm_result = packetHandler->write2ByteTxRx(portHandler, msg->id, ADDR_GOAL_POSITION, goal_position, &dxl_error); + // 여기서 msg->id는 메시지에 포함된 모터의 ID입니다. + // ADDR_GOAL_POSITION은 AX 시리즈 모터의 목표 위치 레지스터 주소를 나타냅니다. + + if (dxl_comm_result != COMM_SUCCESS) { + RCLCPP_INFO(this->get_logger(), "%s", packetHandler->getTxRxResult(dxl_comm_result)); + } + else if (dxl_error != 0) { + RCLCPP_INFO(this->get_logger(), "%s", packetHandler->getRxPacketError(dxl_error)); + } + else { + RCLCPP_INFO(this->get_logger(), "Set [ID: %d] [Goal Position: %d]", msg->id, msg->position); + } + }); + + auto get_present_position = [this](const std::shared_ptr request, std::shared_ptr response) -> void { + // Read Present Position (length : 4 bytes) and Convert uint32 -> int32 + // When reading 2 byte data from AX / MX(1.0), use read2ByteTxRx() instead. + dxl_comm_result = packetHandler->read2ByteTxRx(portHandler, (uint8_t)request->id, ADDR_PRESENT_POSITION, reinterpret_cast(&present_position), &dxl_error); + + RCLCPP_INFO(this->get_logger(), "Get [ID: %d] [Present Position: %d]", request->id, present_position); + + response->position = present_position; }; - get_position_server_ = create_service("get_position", get_present_position); + get_position_server_ = create_service("get_position", get_present_position); } -ReadWriteNode::~ReadWriteNode() -{ -} +ReadWriteNode::~ReadWriteNode() {} void setupDynamixel(uint8_t dxl_id) { - // Use Position Control Mode - dxl_comm_result = packetHandler->write1ByteTxRx( - portHandler, - dxl_id, - ADDR_OPERATING_MODE, - 3, - &dxl_error - ); - - if (dxl_comm_result != COMM_SUCCESS) { - RCLCPP_ERROR(rclcpp::get_logger("read_write_node"), "Failed to set Position Control Mode."); - } else { - RCLCPP_INFO(rclcpp::get_logger("read_write_node"), "Succeeded to set Position Control Mode."); - } - - // Enable Torque of DYNAMIXEL - dxl_comm_result = packetHandler->write1ByteTxRx( - portHandler, - dxl_id, - ADDR_TORQUE_ENABLE, - 1, - &dxl_error - ); - - if (dxl_comm_result != COMM_SUCCESS) { - RCLCPP_ERROR(rclcpp::get_logger("read_write_node"), "Failed to enable torque."); - } else { - RCLCPP_INFO(rclcpp::get_logger("read_write_node"), "Succeeded to enable torque."); - } + // Use Position Control Mode + dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, dxl_id, ADDR_OPERATING_MODE, 3, &dxl_error); + + if (dxl_comm_result != COMM_SUCCESS) { + RCLCPP_ERROR(rclcpp::get_logger("read_write_node"), "Failed to set Position Control Mode."); + } + else { + RCLCPP_INFO(rclcpp::get_logger("read_write_node"), "Succeeded to set Position Control Mode."); + } + + // Enable Torque of DYNAMIXEL + dxl_comm_result = packetHandler->write1ByteTxRx(portHandler, dxl_id, ADDR_TORQUE_ENABLE, 1, &dxl_error); + + if (dxl_comm_result != COMM_SUCCESS) { + RCLCPP_ERROR(rclcpp::get_logger("read_write_node"), "Failed to enable torque."); + } + else { + RCLCPP_INFO(rclcpp::get_logger("read_write_node"), "Succeeded to enable torque."); + } } -int main(int argc, char * argv[]) +int main(int argc, char* argv[]) { - portHandler = dynamixel::PortHandler::getPortHandler(DEVICE_NAME); - packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION); - - // Open Serial Port - dxl_comm_result = portHandler->openPort(); - if (dxl_comm_result == false) { - RCLCPP_ERROR(rclcpp::get_logger("read_write_node"), "Failed to open the port!"); - return -1; - } else { - RCLCPP_INFO(rclcpp::get_logger("read_write_node"), "Succeeded to open the port."); - } - - // Set the baudrate of the serial port (use DYNAMIXEL Baudrate) - dxl_comm_result = portHandler->setBaudRate(BAUDRATE); - if (dxl_comm_result == false) { - RCLCPP_ERROR(rclcpp::get_logger("read_write_node"), "Failed to set the baudrate!"); - return -1; - } else { - RCLCPP_INFO(rclcpp::get_logger("read_write_node"), "Succeeded to set the baudrate."); - } - - setupDynamixel(BROADCAST_ID); - - rclcpp::init(argc, argv); - - auto readwritenode = std::make_shared(); - rclcpp::spin(readwritenode); - rclcpp::shutdown(); - - // Disable Torque of DYNAMIXEL - packetHandler->write1ByteTxRx( - portHandler, - BROADCAST_ID, - ADDR_TORQUE_ENABLE, - 0, - &dxl_error - ); - - return 0; -} + portHandler = dynamixel::PortHandler::getPortHandler(DEVICE_NAME); + packetHandler = dynamixel::PacketHandler::getPacketHandler(PROTOCOL_VERSION); + + // Open Serial Port + dxl_comm_result = portHandler->openPort(); + if (dxl_comm_result == false) { + RCLCPP_ERROR(rclcpp::get_logger("read_write_node"), "Failed to open the port!"); + return -1; + } + else { + RCLCPP_INFO(rclcpp::get_logger("read_write_node"), "Succeeded to open the port."); + } + + // Set the baudrate of the serial port (use DYNAMIXEL Baudrate) + dxl_comm_result = portHandler->setBaudRate(BAUDRATE); + if (dxl_comm_result == false) { + RCLCPP_ERROR(rclcpp::get_logger("read_write_node"), "Failed to set the baudrate!"); + return -1; + } + else { + RCLCPP_INFO(rclcpp::get_logger("read_write_node"), "Succeeded to set the baudrate."); + } + + setupDynamixel(BROADCAST_ID); + + rclcpp::init(argc, argv); + + auto readwritenode = std::make_shared(); + rclcpp::spin(readwritenode); + rclcpp::shutdown(); + + // Disable Torque of DYNAMIXEL + packetHandler->write1ByteTxRx(portHandler, BROADCAST_ID, ADDR_TORQUE_ENABLE, 0, &dxl_error); + + return 0; +} \ No newline at end of file From bb3d7d32cb6894a5e92d5c79931d4b471b10600d Mon Sep 17 00:00:00 2001 From: ggh Date: Tue, 22 Aug 2023 10:27:08 +0900 Subject: [PATCH 4/6] readme update --- README.md | 83 +++++++++++++++++++++++++++---------- yacyac_core/src/bt_ros2.cpp | 1 + 2 files changed, 63 insertions(+), 21 deletions(-) diff --git a/README.md b/README.md index 5d3e39f..a249814 100644 --- a/README.md +++ b/README.md @@ -1,30 +1,25 @@ -# yacyac -yacyac robot description - -# clone -```bash -git clone --recursive https://github.com/dawan0111/yacyac.git -``` - # Dependence Package -```bash -$ sudo apt install libzbar-dev -$ sudo apt install libzmq3-dev -$ sudo apt install ros-${ROS_DISTRO}-rosbridge-server -$ sudo apt install ros-${ROS_DISTRO}-image-transport -$ sudo apt install ros-${ROS_DISTRO}-compressed-image-transport + +```jsx +sudo apt install libzbar-dev +sudo apt install libzmq3-dev +sudo apt install ros-${ROS_DISTRO}-rosbridge-server +sudo apt install ros-${ROS_DISTRO}-image-transport +sudo apt install ros-${ROS_DISTRO}-compressed-image-transport ``` ## install & build --- +## YDLIDAR + ### sdk install ```bash cd ~/ros2_ws/src git clone https://github.com/YDLIDAR/YDLidar-SDK.git -cd YDLidar-SDK/build +cd YDLidar-SDK/cmake cmake .. make sudo make install @@ -46,23 +41,23 @@ colcon build --symlink-install ### workbench install -```bash +```jsx cd ~/ros2_ws/src -git clone -b galactic_devel https://github.com/ROBOTIS-GIT/DynamixelSDK.git +git clone -b galactic-devel https://github.com/ROBOTIS-GIT/DynamixelSDK.git ``` - ### cartographer ```bash sudo apt install ros-galactic-cartographer-ros +sudo apt install ros-galactic-cartographer ``` ### nav2 ```bash -sudo apt-get install ros-galatic-nav2* +sudo apt-get install ros-galactic-nav2* ``` ## robot setup @@ -70,7 +65,53 @@ sudo apt-get install ros-galatic-nav2* --- ```bash -chmod 0777 ros2_ws/src/yacyac/yacyac_setup/* -cd ~/ros2_ws/src/yacyac/yacyac_setup +chmod 0777 ~/ros2_ws/src/yacyac/* +cd ~/ros2_ws/src/yacyac sudo sh initenv.sh ``` + +## tts + +--- + +```bash +cd ~/ros2_ws/src/yacyac/yacyac_io/yacyac_io +python3 -m venv env +source env/bin/activate +pip install --upgrade google-cloud-texttospeech +pip install playsound +``` + +### Qt serial port + +```jsx +sudo apt-get install libqt5serialport5-dev +``` + +## bashrc + +--- + +```jsx +alias eb='nano ~/.bashrc' +alias sb='source ~/.bashrc' +alias gs='git status' +alias gp='git pull' + +alias gala="source /opt/ros/galactic/setup.bash; echo \"ROS2 galactic\"" +alias killgazebo="killall gzserver gzclient" +alias ros_domain="export ROS_DOMAIN_ID=13" +alias ros2study="gala; source ~/ros2_ws/install/local_setup.bash; echo \"ros2 ws is activated.!!\"" + +alias cb="cd ~/ros2_ws && colcon build --symlink-install && source install/local_setup.bash" +export ROS_DISTRO=galactic + +export YDLIDAR_SDK=/path/to/YDLidar-SDK +export RCUTILS_COLORIZED_OUTPUT=1 +source /opt/ros/galactic/setup.bash +source ~/ros2_ws/install/local_setup.bash +source /opt/ros/galactic/setup.bash + +export GOOGLE_APPLICATION_CREDENTIALS=~/yacyac-gcp.json +#export ROS_LOCALHOST_ONLY=1 +``` \ No newline at end of file diff --git a/yacyac_core/src/bt_ros2.cpp b/yacyac_core/src/bt_ros2.cpp index c99f88c..3026318 100644 --- a/yacyac_core/src/bt_ros2.cpp +++ b/yacyac_core/src/bt_ros2.cpp @@ -22,6 +22,7 @@ int main(int argc, char** argv) bt_xml = packagePath + "/bt_xml/" + bt_xml; RCLCPP_INFO(nh->get_logger(), "Loading XML : %s", bt_xml.c_str()); // RCLCPP_INFO(nh->get_logger(), "Loading XML : %s", bt_xml.c_str()); + // std::cout << "start" << std::endl; // We use the BehaviorTreeFactory to register our custom nodes From 1d624ef89b180ad84e182f2a36f1acc12c986d6d Mon Sep 17 00:00:00 2001 From: ggh Date: Fri, 25 Aug 2023 13:05:21 +0900 Subject: [PATCH 5/6] yacyac behavior tree update pill supply and nav2 client --- yacyac_core/CMakeLists.txt | 3 +- yacyac_core/bt_xml/bt_nav_mememan.xml | 28 ++-- yacyac_core/bt_xml/bt_nav_yac_supply copy.xml | 71 ++++++++++ yacyac_core/bt_xml/bt_nav_yac_supply.xml | 82 +++++++++++ yacyac_core/bt_xml/bt_yac_supply.xml | 26 ++++ yacyac_core/src/bt_ros2.cpp | 10 +- yacyac_core/src/send_goal3.cpp | 94 ++++++++++++ yacyac_core/src/yac_supply_client.cpp | 134 ++++++++++++++++++ yacyac_interface/action/Supply.action | 2 +- .../yacyac_servo/action_servo_ctrl.py | 2 +- 10 files changed, 434 insertions(+), 18 deletions(-) create mode 100644 yacyac_core/bt_xml/bt_nav_yac_supply copy.xml create mode 100644 yacyac_core/bt_xml/bt_nav_yac_supply.xml create mode 100644 yacyac_core/bt_xml/bt_yac_supply.xml create mode 100644 yacyac_core/src/send_goal3.cpp create mode 100644 yacyac_core/src/yac_supply_client.cpp diff --git a/yacyac_core/CMakeLists.txt b/yacyac_core/CMakeLists.txt index ff7d9f9..6910ef3 100644 --- a/yacyac_core/CMakeLists.txt +++ b/yacyac_core/CMakeLists.txt @@ -36,6 +36,7 @@ set(DEPENDENCIES nav2_behavior_tree nav2_msgs behaviortree_cpp + yacyac_interface ${OTHER_DEPS} ) @@ -73,7 +74,7 @@ ament_target_dependencies(${PROJECT_NAME}_lib rclcpp std_msgs behaviortree_cpp yacyac_interface nav2_behavior_tree nav2_msgs ) -ament_target_dependencies(send_goal rclcpp std_msgs rclcpp_action geometry_msgs nav2_msgs) +ament_target_dependencies(send_goal rclcpp std_msgs rclcpp_action geometry_msgs nav2_msgs yacyac_interface) target_link_libraries(${PROJECT_NAME} ${PROJECT_NAME}_lib) target_link_libraries(bt_ros2 ${PROJECT_NAME}_lib) diff --git a/yacyac_core/bt_xml/bt_nav_mememan.xml b/yacyac_core/bt_xml/bt_nav_mememan.xml index 4f3aef0..4d93a02 100644 --- a/yacyac_core/bt_xml/bt_nav_mememan.xml +++ b/yacyac_core/bt_xml/bt_nav_mememan.xml @@ -1,26 +1,26 @@ + - - - - - - - - - - - - + + + - - + + + + + + + + + + \ No newline at end of file diff --git a/yacyac_core/bt_xml/bt_nav_yac_supply copy.xml b/yacyac_core/bt_xml/bt_nav_yac_supply copy.xml new file mode 100644 index 0000000..3f1a3f9 --- /dev/null +++ b/yacyac_core/bt_xml/bt_nav_yac_supply copy.xml @@ -0,0 +1,71 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/yacyac_core/bt_xml/bt_nav_yac_supply.xml b/yacyac_core/bt_xml/bt_nav_yac_supply.xml new file mode 100644 index 0000000..0026c3b --- /dev/null +++ b/yacyac_core/bt_xml/bt_nav_yac_supply.xml @@ -0,0 +1,82 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/yacyac_core/bt_xml/bt_yac_supply.xml b/yacyac_core/bt_xml/bt_yac_supply.xml new file mode 100644 index 0000000..da4ddb0 --- /dev/null +++ b/yacyac_core/bt_xml/bt_yac_supply.xml @@ -0,0 +1,26 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/yacyac_core/src/bt_ros2.cpp b/yacyac_core/src/bt_ros2.cpp index 3026318..36a1e7f 100644 --- a/yacyac_core/src/bt_ros2.cpp +++ b/yacyac_core/src/bt_ros2.cpp @@ -1,4 +1,7 @@ #include "nav2_client.cpp" +#include "yac_supply_client.cpp" +#include "yacyac_core/message.hpp" +#include "yacyac_core/qr_client.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/rclcpp_action.hpp" @@ -16,7 +19,9 @@ int main(int argc, char** argv) const std::string packagePath = ament_index_cpp::get_package_share_directory("yacyac_core"); auto nh = rclcpp::Node::make_shared("yacyac_core"); - nh->declare_parameter("bt_xml", "bt_nav_mememan.xml"); + // nh->declare_parameter("bt_xml", "bt_nav_mememan.xml"); + // nh->declare_parameter("bt_xml", "bt_yac_supply.xml"); + nh->declare_parameter("bt_xml", "bt_nav_yac_supply.xml"); std::string bt_xml; nh->get_parameter("bt_xml", bt_xml); bt_xml = packagePath + "/bt_xml/" + bt_xml; @@ -30,6 +35,9 @@ int main(int argc, char** argv) // RCLCPP_INFO(nh->get_logger(), "Loading XML : %s", bt_xml.c_str()); factory.registerNodeType("Nav2Client"); + factory.registerNodeType("YacSupplyCilent"); + factory.registerNodeType("QRClient"); + factory.registerNodeType("Message"); // Trees are created at deployment-time (i.e. at run-time, but only once at // the beginning). The currently supported format is XML. IMPORTANT: when the diff --git a/yacyac_core/src/send_goal3.cpp b/yacyac_core/src/send_goal3.cpp new file mode 100644 index 0000000..8d9976f --- /dev/null +++ b/yacyac_core/src/send_goal3.cpp @@ -0,0 +1,94 @@ +#include +#include +#include +#include +#include + +#include "yacyac_interface/action/supply.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include "rclcpp_components/register_node_macro.hpp" + +namespace action_tutorials_cpp { +class FibonacciActionClient : public rclcpp::Node { +public: + using Fibonacci = yacyac_interface::action::Supply; + using GoalHandleFibonacci = rclcpp_action::ClientGoalHandle; + + explicit FibonacciActionClient(const rclcpp::NodeOptions& options) : Node("fibonacci_action_client", options) + { + this->client_ptr_ = rclcpp_action::create_client(this, "fibonacci"); + + this->timer_ = this->create_wall_timer(std::chrono::milliseconds(500), std::bind(&FibonacciActionClient::send_goal, this)); + } + + void send_goal() + { + using namespace std::placeholders; + + this->timer_->cancel(); + + if (!this->client_ptr_->wait_for_action_server()) { + RCLCPP_ERROR(this->get_logger(), "Action server not available after waiting"); + rclcpp::shutdown(); + } + + auto goal_msg = Fibonacci::Goal(); + goal_msg.yac_supply_list[0] = 6; + + RCLCPP_INFO(this->get_logger(), "Sending goal"); + + auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); + send_goal_options.goal_response_callback = std::bind(&FibonacciActionClient::goal_response_callback, this, _1); + send_goal_options.feedback_callback = std::bind(&FibonacciActionClient::feedback_callback, this, _1, _2); + send_goal_options.result_callback = std::bind(&FibonacciActionClient::result_callback, this, _1); + this->client_ptr_->async_send_goal(goal_msg, send_goal_options); + } + +private: + rclcpp_action::Client::SharedPtr client_ptr_; + rclcpp::TimerBase::SharedPtr timer_; + + void goal_response_callback(std::shared_future future) + { + auto goal_handle = future.get(); + if (!goal_handle) { + RCLCPP_ERROR(this->get_logger(), "Goal was rejected by server"); + } + else { + RCLCPP_INFO(this->get_logger(), "Goal accepted by server, waiting for result"); + } + } + + void feedback_callback(GoalHandleFibonacci::SharedPtr, const std::shared_ptr feedback) + { + std::stringstream ss; + ss << "Next number in sequence received: "; + for (auto number : feedback->partial_sequence) { + ss << number << " "; + } + RCLCPP_INFO(this->get_logger(), ss.str().c_str()); + } + + void result_callback(const GoalHandleFibonacci::WrappedResult& result) + { + switch (result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: break; + case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR(this->get_logger(), "Goal was aborted"); return; + case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR(this->get_logger(), "Goal was canceled"); return; + default: RCLCPP_ERROR(this->get_logger(), "Unknown result code"); return; + } + std::stringstream ss; + ss << "Result received: "; + for (auto number : result.result->sequence) { + ss << number << " "; + } + RCLCPP_INFO(this->get_logger(), ss.str().c_str()); + rclcpp::shutdown(); + } +}; // class FibonacciActionClient + +} // namespace action_tutorials_cpp + +RCLCPP_COMPONENTS_REGISTER_NODE(action_tutorials_cpp::FibonacciActionClient) \ No newline at end of file diff --git a/yacyac_core/src/yac_supply_client.cpp b/yacyac_core/src/yac_supply_client.cpp new file mode 100644 index 0000000..50bd5fd --- /dev/null +++ b/yacyac_core/src/yac_supply_client.cpp @@ -0,0 +1,134 @@ +// // #include "yacyac_core/nav2_clinent.hpp" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/rclcpp_action.hpp" +#include +#include +#include +#include + +#include "behaviortree_cpp/action_node.h" +#include "std_msgs/msg/header.hpp" +#include "yacyac_interface/action/supply.hpp" + +// Custom type +struct YacSupplyList { + int yac_supply_list[8]; +}; + +namespace BT { +template <> +inline YacSupplyList convertFromString(StringView key) +{ + // three real numbers separated by semicolons + auto parts = BT::splitString(key, ';'); + if (parts.size() != 8) { + throw BT::RuntimeError("invalid input)"); + } + else { + YacSupplyList output; + + for (int i = 0; i < 8; i++) { + output.yac_supply_list[i] = convertFromString(parts[i]); + } + std::cout << "yac supply list" << std::endl; + for (int i = 0; i < 8; i++) { + std::cout << output.yac_supply_list[i] << " "; + } + std::cout << std::endl; + return output; + } +} +} // namespace BT +// SyncActionNode + +// end namespace BT + +class YacSupplyCilent : public BT::SyncActionNode { +public: + YacSupplyCilent(const std::string& name, const BT::NodeConfig& config) : BT::SyncActionNode(name, config) { std::cout << "yac supplyt client start" << std::endl; } + + static BT::PortsList providedPorts() { return { BT::InputPort("goal") }; } + + virtual BT::NodeStatus tick() override + { + std::cout << "yac supply client tick" << std::endl; + node_ = rclcpp::Node::make_shared("yac_supply_client"); + auto action_client = rclcpp_action::create_client(node_, "yacyac/supply_action"); + // if no server is present, fail after 5 seconds + std::cout << "yac supply server wait" << std::endl; + if (!action_client->wait_for_action_server(std::chrono::seconds(20))) { + RCLCPP_ERROR(node_->get_logger(), "Action server not available after waiting"); + return BT::NodeStatus::FAILURE; + } + // Take the goal from the InputPort of the Node + YacSupplyList goal; + if (!getInput("goal", goal)) { + // if I can't get this, there is something wrong with your BT. + // For this reason throw an exception instead of returning FAILURE + throw BT::RuntimeError("missing required input [goal]"); + } + + _aborted = false; + + RCLCPP_INFO(node_->get_logger(), "Yac supply Sending goal %d %d %d %d %d %d %d %d", goal.yac_supply_list[0], goal.yac_supply_list[1], goal.yac_supply_list[2], goal.yac_supply_list[3], + goal.yac_supply_list[4], goal.yac_supply_list[5], goal.yac_supply_list[6], goal.yac_supply_list[7]); + + // #include "yacyac_interface/action/supply.hpp" + + auto goal_msg = yacyac_interface::action::Supply::Goal(); + // yacyac_interface::action::Supply goal_msg; + // msg->qr_infos + for (int i = 0; i < 8; i++) { + goal_msg.yac_supply_list[i] = (goal.yac_supply_list[i]); + std::cout << goal_msg.yac_supply_list[i] << " "; + } + std::cout << std::endl; + // // goal_msg.yac_supply_list[0] = 8; + + auto goal_handle_future = action_client->async_send_goal(goal_msg); + if (rclcpp::spin_until_future_complete(node_, goal_handle_future) != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR(node_->get_logger(), "send goal call failed"); + return BT::NodeStatus::FAILURE; + } + + rclcpp_action::ClientGoalHandle::SharedPtr goal_handle = goal_handle_future.get(); + if (!goal_handle) { + RCLCPP_ERROR(node_->get_logger(), "Goal was rejected by server"); + return BT::NodeStatus::FAILURE; + } + + auto result_future = action_client->async_get_result(goal_handle); + + RCLCPP_INFO(node_->get_logger(), "Waiting for result"); + if (rclcpp::spin_until_future_complete(node_, result_future) != rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR(node_->get_logger(), "get result call failed "); + return BT::NodeStatus::FAILURE; + } + + rclcpp_action::ClientGoalHandle::WrappedResult wrapped_result = result_future.get(); + + switch (wrapped_result.code) { + case rclcpp_action::ResultCode::SUCCEEDED: break; + case rclcpp_action::ResultCode::ABORTED: RCLCPP_ERROR(node_->get_logger(), "Goal was aborted"); return BT::NodeStatus::FAILURE; + case rclcpp_action::ResultCode::CANCELED: RCLCPP_ERROR(node_->get_logger(), "Goal was canceled"); return BT::NodeStatus::FAILURE; + default: RCLCPP_ERROR(node_->get_logger(), "Unknown result code"); return BT::NodeStatus::FAILURE; + } + + if (_aborted) { + // this happens only if method halt() was invoked + //_client.cancelAllGoals(); + RCLCPP_INFO(node_->get_logger(), "Yac supply aborted"); + return BT::NodeStatus::FAILURE; + } + + RCLCPP_INFO(node_->get_logger(), "Yac supply return received"); + return BT::NodeStatus::SUCCESS; + } + +private: + bool _aborted; + + // auto node_ = std::make_shared("nav2_client"); + rclcpp::Node::SharedPtr node_; +}; \ No newline at end of file diff --git a/yacyac_interface/action/Supply.action b/yacyac_interface/action/Supply.action index 7d00a07..86f8d05 100644 --- a/yacyac_interface/action/Supply.action +++ b/yacyac_interface/action/Supply.action @@ -1,4 +1,4 @@ -uint8[8] supply_list +uint8[8] yac_supply_list --- uint8 sequence --- diff --git a/yacyac_servo/yacyac_servo/action_servo_ctrl.py b/yacyac_servo/yacyac_servo/action_servo_ctrl.py index 704b217..566aca5 100644 --- a/yacyac_servo/yacyac_servo/action_servo_ctrl.py +++ b/yacyac_servo/yacyac_servo/action_servo_ctrl.py @@ -56,7 +56,7 @@ def __init__(self): def execute_callback(self, goal_handle): print('제조 정보를 입력 받았습니다...') - supply_list = list(goal_handle.request.supply_list) + supply_list = list(goal_handle.request.yac_supply_list) print("약 제조 리스트") print("list : ", supply_list) From 8d81ad2b17a13659c4760bd50df827ac07123c6a Mon Sep 17 00:00:00 2001 From: KDW Date: Sat, 26 Aug 2023 01:37:36 +0900 Subject: [PATCH 6/6] DynamixelSDK re formatting --- .../include/dynamixel_sdk/dynamixel_sdk.h | 30 +- .../include/dynamixel_sdk/group_bulk_read.h | 288 ++- .../include/dynamixel_sdk/group_bulk_write.h | 208 ++- .../include/dynamixel_sdk/group_sync_read.h | 296 ++-- .../include/dynamixel_sdk/group_sync_write.h | 202 ++- .../include/dynamixel_sdk/packet_handler.h | 1115 ++++++------ .../include/dynamixel_sdk/port_handler.h | 275 ++- .../dynamixel_sdk/port_handler_arduino.h | 310 ++-- .../dynamixel_sdk/port_handler_linux.h | 298 ++-- .../include/dynamixel_sdk/port_handler_mac.h | 300 ++-- .../dynamixel_sdk/port_handler_windows.h | 297 ++-- .../dynamixel_sdk/protocol1_packet_handler.h | 1031 ++++++----- .../dynamixel_sdk/protocol2_packet_handler.h | 1065 ++++++------ .../src/dynamixel_sdk/group_bulk_read.cpp | 287 ++- .../src/dynamixel_sdk/group_bulk_write.cpp | 210 ++- .../src/dynamixel_sdk/group_sync_read.cpp | 222 ++- .../src/dynamixel_sdk/group_sync_write.cpp | 149 +- .../src/dynamixel_sdk/packet_handler.cpp | 46 +- .../src/dynamixel_sdk/port_handler.cpp | 38 +- .../dynamixel_sdk/port_handler_arduino.cpp | 273 ++- .../src/dynamixel_sdk/port_handler_linux.cpp | 336 ++-- .../src/dynamixel_sdk/port_handler_mac.cpp | 245 ++- .../dynamixel_sdk/port_handler_windows.cpp | 281 ++- .../protocol1_packet_handler.cpp | 1021 ++++++----- .../protocol2_packet_handler.cpp | 1543 ++++++++--------- serial-ros2/include/serial/serial.h | 20 +- 26 files changed, 5055 insertions(+), 5331 deletions(-) mode change 100755 => 100644 DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp diff --git a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/dynamixel_sdk.h b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/dynamixel_sdk.h index 9e3bc7f..55b5b59 100644 --- a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/dynamixel_sdk.h +++ b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/dynamixel_sdk.h @@ -1,18 +1,18 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ //////////////////////////////////////////////////////////////////////////////// /// @file The file that includes whole Dynamixel SDK libraries @@ -22,7 +22,6 @@ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_DYNAMIXELSDK_H_ #define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_DYNAMIXELSDK_H_ - #include "group_bulk_read.h" #include "group_bulk_write.h" #include "group_sync_read.h" @@ -30,5 +29,4 @@ #include "packet_handler.h" #include "port_handler.h" - #endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_DYNAMIXELSDK_H_ */ diff --git a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/group_bulk_read.h b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/group_bulk_read.h index 624c00d..ee177e0 100644 --- a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/group_bulk_read.h +++ b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/group_bulk_read.h @@ -1,18 +1,18 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ //////////////////////////////////////////////////////////////////////////////// /// @file The file for Dynamixel Bulk Read @@ -22,146 +22,142 @@ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ #define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ - +#include "packet_handler.h" +#include "port_handler.h" #include #include -#include "port_handler.h" -#include "packet_handler.h" -namespace dynamixel -{ +namespace dynamixel { //////////////////////////////////////////////////////////////////////////////// /// @brief The class for reading multiple Dynamixel data from different addresses with different lengths at once //////////////////////////////////////////////////////////////////////////////// -class WINDECLSPEC GroupBulkRead -{ - private: - PortHandler *port_; - PacketHandler *ph_; - - std::vector id_list_; - std::map address_list_; // - std::map length_list_; // - std::map data_list_; // - std::map error_list_; // - - bool last_result_; - bool is_param_changed_; - - uint8_t *param_; - - void makeParam(); - - public: - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that Initializes instance for Bulk Read - /// @param port PortHandler instance - /// @param ph PacketHandler instance - //////////////////////////////////////////////////////////////////////////////// - GroupBulkRead(PortHandler *port, PacketHandler *ph); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls clearParam function to clear the parameter list for Bulk Read - //////////////////////////////////////////////////////////////////////////////// - ~GroupBulkRead() { clearParam(); } - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that returns PortHandler instance - /// @return PortHandler instance - //////////////////////////////////////////////////////////////////////////////// - PortHandler *getPortHandler() { return port_; } - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that returns PacketHandler instance - /// @return PacketHandler instance - //////////////////////////////////////////////////////////////////////////////// - PacketHandler *getPacketHandler() { return ph_; } - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that adds id, start_address, data_length to the Bulk Read list - /// @param id Dynamixel ID - /// @param start_address Address of the data for read - /// @data_length Length of the data for read - /// @return false - /// @return when the ID exists already in the list - /// @return or true - //////////////////////////////////////////////////////////////////////////////// - bool addParam (uint8_t id, uint16_t start_address, uint16_t data_length); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that removes id from the Bulk Read list - /// @param id Dynamixel ID - //////////////////////////////////////////////////////////////////////////////// - void removeParam (uint8_t id); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that clears the Bulk Read list - //////////////////////////////////////////////////////////////////////////////// - void clearParam (); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits the Bulk Read instruction packet which might be constructed by GroupBulkRead::addParam function - /// @return COMM_NOT_AVAILABLE - /// @return when the list for Bulk Read is empty - /// @return or the other communication results which come from PacketHandler::bulkReadTx - //////////////////////////////////////////////////////////////////////////////// - int txPacket(); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that receives the packet which might be come from the Dynamixel - /// @return COMM_NOT_AVAILABLE - /// @return when the list for Bulk Read is empty - /// @return COMM_RX_FAIL - /// @return when there is no packet recieved - /// @return COMM_SUCCESS - /// @return when there is packet recieved - /// @return or the other communication results - //////////////////////////////////////////////////////////////////////////////// - int rxPacket(); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits and receives the packet which might be come from the Dynamixel - /// @return COMM_RX_FAIL - /// @return when there is no packet recieved - /// @return COMM_SUCCESS - /// @return when there is packet recieved - /// @return or the other communication results which come from GroupBulkRead::txPacket or GroupBulkRead::rxPacket - //////////////////////////////////////////////////////////////////////////////// - int txRxPacket(); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that checks whether there are available data which might be received by GroupBulkRead::rxPacket or GroupBulkRead::txRxPacket - /// @param id Dynamixel ID - /// @param address Address of the data for read - /// @param data_length Length of the data for read - /// @return false - /// @return when there are no data available - /// @return or true - //////////////////////////////////////////////////////////////////////////////// - bool isAvailable (uint8_t id, uint16_t address, uint16_t data_length); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that gets the data which might be received by GroupBulkRead::rxPacket or GroupBulkRead::txRxPacket - /// @param id Dynamixel ID - /// @param address Address of the data for read - /// @data_length Length of the data for read - /// @return data value - //////////////////////////////////////////////////////////////////////////////// - uint32_t getData (uint8_t id, uint16_t address, uint16_t data_length); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that gets the error which might be received by GroupBulkRead::rxPacket or GroupBulkRead::txRxPacket - /// @param id Dynamixel ID - /// @error error of Dynamixel - /// @return true - /// @return when Dynamixel returned specific error byte - /// @return or false - //////////////////////////////////////////////////////////////////////////////// - bool getError (uint8_t id, uint8_t* error); +class WINDECLSPEC GroupBulkRead { +private: + PortHandler* port_; + PacketHandler* ph_; + + std::vector id_list_; + std::map address_list_; // + std::map length_list_; // + std::map data_list_; // + std::map error_list_; // + + bool last_result_; + bool is_param_changed_; + + uint8_t* param_; + + void makeParam(); + +public: + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that Initializes instance for Bulk Read + /// @param port PortHandler instance + /// @param ph PacketHandler instance + //////////////////////////////////////////////////////////////////////////////// + GroupBulkRead(PortHandler* port, PacketHandler* ph); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls clearParam function to clear the parameter list for Bulk Read + //////////////////////////////////////////////////////////////////////////////// + ~GroupBulkRead() { clearParam(); } + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns PortHandler instance + /// @return PortHandler instance + //////////////////////////////////////////////////////////////////////////////// + PortHandler* getPortHandler() { return port_; } + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns PacketHandler instance + /// @return PacketHandler instance + //////////////////////////////////////////////////////////////////////////////// + PacketHandler* getPacketHandler() { return ph_; } + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that adds id, start_address, data_length to the Bulk Read list + /// @param id Dynamixel ID + /// @param start_address Address of the data for read + /// @data_length Length of the data for read + /// @return false + /// @return when the ID exists already in the list + /// @return or true + //////////////////////////////////////////////////////////////////////////////// + bool addParam(uint8_t id, uint16_t start_address, uint16_t data_length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that removes id from the Bulk Read list + /// @param id Dynamixel ID + //////////////////////////////////////////////////////////////////////////////// + void removeParam(uint8_t id); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that clears the Bulk Read list + //////////////////////////////////////////////////////////////////////////////// + void clearParam(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits the Bulk Read instruction packet which might be constructed by GroupBulkRead::addParam function + /// @return COMM_NOT_AVAILABLE + /// @return when the list for Bulk Read is empty + /// @return or the other communication results which come from PacketHandler::bulkReadTx + //////////////////////////////////////////////////////////////////////////////// + int txPacket(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that receives the packet which might be come from the Dynamixel + /// @return COMM_NOT_AVAILABLE + /// @return when the list for Bulk Read is empty + /// @return COMM_RX_FAIL + /// @return when there is no packet recieved + /// @return COMM_SUCCESS + /// @return when there is packet recieved + /// @return or the other communication results + //////////////////////////////////////////////////////////////////////////////// + int rxPacket(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits and receives the packet which might be come from the Dynamixel + /// @return COMM_RX_FAIL + /// @return when there is no packet recieved + /// @return COMM_SUCCESS + /// @return when there is packet recieved + /// @return or the other communication results which come from GroupBulkRead::txPacket or GroupBulkRead::rxPacket + //////////////////////////////////////////////////////////////////////////////// + int txRxPacket(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that checks whether there are available data which might be received by GroupBulkRead::rxPacket or GroupBulkRead::txRxPacket + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @param data_length Length of the data for read + /// @return false + /// @return when there are no data available + /// @return or true + //////////////////////////////////////////////////////////////////////////////// + bool isAvailable(uint8_t id, uint16_t address, uint16_t data_length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that gets the data which might be received by GroupBulkRead::rxPacket or GroupBulkRead::txRxPacket + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @data_length Length of the data for read + /// @return data value + //////////////////////////////////////////////////////////////////////////////// + uint32_t getData(uint8_t id, uint16_t address, uint16_t data_length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that gets the error which might be received by GroupBulkRead::rxPacket or GroupBulkRead::txRxPacket + /// @param id Dynamixel ID + /// @error error of Dynamixel + /// @return true + /// @return when Dynamixel returned specific error byte + /// @return or false + //////////////////////////////////////////////////////////////////////////////// + bool getError(uint8_t id, uint8_t* error); }; -} - +} // namespace dynamixel #endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKREAD_H_ */ diff --git a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/group_bulk_write.h b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/group_bulk_write.h index f3c2af2..8d65ab3 100644 --- a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/group_bulk_write.h +++ b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/group_bulk_write.h @@ -1,18 +1,18 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ //////////////////////////////////////////////////////////////////////////////// /// @file The file for Dynamixel Bulk Write @@ -22,106 +22,102 @@ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ #define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ - +#include "packet_handler.h" +#include "port_handler.h" #include #include -#include "port_handler.h" -#include "packet_handler.h" -namespace dynamixel -{ +namespace dynamixel { //////////////////////////////////////////////////////////////////////////////// /// @brief The class for writing multiple Dynamixel data from different addresses with different lengths at once //////////////////////////////////////////////////////////////////////////////// -class WINDECLSPEC GroupBulkWrite -{ - private: - PortHandler *port_; - PacketHandler *ph_; - - std::vector id_list_; - std::map address_list_; // - std::map length_list_; // - std::map data_list_; // - - bool is_param_changed_; - - uint8_t *param_; - uint16_t param_length_; - - void makeParam(); - - public: - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that Initializes instance for Bulk Write - /// @param port PortHandler instance - /// @param ph PacketHandler instance - //////////////////////////////////////////////////////////////////////////////// - GroupBulkWrite(PortHandler *port, PacketHandler *ph); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls clearParam function to clear the parameter list for Bulk Write - //////////////////////////////////////////////////////////////////////////////// - ~GroupBulkWrite() { clearParam(); } - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that returns PortHandler instance - /// @return PortHandler instance - //////////////////////////////////////////////////////////////////////////////// - PortHandler *getPortHandler() { return port_; } - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that returns PacketHandler instance - /// @return PacketHandler instance - //////////////////////////////////////////////////////////////////////////////// - PacketHandler *getPacketHandler() { return ph_; } - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that adds id, start_address, data_length to the Bulk Write list - /// @param id Dynamixel ID - /// @param start_address Address of the data for write - /// @param data_length Length of the data for write - /// @return false - /// @return when the ID exists already in the list - /// @return or true - //////////////////////////////////////////////////////////////////////////////// - bool addParam (uint8_t id, uint16_t start_address, uint16_t data_length, uint8_t *data); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that removes id from the Bulk Write list - /// @param id Dynamixel ID - //////////////////////////////////////////////////////////////////////////////// - void removeParam (uint8_t id); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that changes the data for write in id -> start_address -> data_length to the Bulk Write list - /// @param id Dynamixel ID - /// @param start_address Address of the data for write - /// @param data_length Length of the data for write - /// @param data for replacement - /// @return false - /// @return when the ID doesn't exist in the list - /// @return or true - //////////////////////////////////////////////////////////////////////////////// - bool changeParam (uint8_t id, uint16_t start_address, uint16_t data_length, uint8_t *data); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that clears the Bulk Write list - //////////////////////////////////////////////////////////////////////////////// - void clearParam (); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits the Bulk Write instruction packet which might be constructed by GroupBulkWrite::addParam function - /// @return COMM_NOT_AVAILABLE - /// @return when the list for Bulk Write is empty - /// @return when Protocol1.0 has been used - /// @return or the other communication results which come from PacketHandler::bulkWriteTxOnly - //////////////////////////////////////////////////////////////////////////////// - int txPacket(); +class WINDECLSPEC GroupBulkWrite { +private: + PortHandler* port_; + PacketHandler* ph_; + + std::vector id_list_; + std::map address_list_; // + std::map length_list_; // + std::map data_list_; // + + bool is_param_changed_; + + uint8_t* param_; + uint16_t param_length_; + + void makeParam(); + +public: + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that Initializes instance for Bulk Write + /// @param port PortHandler instance + /// @param ph PacketHandler instance + //////////////////////////////////////////////////////////////////////////////// + GroupBulkWrite(PortHandler* port, PacketHandler* ph); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls clearParam function to clear the parameter list for Bulk Write + //////////////////////////////////////////////////////////////////////////////// + ~GroupBulkWrite() { clearParam(); } + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns PortHandler instance + /// @return PortHandler instance + //////////////////////////////////////////////////////////////////////////////// + PortHandler* getPortHandler() { return port_; } + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns PacketHandler instance + /// @return PacketHandler instance + //////////////////////////////////////////////////////////////////////////////// + PacketHandler* getPacketHandler() { return ph_; } + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that adds id, start_address, data_length to the Bulk Write list + /// @param id Dynamixel ID + /// @param start_address Address of the data for write + /// @param data_length Length of the data for write + /// @return false + /// @return when the ID exists already in the list + /// @return or true + //////////////////////////////////////////////////////////////////////////////// + bool addParam(uint8_t id, uint16_t start_address, uint16_t data_length, uint8_t* data); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that removes id from the Bulk Write list + /// @param id Dynamixel ID + //////////////////////////////////////////////////////////////////////////////// + void removeParam(uint8_t id); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that changes the data for write in id -> start_address -> data_length to the Bulk Write list + /// @param id Dynamixel ID + /// @param start_address Address of the data for write + /// @param data_length Length of the data for write + /// @param data for replacement + /// @return false + /// @return when the ID doesn't exist in the list + /// @return or true + //////////////////////////////////////////////////////////////////////////////// + bool changeParam(uint8_t id, uint16_t start_address, uint16_t data_length, uint8_t* data); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that clears the Bulk Write list + //////////////////////////////////////////////////////////////////////////////// + void clearParam(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits the Bulk Write instruction packet which might be constructed by GroupBulkWrite::addParam function + /// @return COMM_NOT_AVAILABLE + /// @return when the list for Bulk Write is empty + /// @return when Protocol1.0 has been used + /// @return or the other communication results which come from PacketHandler::bulkWriteTxOnly + //////////////////////////////////////////////////////////////////////////////// + int txPacket(); }; -} - +} // namespace dynamixel #endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPBULKWRITE_H_ */ diff --git a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/group_sync_read.h b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/group_sync_read.h index 0b934f6..62749a1 100644 --- a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/group_sync_read.h +++ b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/group_sync_read.h @@ -1,18 +1,18 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ //////////////////////////////////////////////////////////////////////////////// /// @file The file for Dynamixel Sync Read @@ -22,150 +22,146 @@ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ #define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ - +#include "packet_handler.h" +#include "port_handler.h" #include #include -#include "port_handler.h" -#include "packet_handler.h" -namespace dynamixel -{ +namespace dynamixel { //////////////////////////////////////////////////////////////////////////////// /// @brief The class for reading multiple Dynamixel data from same address with same length at once //////////////////////////////////////////////////////////////////////////////// -class WINDECLSPEC GroupSyncRead -{ - private: - PortHandler *port_; - PacketHandler *ph_; - - std::vector id_list_; - std::map data_list_; // - std::map error_list_; // - - bool last_result_; - bool is_param_changed_; - - uint8_t *param_; - uint16_t start_address_; - uint16_t data_length_; - - void makeParam(); - - public: - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that Initializes instance for Sync Read - /// @param port PortHandler instance - /// @param ph PacketHandler instance - /// @param start_address Address of the data for read - /// @param data_length Length of the data for read - //////////////////////////////////////////////////////////////////////////////// - GroupSyncRead(PortHandler *port, PacketHandler *ph, uint16_t start_address, uint16_t data_length); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls clearParam function to clear the parameter list for Sync Read - //////////////////////////////////////////////////////////////////////////////// - ~GroupSyncRead() { clearParam(); } - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that returns PortHandler instance - /// @return PortHandler instance - //////////////////////////////////////////////////////////////////////////////// - PortHandler *getPortHandler() { return port_; } - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that returns PacketHandler instance - /// @return PacketHandler instance - //////////////////////////////////////////////////////////////////////////////// - PacketHandler *getPacketHandler() { return ph_; } - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that adds id, start_address, data_length to the Sync Read list - /// @param id Dynamixel ID - /// @return false - /// @return when the ID exists already in the list - /// @return when the protocol1.0 has been used - /// @return or true - //////////////////////////////////////////////////////////////////////////////// - bool addParam (uint8_t id); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that removes id from the Sync Read list - /// @param id Dynamixel ID - //////////////////////////////////////////////////////////////////////////////// - void removeParam (uint8_t id); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that clears the Sync Read list - //////////////////////////////////////////////////////////////////////////////// - void clearParam (); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits the Sync Read instruction packet which might be constructed by GroupSyncRead::addParam function - /// @return COMM_NOT_AVAILABLE - /// @return when the list for Sync Read is empty - /// @return when the protocol1.0 has been used - /// @return or the other communication results which come from PacketHandler::syncReadTx - //////////////////////////////////////////////////////////////////////////////// - int txPacket(); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that receives the packet which might be come from the Dynamixel - /// @return COMM_NOT_AVAILABLE - /// @return when the list for Sync Read is empty - /// @return when the protocol1.0 has been used - /// @return COMM_SUCCESS - /// @return when there is packet recieved - /// @return or the other communication results - //////////////////////////////////////////////////////////////////////////////// - int rxPacket(); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits and receives the packet which might be come from the Dynamixel - /// @return COMM_NOT_AVAILABLE - /// @return when the protocol1.0 has been used - /// @return COMM_RX_FAIL - /// @return when there is no packet recieved - /// @return COMM_SUCCESS - /// @return when there is packet recieved - /// @return or the other communication results which come from GroupBulkRead::txPacket or GroupBulkRead::rxPacket - //////////////////////////////////////////////////////////////////////////////// - int txRxPacket(); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that checks whether there are available data which might be received by GroupSyncRead::rxPacket or GroupSyncRead::txRxPacket - /// @param id Dynamixel ID - /// @param address Address of the data for read - /// @param data_length Length of the data for read - /// @return false - /// @return when there are no data available - /// @return when the protocol1.0 has been used - /// @return or true - //////////////////////////////////////////////////////////////////////////////// - bool isAvailable (uint8_t id, uint16_t address, uint16_t data_length); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that gets the data which might be received by GroupSyncRead::rxPacket or GroupSyncRead::txRxPacket - /// @param id Dynamixel ID - /// @param address Address of the data for read - /// @data_length Length of the data for read - /// @return data value - //////////////////////////////////////////////////////////////////////////////// - uint32_t getData (uint8_t id, uint16_t address, uint16_t data_length); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that gets the error which might be received by GroupSyncRead::rxPacket or GroupSyncRead::txRxPacket - /// @param id Dynamixel ID - /// @error error of Dynamixel - /// @return true - /// @return when Dynamixel returned specific error byte - /// @return or false - //////////////////////////////////////////////////////////////////////////////// - bool getError (uint8_t id, uint8_t* error); +class WINDECLSPEC GroupSyncRead { +private: + PortHandler* port_; + PacketHandler* ph_; + + std::vector id_list_; + std::map data_list_; // + std::map error_list_; // + + bool last_result_; + bool is_param_changed_; + + uint8_t* param_; + uint16_t start_address_; + uint16_t data_length_; + + void makeParam(); + +public: + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that Initializes instance for Sync Read + /// @param port PortHandler instance + /// @param ph PacketHandler instance + /// @param start_address Address of the data for read + /// @param data_length Length of the data for read + //////////////////////////////////////////////////////////////////////////////// + GroupSyncRead(PortHandler* port, PacketHandler* ph, uint16_t start_address, uint16_t data_length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls clearParam function to clear the parameter list for Sync Read + //////////////////////////////////////////////////////////////////////////////// + ~GroupSyncRead() { clearParam(); } + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns PortHandler instance + /// @return PortHandler instance + //////////////////////////////////////////////////////////////////////////////// + PortHandler* getPortHandler() { return port_; } + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns PacketHandler instance + /// @return PacketHandler instance + //////////////////////////////////////////////////////////////////////////////// + PacketHandler* getPacketHandler() { return ph_; } + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that adds id, start_address, data_length to the Sync Read list + /// @param id Dynamixel ID + /// @return false + /// @return when the ID exists already in the list + /// @return when the protocol1.0 has been used + /// @return or true + //////////////////////////////////////////////////////////////////////////////// + bool addParam(uint8_t id); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that removes id from the Sync Read list + /// @param id Dynamixel ID + //////////////////////////////////////////////////////////////////////////////// + void removeParam(uint8_t id); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that clears the Sync Read list + //////////////////////////////////////////////////////////////////////////////// + void clearParam(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits the Sync Read instruction packet which might be constructed by GroupSyncRead::addParam function + /// @return COMM_NOT_AVAILABLE + /// @return when the list for Sync Read is empty + /// @return when the protocol1.0 has been used + /// @return or the other communication results which come from PacketHandler::syncReadTx + //////////////////////////////////////////////////////////////////////////////// + int txPacket(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that receives the packet which might be come from the Dynamixel + /// @return COMM_NOT_AVAILABLE + /// @return when the list for Sync Read is empty + /// @return when the protocol1.0 has been used + /// @return COMM_SUCCESS + /// @return when there is packet recieved + /// @return or the other communication results + //////////////////////////////////////////////////////////////////////////////// + int rxPacket(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits and receives the packet which might be come from the Dynamixel + /// @return COMM_NOT_AVAILABLE + /// @return when the protocol1.0 has been used + /// @return COMM_RX_FAIL + /// @return when there is no packet recieved + /// @return COMM_SUCCESS + /// @return when there is packet recieved + /// @return or the other communication results which come from GroupBulkRead::txPacket or GroupBulkRead::rxPacket + //////////////////////////////////////////////////////////////////////////////// + int txRxPacket(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that checks whether there are available data which might be received by GroupSyncRead::rxPacket or GroupSyncRead::txRxPacket + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @param data_length Length of the data for read + /// @return false + /// @return when there are no data available + /// @return when the protocol1.0 has been used + /// @return or true + //////////////////////////////////////////////////////////////////////////////// + bool isAvailable(uint8_t id, uint16_t address, uint16_t data_length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that gets the data which might be received by GroupSyncRead::rxPacket or GroupSyncRead::txRxPacket + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @data_length Length of the data for read + /// @return data value + //////////////////////////////////////////////////////////////////////////////// + uint32_t getData(uint8_t id, uint16_t address, uint16_t data_length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that gets the error which might be received by GroupSyncRead::rxPacket or GroupSyncRead::txRxPacket + /// @param id Dynamixel ID + /// @error error of Dynamixel + /// @return true + /// @return when Dynamixel returned specific error byte + /// @return or false + //////////////////////////////////////////////////////////////////////////////// + bool getError(uint8_t id, uint8_t* error); }; -} - +} // namespace dynamixel #endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCREAD_H_ */ diff --git a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/group_sync_write.h b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/group_sync_write.h index 81f5e8c..eee2054 100644 --- a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/group_sync_write.h +++ b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/group_sync_write.h @@ -1,18 +1,18 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ //////////////////////////////////////////////////////////////////////////////// /// @file The file for Dynamixel Sync Write @@ -22,103 +22,99 @@ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ #define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ - +#include "packet_handler.h" +#include "port_handler.h" #include #include -#include "port_handler.h" -#include "packet_handler.h" -namespace dynamixel -{ +namespace dynamixel { //////////////////////////////////////////////////////////////////////////////// /// @brief The class for writing multiple Dynamixel data from same address with same length at once //////////////////////////////////////////////////////////////////////////////// -class WINDECLSPEC GroupSyncWrite -{ - private: - PortHandler *port_; - PacketHandler *ph_; - - std::vector id_list_; - std::map data_list_; // - - bool is_param_changed_; - - uint8_t *param_; - uint16_t start_address_; - uint16_t data_length_; - - void makeParam(); - - public: - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that Initializes instance for Sync Write - /// @param port PortHandler instance - /// @param ph PacketHandler instance - /// @param start_address Address of the data for write - /// @param data_length Length of the data for write - //////////////////////////////////////////////////////////////////////////////// - GroupSyncWrite(PortHandler *port, PacketHandler *ph, uint16_t start_address, uint16_t data_length); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls clearParam function to clear the parameter list for Sync Write - //////////////////////////////////////////////////////////////////////////////// - ~GroupSyncWrite() { clearParam(); } - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that returns PortHandler instance - /// @return PortHandler instance - //////////////////////////////////////////////////////////////////////////////// - PortHandler *getPortHandler() { return port_; } - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that returns PacketHandler instance - /// @return PacketHandler instance - //////////////////////////////////////////////////////////////////////////////// - PacketHandler *getPacketHandler() { return ph_; } - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that adds id, start_address, data_length to the Sync Write list - /// @param id Dynamixel ID - /// @param data Data for write - /// @return false - /// @return when the ID exists already in the list - /// @return or true - //////////////////////////////////////////////////////////////////////////////// - bool addParam (uint8_t id, uint8_t *data); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that removes id from the Sync Write list - /// @param id Dynamixel ID - //////////////////////////////////////////////////////////////////////////////// - void removeParam (uint8_t id); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that changes the data for write in id -> start_address -> data_length to the Sync Write list - /// @param id Dynamixel ID - /// @param data for replacement - /// @return false - /// @return when the ID doesn't exist in the list - /// @return or true - //////////////////////////////////////////////////////////////////////////////// - bool changeParam (uint8_t id, uint8_t *data); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that clears the Sync Write list - //////////////////////////////////////////////////////////////////////////////// - void clearParam (); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits the Sync Write instruction packet which might be constructed by GroupSyncWrite::addParam function - /// @return COMM_NOT_AVAILABLE - /// @return when the list for Sync Write is empty - /// @return or the other communication results which come from PacketHandler::syncWriteTxOnly - //////////////////////////////////////////////////////////////////////////////// - int txPacket(); +class WINDECLSPEC GroupSyncWrite { +private: + PortHandler* port_; + PacketHandler* ph_; + + std::vector id_list_; + std::map data_list_; // + + bool is_param_changed_; + + uint8_t* param_; + uint16_t start_address_; + uint16_t data_length_; + + void makeParam(); + +public: + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that Initializes instance for Sync Write + /// @param port PortHandler instance + /// @param ph PacketHandler instance + /// @param start_address Address of the data for write + /// @param data_length Length of the data for write + //////////////////////////////////////////////////////////////////////////////// + GroupSyncWrite(PortHandler* port, PacketHandler* ph, uint16_t start_address, uint16_t data_length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls clearParam function to clear the parameter list for Sync Write + //////////////////////////////////////////////////////////////////////////////// + ~GroupSyncWrite() { clearParam(); } + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns PortHandler instance + /// @return PortHandler instance + //////////////////////////////////////////////////////////////////////////////// + PortHandler* getPortHandler() { return port_; } + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns PacketHandler instance + /// @return PacketHandler instance + //////////////////////////////////////////////////////////////////////////////// + PacketHandler* getPacketHandler() { return ph_; } + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that adds id, start_address, data_length to the Sync Write list + /// @param id Dynamixel ID + /// @param data Data for write + /// @return false + /// @return when the ID exists already in the list + /// @return or true + //////////////////////////////////////////////////////////////////////////////// + bool addParam(uint8_t id, uint8_t* data); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that removes id from the Sync Write list + /// @param id Dynamixel ID + //////////////////////////////////////////////////////////////////////////////// + void removeParam(uint8_t id); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that changes the data for write in id -> start_address -> data_length to the Sync Write list + /// @param id Dynamixel ID + /// @param data for replacement + /// @return false + /// @return when the ID doesn't exist in the list + /// @return or true + //////////////////////////////////////////////////////////////////////////////// + bool changeParam(uint8_t id, uint8_t* data); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that clears the Sync Write list + //////////////////////////////////////////////////////////////////////////////// + void clearParam(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits the Sync Write instruction packet which might be constructed by GroupSyncWrite::addParam function + /// @return COMM_NOT_AVAILABLE + /// @return when the list for Sync Write is empty + /// @return or the other communication results which come from PacketHandler::syncWriteTxOnly + //////////////////////////////////////////////////////////////////////////////// + int txPacket(); }; -} - +} // namespace dynamixel #endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_GROUPSYNCWRITE_H_ */ diff --git a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/packet_handler.h b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/packet_handler.h index 8b20e26..50206c7 100644 --- a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/packet_handler.h +++ b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/packet_handler.h @@ -1,18 +1,18 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ //////////////////////////////////////////////////////////////////////////////// /// @file The file for Dynamixel packet control @@ -25,571 +25,568 @@ #if defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__) #include -#define ERROR_PRINT SerialBT2.print +#define ERROR_PRINT SerialBT2.print #else -#define ERROR_PRINT printf +#define ERROR_PRINT printf #endif +#include "port_handler.h" #include #include -#include "port_handler.h" -#define BROADCAST_ID 0xFE // 254 -#define MAX_ID 0xFC // 252 +#define BROADCAST_ID 0xFE // 254 +#define MAX_ID 0xFC // 252 /* Macro for Control Table Value */ -#define DXL_MAKEWORD(a, b) ((uint16_t)(((uint8_t)(((uint64_t)(a)) & 0xff)) | ((uint16_t)((uint8_t)(((uint64_t)(b)) & 0xff))) << 8)) +#define DXL_MAKEWORD(a, b) ((uint16_t)(((uint8_t)(((uint64_t)(a)) & 0xff)) | ((uint16_t)((uint8_t)(((uint64_t)(b)) & 0xff))) << 8)) #define DXL_MAKEDWORD(a, b) ((uint32_t)(((uint16_t)(((uint64_t)(a)) & 0xffff)) | ((uint32_t)((uint16_t)(((uint64_t)(b)) & 0xffff))) << 16)) -#define DXL_LOWORD(l) ((uint16_t)(((uint64_t)(l)) & 0xffff)) -#define DXL_HIWORD(l) ((uint16_t)((((uint64_t)(l)) >> 16) & 0xffff)) -#define DXL_LOBYTE(w) ((uint8_t)(((uint64_t)(w)) & 0xff)) -#define DXL_HIBYTE(w) ((uint8_t)((((uint64_t)(w)) >> 8) & 0xff)) +#define DXL_LOWORD(l) ((uint16_t)(((uint64_t)(l)) & 0xffff)) +#define DXL_HIWORD(l) ((uint16_t)((((uint64_t)(l)) >> 16) & 0xffff)) +#define DXL_LOBYTE(w) ((uint8_t)(((uint64_t)(w)) & 0xff)) +#define DXL_HIBYTE(w) ((uint8_t)((((uint64_t)(w)) >> 8) & 0xff)) /* Instruction for DXL Protocol */ -#define INST_PING 1 -#define INST_READ 2 -#define INST_WRITE 3 -#define INST_REG_WRITE 4 -#define INST_ACTION 5 -#define INST_FACTORY_RESET 6 -#define INST_SYNC_WRITE 131 // 0x83 -#define INST_BULK_READ 146 // 0x92 +#define INST_PING 1 +#define INST_READ 2 +#define INST_WRITE 3 +#define INST_REG_WRITE 4 +#define INST_ACTION 5 +#define INST_FACTORY_RESET 6 +#define INST_SYNC_WRITE 131 // 0x83 +#define INST_BULK_READ 146 // 0x92 // --- Only for 2.0 --- // -#define INST_REBOOT 8 -#define INST_CLEAR 16 // 0x10 -#define INST_STATUS 85 // 0x55 -#define INST_SYNC_READ 130 // 0x82 -#define INST_BULK_WRITE 147 // 0x93 +#define INST_REBOOT 8 +#define INST_CLEAR 16 // 0x10 +#define INST_STATUS 85 // 0x55 +#define INST_SYNC_READ 130 // 0x82 +#define INST_BULK_WRITE 147 // 0x93 // Communication Result -#define COMM_SUCCESS 0 // tx or rx packet communication success -#define COMM_PORT_BUSY -1000 // Port is busy (in use) -#define COMM_TX_FAIL -1001 // Failed transmit instruction packet -#define COMM_RX_FAIL -1002 // Failed get status packet -#define COMM_TX_ERROR -2000 // Incorrect instruction packet -#define COMM_RX_WAITING -3000 // Now recieving status packet -#define COMM_RX_TIMEOUT -3001 // There is no status packet -#define COMM_RX_CORRUPT -3002 // Incorrect status packet -#define COMM_NOT_AVAILABLE -9000 // - -namespace dynamixel -{ +#define COMM_SUCCESS 0 // tx or rx packet communication success +#define COMM_PORT_BUSY -1000 // Port is busy (in use) +#define COMM_TX_FAIL -1001 // Failed transmit instruction packet +#define COMM_RX_FAIL -1002 // Failed get status packet +#define COMM_TX_ERROR -2000 // Incorrect instruction packet +#define COMM_RX_WAITING -3000 // Now recieving status packet +#define COMM_RX_TIMEOUT -3001 // There is no status packet +#define COMM_RX_CORRUPT -3002 // Incorrect status packet +#define COMM_NOT_AVAILABLE -9000 // + +namespace dynamixel { //////////////////////////////////////////////////////////////////////////////// /// @brief The class that inherits Protocol1PacketHandler class or Protocol2PacketHandler class //////////////////////////////////////////////////////////////////////////////// -class WINDECLSPEC PacketHandler -{ - protected: - PacketHandler() { } - - public: - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that returns PacketHandler instance - /// @return PacketHandler instance - //////////////////////////////////////////////////////////////////////////////// - static PacketHandler *getPacketHandler(float protocol_version = 2.0); - - virtual ~PacketHandler() { } - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that returns Protocol version - /// @return protocol version - //////////////////////////////////////////////////////////////////////////////// - virtual float getProtocolVersion() = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that gets description of communication result - /// @param result Communication result which might be gotten by the tx rx functions - /// @return description of communication result in const char* (string) - //////////////////////////////////////////////////////////////////////////////// - virtual const char *getTxRxResult (int result) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that gets description of hardware error - /// @param error Dynamixel hardware error which might be gotten by the tx rx functions - /// @return description of hardware error in const char* (string) - //////////////////////////////////////////////////////////////////////////////// - virtual const char *getRxPacketError (uint8_t error) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits the instruction packet txpacket via PortHandler port. - /// @description The function clears the port buffer by PortHandler::clearPort() function, - /// @description then transmits txpacket by PortHandler::writePort() function. - /// @description The function activates only when the port is not busy and when the packet is already written on the port buffer - /// @param port PortHandler instance - /// @param txpacket packet for transmission - /// @return COMM_PORT_BUSY - /// @return when the port is already in use - /// @return COMM_TX_ERROR - /// @return when txpacket is out of range described by TXPACKET_MAX_LEN - /// @return COMM_TX_FAIL - /// @return when written packet is shorter than expected - /// @return or COMM_SUCCESS - //////////////////////////////////////////////////////////////////////////////// - virtual int txPacket (PortHandler *port, uint8_t *txpacket) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that receives packet (rxpacket) during designated time via PortHandler port - /// @description The function repeatedly tries to receive rxpacket by PortHandler::readPort() function. - /// @description It breaks out - /// @description when PortHandler::isPacketTimeout() shows the timeout, - /// @description when rxpacket seemed as corrupted, or - /// @description when nothing received - /// @param port PortHandler instance - /// @param rxpacket received packet - /// @return COMM_RX_CORRUPT - /// @return when it received the packet but it couldn't find header in the packet - /// @return when it found header in the packet but the id, length or error value is out of range - /// @return when it received the packet but it is shorted than expected - /// @return COMM_RX_TIMEOUT - /// @return when there is no rxpacket received until PortHandler::isPacketTimeout() shows the timeout - /// @return COMM_SUCCESS - /// @return when rxpacket passes checksum test - /// @return or COMM_RX_FAIL - //////////////////////////////////////////////////////////////////////////////// - virtual int rxPacket (PortHandler *port, uint8_t *rxpacket) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits packet (txpacket) and receives packet (rxpacket) during designated time via PortHandler port - /// @description The function calls PacketHandler::txPacket(), - /// @description and calls PacketHandler::rxPacket() if it succeeds PacketHandler::txPacket(). - /// @description It breaks out - /// @description when it fails PacketHandler::txPacket(), - /// @description when txpacket is called by PacketHandler::broadcastPing() / PacketHandler::syncWriteTxOnly() / PacketHandler::regWriteTxOnly / PacketHandler::action - /// @param port PortHandler instance - /// @param txpacket packet for transmission - /// @param rxpacket received packet - /// @return COMM_SUCCESS - /// @return when it succeeds PacketHandler::txPacket() and PacketHandler::rxPacket() - /// @return or the other communication results which come from PacketHandler::txPacket() and PacketHandler::rxPacket() - //////////////////////////////////////////////////////////////////////////////// - virtual int txRxPacket (PortHandler *port, uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error = 0) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that pings Dynamixel but doesn't take its model number - /// @description The function calls PacketHandler::ping() which gets Dynamixel model number, - /// @description but doesn't carry the model number - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param error Dynamixel hardware error - /// @return communication results which come from PacketHandler::ping() - //////////////////////////////////////////////////////////////////////////////// - virtual int ping (PortHandler *port, uint8_t id, uint8_t *error = 0) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that pings Dynamixel and takes its model number - /// @description The function makes an instruction packet with INST_PING, - /// @description transmits the packet with PacketHandler::txRxPacket(), - /// @description and call PacketHandler::readTxRx to read model_number in the rx buffer. - /// @description It breaks out - /// @description when it tries to transmit to BROADCAST_ID - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param error Dynamixel hardware error - /// @return COMM_NOT_AVAILABLE - /// @return when it tries to transmit to BROADCAST_ID - /// @return COMM_SUCCESS - /// @return when it succeeds to ping Dynamixel and get model_number from it - /// @return or the other communication results which come from PacketHandler::txRxPacket() and PacketHandler::readTxRx() - //////////////////////////////////////////////////////////////////////////////// - virtual int ping (PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error = 0) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief (Available only in Protocol 2.0) The function that pings all connected Dynamixel - /// @param port PortHandler instance - /// @param id_list ID list of Dynamixels which are found by broadcast ping - /// @return COMM_NOT_AVAILABLE - //////////////////////////////////////////////////////////////////////////////// - virtual int broadcastPing (PortHandler *port, std::vector &id_list) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that makes Dynamixels run as written in the Dynamixel register - /// @description The function makes an instruction packet with INST_ACTION, - /// @description transmits the packet with PacketHandler::txRxPacket(). - /// @description To use this function, Dynamixel register should be set by PacketHandler::regWriteTxOnly() or PacketHandler::regWriteTxRx() - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @return communication results which come from PacketHandler::txRxPacket() - //////////////////////////////////////////////////////////////////////////////// - virtual int action (PortHandler *port, uint8_t id) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that makes Dynamixel reboot - /// @description The function makes an instruction packet with INST_REBOOT, - /// @description transmits the packet with PacketHandler::txRxPacket(), - /// @description then Dynamixel reboots. - /// @description During reboot, its LED will blink. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param error Dynamixel hardware error - /// @return COMM_NOT_AVAILABLE - //////////////////////////////////////////////////////////////////////////////// - virtual int reboot (PortHandler *port, uint8_t id, uint8_t *error = 0) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that reset multi-turn revolution information of Dynamixel - /// @description The function makes an instruction packet with INST_CLEAR, - /// @description transmits the packet with PacketHandler::txRxPacket(). - /// @description Applied Products : MX with Protocol 2.0 (Firmware v42 or above), - /// @description Dynamixel X-series (Firmware v42 or above). - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param error Dynamixel hardware error - /// @return communication results which come from PacketHandler::txRxPacket() - //////////////////////////////////////////////////////////////////////////////// - virtual int clearMultiTurn (PortHandler *port, uint8_t id, uint8_t *error = 0) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that makes Dynamixel reset as it was produced in the factory - /// @description The function makes an instruction packet with INST_FACTORY_RESET, - /// @description transmits the packet with PacketHandler::txRxPacket(). - /// @description Be careful of the use. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param option Reset option - /// @param error Dynamixel hardware error - /// @return communication results which come from PacketHandler::txRxPacket() - //////////////////////////////////////////////////////////////////////////////// - virtual int factoryReset (PortHandler *port, uint8_t id, uint8_t option = 0, uint8_t *error = 0) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits INST_READ instruction packet - /// @description The function makes an instruction packet with INST_READ, - /// @description transmits the packet with PacketHandler::txPacket(). - /// @description It breaks out - /// @description when it tries to transmit to BROADCAST_ID - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for read - /// @param length Length of the data for read - /// @return COMM_NOT_AVAILABLE - /// @return when it tries to transmit to BROADCAST_ID - /// @return or the other communication results which come from PacketHandler::txPacket() - //////////////////////////////////////////////////////////////////////////////// - virtual int readTx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that receives the packet and reads the data in the packet - /// @description The function receives the packet which might be come by previous INST_READ instruction packet transmission, - /// @description gets the data from the packet. - /// @param port PortHandler instance - /// @param length Length of the data for read - /// @param data Data extracted from the packet - /// @param error Dynamixel hardware error - /// @return communication results which come from PacketHandler::rxPacket() - //////////////////////////////////////////////////////////////////////////////// - virtual int readRx (PortHandler *port, uint8_t id, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits INST_READ instruction packet, and read data from received packet - /// @description The function makes an instruction packet with INST_READ, - /// @description transmits and receives the packet with PacketHandler::txRxPacket(), - /// @description gets the data from the packet. - /// @description It breaks out - /// @description when it tries to transmit to BROADCAST_ID - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for read - /// @param length Length of the data for read - /// @param data Data extracted from the packet - /// @param error Dynamixel hardware error - /// @return COMM_NOT_AVAILABLE - /// @return when it tries to transmit to BROADCAST_ID - /// @return or the other communication results which come from PacketHandler::txRxPacket() - //////////////////////////////////////////////////////////////////////////////// - virtual int readTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls PacketHandler::readTx() function for reading 1 byte data - /// @description The function calls PacketHandler::readTx() function for reading 1 byte data - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for read - /// @return communication results which come from PacketHandler::readTx() - //////////////////////////////////////////////////////////////////////////////// - virtual int read1ByteTx (PortHandler *port, uint8_t id, uint16_t address) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls PacketHandler::readRx() function and reads 1 byte data on the packet - /// @description The function calls PacketHandler::readRx() function, - /// @description gets 1 byte data from the packet. - /// @param port PortHandler instance - /// @param data Data extracted from the packet - /// @param error Dynamixel hardware error - /// @return communication results which come from PacketHandler::readRx() - //////////////////////////////////////////////////////////////////////////////// - virtual int read1ByteRx (PortHandler *port, uint8_t id, uint8_t *data, uint8_t *error = 0) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls PacketHandler::readTxRx() function for reading 1 byte data - /// @description The function calls PacketHandler::readTxRx(), - /// @description gets 1 byte data from the packet. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for read - /// @param length Length of the data for read - /// @param data Data extracted from the packet - /// @param error Dynamixel hardware error - /// @return communication results which come from PacketHandler::txRxPacket() - //////////////////////////////////////////////////////////////////////////////// - virtual int read1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error = 0) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls PacketHandler::readTx() function for reading 2 byte data - /// @description The function calls PacketHandler::readTx() function for reading 2 byte data - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for read - /// @return communication results which come from PacketHandler::readTx() - //////////////////////////////////////////////////////////////////////////////// - virtual int read2ByteTx (PortHandler *port, uint8_t id, uint16_t address) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls PacketHandler::readRx() function and reads 2 byte data on the packet - /// @description The function calls PacketHandler::readRx() function, - /// @description gets 2 byte data from the packet. - /// @param port PortHandler instance - /// @param data Data extracted from the packet - /// @param error Dynamixel hardware error - /// @return communication results which come from PacketHandler::readRx() - //////////////////////////////////////////////////////////////////////////////// - virtual int read2ByteRx (PortHandler *port, uint8_t id, uint16_t *data, uint8_t *error = 0) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls PacketHandler::readTxRx() function for reading 2 byte data - /// @description The function calls PacketHandler::readTxRx(), - /// @description gets 2 byte data from the packet. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for read - /// @param length Length of the data for read - /// @param data Data extracted from the packet - /// @param error Dynamixel hardware error - /// @return communication results which come from PacketHandler::txRxPacket() - //////////////////////////////////////////////////////////////////////////////// - virtual int read2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error = 0) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls PacketHandler::readTx() function for reading 4 byte data - /// @description The function calls PacketHandler::readTx() function for reading 4 byte data - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for read - /// @return communication results which come from PacketHandler::readTx() - //////////////////////////////////////////////////////////////////////////////// - virtual int read4ByteTx (PortHandler *port, uint8_t id, uint16_t address) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls PacketHandler::readRx() function and reads 4 byte data on the packet - /// @description The function calls PacketHandler::readRx() function, - /// @description gets 4 byte data from the packet. - /// @param port PortHandler instance - /// @param data Data extracted from the packet - /// @param error Dynamixel hardware error - /// @return communication results which come from PacketHandler::readRx() - //////////////////////////////////////////////////////////////////////////////// - virtual int read4ByteRx (PortHandler *port, uint8_t id, uint32_t *data, uint8_t *error = 0) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls PacketHandler::readTxRx() function for reading 4 byte data - /// @description The function calls PacketHandler::readTxRx(), - /// @description gets 4 byte data from the packet. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for read - /// @param length Length of the data for read - /// @param data Data extracted from the packet - /// @param error Dynamixel hardware error - /// @return communication results which come from PacketHandler::txRxPacket() - //////////////////////////////////////////////////////////////////////////////// - virtual int read4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error = 0) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits INST_WRITE instruction packet with the data for write - /// @description The function makes an instruction packet with INST_WRITE and the data for write, - /// @description transmits the packet with PacketHandler::txPacket(). - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for write - /// @param length Length of the data for write - /// @param data Data for write - /// @return communication results which come from PacketHandler::txPacket() - //////////////////////////////////////////////////////////////////////////////// - virtual int writeTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits INST_WRITE instruction packet with the data for write, and receives the packet - /// @description The function makes an instruction packet with INST_WRITE and the data for write, - /// @description transmits and receives the packet with PacketHandler::txRxPacket(), - /// @description gets the error from the packet. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for write - /// @param length Length of the data for write - /// @param data Data for write - /// @param error Dynamixel hardware error - /// @return communication results which come from PacketHandler::txRxPacket() - //////////////////////////////////////////////////////////////////////////////// - virtual int writeTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls PacketHandler::writeTxOnly() for writing 1 byte data - /// @description The function calls PacketHandler::writeTxOnly() for writing 1 byte data. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for write - /// @param data Data for write - /// @return communication results which come from PacketHandler::writeTxOnly() - //////////////////////////////////////////////////////////////////////////////// - virtual int write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls PacketHandler::writeTxRx() for writing 1 byte data and receives the packet - /// @description The function calls PacketHandler::writeTxRx() for writing 1 byte data and receves the packet, - /// @description gets the error from the packet. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for write - /// @param data Data for write - /// @param error Dynamixel hardware error - /// @return communication results which come from PacketHandler::writeTxRx() - //////////////////////////////////////////////////////////////////////////////// - virtual int write1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error = 0) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls PacketHandler::writeTxOnly() for writing 2 byte data - /// @description The function calls PacketHandler::writeTxOnly() for writing 2 byte data. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for write - /// @param data Data for write - /// @return communication results which come from PacketHandler::writeTxOnly() - //////////////////////////////////////////////////////////////////////////////// - virtual int write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls PacketHandler::writeTxRx() for writing 2 byte data and receives the packet - /// @description The function calls PacketHandler::writeTxRx() for writing 2 byte data and receves the packet, - /// @description gets the error from the packet. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for write - /// @param data Data for write - /// @param error Dynamixel hardware error - /// @return communication results which come from PacketHandler::writeTxRx() - //////////////////////////////////////////////////////////////////////////////// - virtual int write2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error = 0) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls PacketHandler::writeTxOnly() for writing 4 byte data - /// @description The function calls PacketHandler::writeTxOnly() for writing 4 byte data. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for write - /// @param data Data for write - /// @return communication results which come from PacketHandler::writeTxOnly() - //////////////////////////////////////////////////////////////////////////////// - virtual int write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls PacketHandler::writeTxRx() for writing 4 byte data and receives the packet - /// @description The function calls PacketHandler::writeTxRx() for writing 4 byte data and receves the packet, - /// @description gets the error from the packet. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for write - /// @param data Data for write - /// @param error Dynamixel hardware error - /// @return communication results which come from PacketHandler::writeTxRx() - //////////////////////////////////////////////////////////////////////////////// - virtual int write4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error = 0) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits INST_REG_WRITE instruction packet with the data for writing on the Dynamixel register - /// @description The function makes an instruction packet with INST_REG_WRITE and the data for writing on the Dynamixel register, - /// @description transmits the packet with PacketHandler::txPacket(). - /// @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamixel. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for write - /// @param length Length of the data for write - /// @param data Data for write - /// @return communication results which come from PacketHandler::txPacket() - //////////////////////////////////////////////////////////////////////////////// - virtual int regWriteTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits INST_REG_WRITE instruction packet with the data for writing on the Dynamixel register, and receives the packet - /// @description The function makes an instruction packet with INST_REG_WRITE and the data for writing on the Dynamixel register, - /// @description transmits and receives the packet with PacketHandler::txRxPacket(), - /// @description gets the error from the packet. - /// @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamixel. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for write - /// @param length Length of the data for write - /// @param data Data for write - /// @param error Dynamixel hardware error - /// @return communication results which come from PacketHandler::txRxPacket() - //////////////////////////////////////////////////////////////////////////////// - virtual int regWriteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits INST_SYNC_READ instruction packet - /// @description The function makes an instruction packet with INST_SYNC_READ, - /// @description transmits the packet with PacketHandler::txPacket(). - /// @param port PortHandler instance - /// @param start_address Address of the data for Sync Read - /// @param data_length Length of the data for Sync Read - /// @param param Parameter for Sync Read - /// @param param_length Length of the data for Sync Read - /// @return communication results which come from PacketHandler::txPacket() - //////////////////////////////////////////////////////////////////////////////// - virtual int syncReadTx (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) = 0; - // SyncReadRx -> GroupSyncRead class - // SyncReadTxRx -> GroupSyncRead class - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits INST_SYNC_WRITE instruction packet - /// @description The function makes an instruction packet with INST_SYNC_WRITE, - /// @description transmits the packet with PacketHandler::txRxPacket(). - /// @param port PortHandler instance - /// @param start_address Address of the data for Sync Write - /// @param data_length Length of the data for Sync Write - /// @param param Parameter for Sync Write - /// @param param_length Length of the data for Sync Write - /// @return communication results which come from PacketHandler::txRxPacket() - //////////////////////////////////////////////////////////////////////////////// - virtual int syncWriteTxOnly (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits INST_BULK_READ instruction packet - /// @description The function makes an instruction packet with INST_BULK_READ, - /// @description transmits the packet with PacketHandler::txPacket(). - /// @param port PortHandler instance - /// @param param Parameter for Bulk Read - /// @param param_length Length of the data for Bulk Read - /// @return communication results which come from PacketHandler::txPacket() - //////////////////////////////////////////////////////////////////////////////// - virtual int bulkReadTx (PortHandler *port, uint8_t *param, uint16_t param_length) = 0; - // BulkReadRx -> GroupBulkRead class - // BulkReadTxRx -> GroupBulkRead class - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits INST_BULK_WRITE instruction packet - /// @description The function makes an instruction packet with INST_BULK_WRITE, - /// @description transmits the packet with PacketHandler::txRxPacket(). - /// @param port PortHandler instance - /// @param param Parameter for Bulk Write - /// @param param_length Length of the data for Bulk Write - /// @return communication results which come from PacketHandler::txRxPacket() - //////////////////////////////////////////////////////////////////////////////// - virtual int bulkWriteTxOnly (PortHandler *port, uint8_t *param, uint16_t param_length) = 0; +class WINDECLSPEC PacketHandler { +protected: + PacketHandler() {} + +public: + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns PacketHandler instance + /// @return PacketHandler instance + //////////////////////////////////////////////////////////////////////////////// + static PacketHandler* getPacketHandler(float protocol_version = 2.0); + + virtual ~PacketHandler() {} + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns Protocol version + /// @return protocol version + //////////////////////////////////////////////////////////////////////////////// + virtual float getProtocolVersion() = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that gets description of communication result + /// @param result Communication result which might be gotten by the tx rx functions + /// @return description of communication result in const char* (string) + //////////////////////////////////////////////////////////////////////////////// + virtual const char* getTxRxResult(int result) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that gets description of hardware error + /// @param error Dynamixel hardware error which might be gotten by the tx rx functions + /// @return description of hardware error in const char* (string) + //////////////////////////////////////////////////////////////////////////////// + virtual const char* getRxPacketError(uint8_t error) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits the instruction packet txpacket via PortHandler port. + /// @description The function clears the port buffer by PortHandler::clearPort() function, + /// @description then transmits txpacket by PortHandler::writePort() function. + /// @description The function activates only when the port is not busy and when the packet is already written on the port buffer + /// @param port PortHandler instance + /// @param txpacket packet for transmission + /// @return COMM_PORT_BUSY + /// @return when the port is already in use + /// @return COMM_TX_ERROR + /// @return when txpacket is out of range described by TXPACKET_MAX_LEN + /// @return COMM_TX_FAIL + /// @return when written packet is shorter than expected + /// @return or COMM_SUCCESS + //////////////////////////////////////////////////////////////////////////////// + virtual int txPacket(PortHandler* port, uint8_t* txpacket) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that receives packet (rxpacket) during designated time via PortHandler port + /// @description The function repeatedly tries to receive rxpacket by PortHandler::readPort() function. + /// @description It breaks out + /// @description when PortHandler::isPacketTimeout() shows the timeout, + /// @description when rxpacket seemed as corrupted, or + /// @description when nothing received + /// @param port PortHandler instance + /// @param rxpacket received packet + /// @return COMM_RX_CORRUPT + /// @return when it received the packet but it couldn't find header in the packet + /// @return when it found header in the packet but the id, length or error value is out of range + /// @return when it received the packet but it is shorted than expected + /// @return COMM_RX_TIMEOUT + /// @return when there is no rxpacket received until PortHandler::isPacketTimeout() shows the timeout + /// @return COMM_SUCCESS + /// @return when rxpacket passes checksum test + /// @return or COMM_RX_FAIL + //////////////////////////////////////////////////////////////////////////////// + virtual int rxPacket(PortHandler* port, uint8_t* rxpacket) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits packet (txpacket) and receives packet (rxpacket) during designated time via PortHandler port + /// @description The function calls PacketHandler::txPacket(), + /// @description and calls PacketHandler::rxPacket() if it succeeds PacketHandler::txPacket(). + /// @description It breaks out + /// @description when it fails PacketHandler::txPacket(), + /// @description when txpacket is called by PacketHandler::broadcastPing() / PacketHandler::syncWriteTxOnly() / PacketHandler::regWriteTxOnly / PacketHandler::action + /// @param port PortHandler instance + /// @param txpacket packet for transmission + /// @param rxpacket received packet + /// @return COMM_SUCCESS + /// @return when it succeeds PacketHandler::txPacket() and PacketHandler::rxPacket() + /// @return or the other communication results which come from PacketHandler::txPacket() and PacketHandler::rxPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int txRxPacket(PortHandler* port, uint8_t* txpacket, uint8_t* rxpacket, uint8_t* error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that pings Dynamixel but doesn't take its model number + /// @description The function calls PacketHandler::ping() which gets Dynamixel model number, + /// @description but doesn't carry the model number + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param error Dynamixel hardware error + /// @return communication results which come from PacketHandler::ping() + //////////////////////////////////////////////////////////////////////////////// + virtual int ping(PortHandler* port, uint8_t id, uint8_t* error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that pings Dynamixel and takes its model number + /// @description The function makes an instruction packet with INST_PING, + /// @description transmits the packet with PacketHandler::txRxPacket(), + /// @description and call PacketHandler::readTxRx to read model_number in the rx buffer. + /// @description It breaks out + /// @description when it tries to transmit to BROADCAST_ID + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param error Dynamixel hardware error + /// @return COMM_NOT_AVAILABLE + /// @return when it tries to transmit to BROADCAST_ID + /// @return COMM_SUCCESS + /// @return when it succeeds to ping Dynamixel and get model_number from it + /// @return or the other communication results which come from PacketHandler::txRxPacket() and PacketHandler::readTxRx() + //////////////////////////////////////////////////////////////////////////////// + virtual int ping(PortHandler* port, uint8_t id, uint16_t* model_number, uint8_t* error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief (Available only in Protocol 2.0) The function that pings all connected Dynamixel + /// @param port PortHandler instance + /// @param id_list ID list of Dynamixels which are found by broadcast ping + /// @return COMM_NOT_AVAILABLE + //////////////////////////////////////////////////////////////////////////////// + virtual int broadcastPing(PortHandler* port, std::vector& id_list) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that makes Dynamixels run as written in the Dynamixel register + /// @description The function makes an instruction packet with INST_ACTION, + /// @description transmits the packet with PacketHandler::txRxPacket(). + /// @description To use this function, Dynamixel register should be set by PacketHandler::regWriteTxOnly() or PacketHandler::regWriteTxRx() + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @return communication results which come from PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int action(PortHandler* port, uint8_t id) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that makes Dynamixel reboot + /// @description The function makes an instruction packet with INST_REBOOT, + /// @description transmits the packet with PacketHandler::txRxPacket(), + /// @description then Dynamixel reboots. + /// @description During reboot, its LED will blink. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param error Dynamixel hardware error + /// @return COMM_NOT_AVAILABLE + //////////////////////////////////////////////////////////////////////////////// + virtual int reboot(PortHandler* port, uint8_t id, uint8_t* error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that reset multi-turn revolution information of Dynamixel + /// @description The function makes an instruction packet with INST_CLEAR, + /// @description transmits the packet with PacketHandler::txRxPacket(). + /// @description Applied Products : MX with Protocol 2.0 (Firmware v42 or above), + /// @description Dynamixel X-series (Firmware v42 or above). + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param error Dynamixel hardware error + /// @return communication results which come from PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int clearMultiTurn(PortHandler* port, uint8_t id, uint8_t* error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that makes Dynamixel reset as it was produced in the factory + /// @description The function makes an instruction packet with INST_FACTORY_RESET, + /// @description transmits the packet with PacketHandler::txRxPacket(). + /// @description Be careful of the use. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param option Reset option + /// @param error Dynamixel hardware error + /// @return communication results which come from PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int factoryReset(PortHandler* port, uint8_t id, uint8_t option = 0, uint8_t* error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_READ instruction packet + /// @description The function makes an instruction packet with INST_READ, + /// @description transmits the packet with PacketHandler::txPacket(). + /// @description It breaks out + /// @description when it tries to transmit to BROADCAST_ID + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @param length Length of the data for read + /// @return COMM_NOT_AVAILABLE + /// @return when it tries to transmit to BROADCAST_ID + /// @return or the other communication results which come from PacketHandler::txPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int readTx(PortHandler* port, uint8_t id, uint16_t address, uint16_t length) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that receives the packet and reads the data in the packet + /// @description The function receives the packet which might be come by previous INST_READ instruction packet transmission, + /// @description gets the data from the packet. + /// @param port PortHandler instance + /// @param length Length of the data for read + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from PacketHandler::rxPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int readRx(PortHandler* port, uint8_t id, uint16_t length, uint8_t* data, uint8_t* error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_READ instruction packet, and read data from received packet + /// @description The function makes an instruction packet with INST_READ, + /// @description transmits and receives the packet with PacketHandler::txRxPacket(), + /// @description gets the data from the packet. + /// @description It breaks out + /// @description when it tries to transmit to BROADCAST_ID + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @param length Length of the data for read + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return COMM_NOT_AVAILABLE + /// @return when it tries to transmit to BROADCAST_ID + /// @return or the other communication results which come from PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int readTxRx(PortHandler* port, uint8_t id, uint16_t address, uint16_t length, uint8_t* data, uint8_t* error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls PacketHandler::readTx() function for reading 1 byte data + /// @description The function calls PacketHandler::readTx() function for reading 1 byte data + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @return communication results which come from PacketHandler::readTx() + //////////////////////////////////////////////////////////////////////////////// + virtual int read1ByteTx(PortHandler* port, uint8_t id, uint16_t address) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls PacketHandler::readRx() function and reads 1 byte data on the packet + /// @description The function calls PacketHandler::readRx() function, + /// @description gets 1 byte data from the packet. + /// @param port PortHandler instance + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from PacketHandler::readRx() + //////////////////////////////////////////////////////////////////////////////// + virtual int read1ByteRx(PortHandler* port, uint8_t id, uint8_t* data, uint8_t* error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls PacketHandler::readTxRx() function for reading 1 byte data + /// @description The function calls PacketHandler::readTxRx(), + /// @description gets 1 byte data from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @param length Length of the data for read + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int read1ByteTxRx(PortHandler* port, uint8_t id, uint16_t address, uint8_t* data, uint8_t* error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls PacketHandler::readTx() function for reading 2 byte data + /// @description The function calls PacketHandler::readTx() function for reading 2 byte data + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @return communication results which come from PacketHandler::readTx() + //////////////////////////////////////////////////////////////////////////////// + virtual int read2ByteTx(PortHandler* port, uint8_t id, uint16_t address) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls PacketHandler::readRx() function and reads 2 byte data on the packet + /// @description The function calls PacketHandler::readRx() function, + /// @description gets 2 byte data from the packet. + /// @param port PortHandler instance + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from PacketHandler::readRx() + //////////////////////////////////////////////////////////////////////////////// + virtual int read2ByteRx(PortHandler* port, uint8_t id, uint16_t* data, uint8_t* error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls PacketHandler::readTxRx() function for reading 2 byte data + /// @description The function calls PacketHandler::readTxRx(), + /// @description gets 2 byte data from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @param length Length of the data for read + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int read2ByteTxRx(PortHandler* port, uint8_t id, uint16_t address, uint16_t* data, uint8_t* error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls PacketHandler::readTx() function for reading 4 byte data + /// @description The function calls PacketHandler::readTx() function for reading 4 byte data + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @return communication results which come from PacketHandler::readTx() + //////////////////////////////////////////////////////////////////////////////// + virtual int read4ByteTx(PortHandler* port, uint8_t id, uint16_t address) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls PacketHandler::readRx() function and reads 4 byte data on the packet + /// @description The function calls PacketHandler::readRx() function, + /// @description gets 4 byte data from the packet. + /// @param port PortHandler instance + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from PacketHandler::readRx() + //////////////////////////////////////////////////////////////////////////////// + virtual int read4ByteRx(PortHandler* port, uint8_t id, uint32_t* data, uint8_t* error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls PacketHandler::readTxRx() function for reading 4 byte data + /// @description The function calls PacketHandler::readTxRx(), + /// @description gets 4 byte data from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @param length Length of the data for read + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int read4ByteTxRx(PortHandler* port, uint8_t id, uint16_t address, uint32_t* data, uint8_t* error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_WRITE instruction packet with the data for write + /// @description The function makes an instruction packet with INST_WRITE and the data for write, + /// @description transmits the packet with PacketHandler::txPacket(). + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param length Length of the data for write + /// @param data Data for write + /// @return communication results which come from PacketHandler::txPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int writeTxOnly(PortHandler* port, uint8_t id, uint16_t address, uint16_t length, uint8_t* data) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_WRITE instruction packet with the data for write, and receives the packet + /// @description The function makes an instruction packet with INST_WRITE and the data for write, + /// @description transmits and receives the packet with PacketHandler::txRxPacket(), + /// @description gets the error from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param length Length of the data for write + /// @param data Data for write + /// @param error Dynamixel hardware error + /// @return communication results which come from PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int writeTxRx(PortHandler* port, uint8_t id, uint16_t address, uint16_t length, uint8_t* data, uint8_t* error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls PacketHandler::writeTxOnly() for writing 1 byte data + /// @description The function calls PacketHandler::writeTxOnly() for writing 1 byte data. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param data Data for write + /// @return communication results which come from PacketHandler::writeTxOnly() + //////////////////////////////////////////////////////////////////////////////// + virtual int write1ByteTxOnly(PortHandler* port, uint8_t id, uint16_t address, uint8_t data) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls PacketHandler::writeTxRx() for writing 1 byte data and receives the packet + /// @description The function calls PacketHandler::writeTxRx() for writing 1 byte data and receves the packet, + /// @description gets the error from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param data Data for write + /// @param error Dynamixel hardware error + /// @return communication results which come from PacketHandler::writeTxRx() + //////////////////////////////////////////////////////////////////////////////// + virtual int write1ByteTxRx(PortHandler* port, uint8_t id, uint16_t address, uint8_t data, uint8_t* error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls PacketHandler::writeTxOnly() for writing 2 byte data + /// @description The function calls PacketHandler::writeTxOnly() for writing 2 byte data. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param data Data for write + /// @return communication results which come from PacketHandler::writeTxOnly() + //////////////////////////////////////////////////////////////////////////////// + virtual int write2ByteTxOnly(PortHandler* port, uint8_t id, uint16_t address, uint16_t data) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls PacketHandler::writeTxRx() for writing 2 byte data and receives the packet + /// @description The function calls PacketHandler::writeTxRx() for writing 2 byte data and receves the packet, + /// @description gets the error from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param data Data for write + /// @param error Dynamixel hardware error + /// @return communication results which come from PacketHandler::writeTxRx() + //////////////////////////////////////////////////////////////////////////////// + virtual int write2ByteTxRx(PortHandler* port, uint8_t id, uint16_t address, uint16_t data, uint8_t* error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls PacketHandler::writeTxOnly() for writing 4 byte data + /// @description The function calls PacketHandler::writeTxOnly() for writing 4 byte data. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param data Data for write + /// @return communication results which come from PacketHandler::writeTxOnly() + //////////////////////////////////////////////////////////////////////////////// + virtual int write4ByteTxOnly(PortHandler* port, uint8_t id, uint16_t address, uint32_t data) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls PacketHandler::writeTxRx() for writing 4 byte data and receives the packet + /// @description The function calls PacketHandler::writeTxRx() for writing 4 byte data and receves the packet, + /// @description gets the error from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param data Data for write + /// @param error Dynamixel hardware error + /// @return communication results which come from PacketHandler::writeTxRx() + //////////////////////////////////////////////////////////////////////////////// + virtual int write4ByteTxRx(PortHandler* port, uint8_t id, uint16_t address, uint32_t data, uint8_t* error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_REG_WRITE instruction packet with the data for writing on the Dynamixel register + /// @description The function makes an instruction packet with INST_REG_WRITE and the data for writing on the Dynamixel register, + /// @description transmits the packet with PacketHandler::txPacket(). + /// @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamixel. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param length Length of the data for write + /// @param data Data for write + /// @return communication results which come from PacketHandler::txPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int regWriteTxOnly(PortHandler* port, uint8_t id, uint16_t address, uint16_t length, uint8_t* data) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_REG_WRITE instruction packet with the data for writing on the Dynamixel register, and receives the packet + /// @description The function makes an instruction packet with INST_REG_WRITE and the data for writing on the Dynamixel register, + /// @description transmits and receives the packet with PacketHandler::txRxPacket(), + /// @description gets the error from the packet. + /// @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamixel. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param length Length of the data for write + /// @param data Data for write + /// @param error Dynamixel hardware error + /// @return communication results which come from PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int regWriteTxRx(PortHandler* port, uint8_t id, uint16_t address, uint16_t length, uint8_t* data, uint8_t* error = 0) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_SYNC_READ instruction packet + /// @description The function makes an instruction packet with INST_SYNC_READ, + /// @description transmits the packet with PacketHandler::txPacket(). + /// @param port PortHandler instance + /// @param start_address Address of the data for Sync Read + /// @param data_length Length of the data for Sync Read + /// @param param Parameter for Sync Read + /// @param param_length Length of the data for Sync Read + /// @return communication results which come from PacketHandler::txPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int syncReadTx(PortHandler* port, uint16_t start_address, uint16_t data_length, uint8_t* param, uint16_t param_length) = 0; + // SyncReadRx -> GroupSyncRead class + // SyncReadTxRx -> GroupSyncRead class + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_SYNC_WRITE instruction packet + /// @description The function makes an instruction packet with INST_SYNC_WRITE, + /// @description transmits the packet with PacketHandler::txRxPacket(). + /// @param port PortHandler instance + /// @param start_address Address of the data for Sync Write + /// @param data_length Length of the data for Sync Write + /// @param param Parameter for Sync Write + /// @param param_length Length of the data for Sync Write + /// @return communication results which come from PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int syncWriteTxOnly(PortHandler* port, uint16_t start_address, uint16_t data_length, uint8_t* param, uint16_t param_length) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_BULK_READ instruction packet + /// @description The function makes an instruction packet with INST_BULK_READ, + /// @description transmits the packet with PacketHandler::txPacket(). + /// @param port PortHandler instance + /// @param param Parameter for Bulk Read + /// @param param_length Length of the data for Bulk Read + /// @return communication results which come from PacketHandler::txPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int bulkReadTx(PortHandler* port, uint8_t* param, uint16_t param_length) = 0; + // BulkReadRx -> GroupBulkRead class + // BulkReadTxRx -> GroupBulkRead class + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_BULK_WRITE instruction packet + /// @description The function makes an instruction packet with INST_BULK_WRITE, + /// @description transmits the packet with PacketHandler::txRxPacket(). + /// @param port PortHandler instance + /// @param param Parameter for Bulk Write + /// @param param_length Length of the data for Bulk Write + /// @return communication results which come from PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + virtual int bulkWriteTxOnly(PortHandler* port, uint8_t* param, uint16_t param_length) = 0; }; -} - +} // namespace dynamixel #endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PACKETHANDLER_H_ */ diff --git a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/port_handler.h b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/port_handler.h index 63976c0..4684dc9 100644 --- a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/port_handler.h +++ b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/port_handler.h @@ -1,18 +1,18 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ //////////////////////////////////////////////////////////////////////////////// /// @file The file for port control @@ -27,11 +27,11 @@ #elif defined(__APPLE__) #define WINDECLSPEC #elif defined(_WIN32) || defined(_WIN64) - #ifdef WINDLLEXPORT - #define WINDECLSPEC __declspec(dllexport) - #else - #define WINDECLSPEC __declspec(dllimport) - #endif +#ifdef WINDLLEXPORT +#define WINDECLSPEC __declspec(dllexport) +#else +#define WINDECLSPEC __declspec(dllimport) +#endif #elif defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__) #define WINDECLSPEC #endif @@ -47,131 +47,128 @@ #include -namespace dynamixel -{ +namespace dynamixel { //////////////////////////////////////////////////////////////////////////////// /// @brief The class for port control that inherits PortHandlerLinux, PortHandlerWindows, PortHandlerMac, or PortHandlerArduino //////////////////////////////////////////////////////////////////////////////// -class WINDECLSPEC PortHandler -{ - public: - static const int DEFAULT_BAUDRATE_ = 57600; ///< Default Baudrate - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that gets PortHandler class inheritance - /// @description The function gets class inheritance (PortHandlerLinux / PortHandlerWindows / PortHandlerMac / PortHandlerArduino. - //////////////////////////////////////////////////////////////////////////////// - static PortHandler *getPortHandler(const char *port_name); - - bool is_using_; ///< shows whether the port is in use - - virtual ~PortHandler() { } - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that opens the port - /// @description The function calls PortHandlerLinux::setBaudRate() to open the port. - /// @return communication results which come from PortHandlerLinux::setBaudRate() - //////////////////////////////////////////////////////////////////////////////// - virtual bool openPort() = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that closes the port - /// @description The function closes the port. - //////////////////////////////////////////////////////////////////////////////// - virtual void closePort() = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that clears the port - /// @description The function clears the port. - //////////////////////////////////////////////////////////////////////////////// - virtual void clearPort() = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that sets port name into the port handler - /// @description The function sets port name into the port handler. - /// @param port_name Port name - //////////////////////////////////////////////////////////////////////////////// - virtual void setPortName(const char* port_name) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that returns port name set into the port handler - /// @description The function returns current port name set into the port handler. - /// @return Port name - //////////////////////////////////////////////////////////////////////////////// - virtual char *getPortName() = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that sets baudrate into the port handler - /// @description The function sets baudrate into the port handler. - /// @param baudrate Baudrate - /// @return false - /// @return when error was occurred during port opening - /// @return or true - //////////////////////////////////////////////////////////////////////////////// - virtual bool setBaudRate(const int baudrate) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that returns current baudrate set into the port handler - /// @description The function returns current baudrate set into the port handler. - /// @return Baudrate - //////////////////////////////////////////////////////////////////////////////// - virtual int getBaudRate() = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that checks how much bytes are able to be read from the port buffer - /// @description The function checks how much bytes are able to be read from the port buffer - /// @description and returns the number. - /// @return Length of read-able bytes in the port buffer - //////////////////////////////////////////////////////////////////////////////// - virtual int getBytesAvailable() = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that reads bytes from the port buffer - /// @description The function gets bytes from the port buffer, - /// @description and returns a number of bytes read. - /// @param packet Buffer for the packet received - /// @param length Length of the buffer for read - /// @return -1 - /// @return when error was occurred - /// @return or Length of bytes read - //////////////////////////////////////////////////////////////////////////////// - virtual int readPort(uint8_t *packet, int length) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that writes bytes on the port buffer - /// @description The function writes bytes on the port buffer, - /// @description and returns a number of bytes which are successfully written. - /// @param packet Buffer which would be written on the port buffer - /// @param length Length of the buffer for write - /// @return -1 - /// @return when error was occurred - /// @return or Length of bytes written - //////////////////////////////////////////////////////////////////////////////// - virtual int writePort(uint8_t *packet, int length) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that sets and starts stopwatch for watching packet timeout - /// @description The function sets the stopwatch by getting current time and the time of packet timeout with packet_length. - /// @param packet_length Length of the packet expected to be received - //////////////////////////////////////////////////////////////////////////////// - virtual void setPacketTimeout(uint16_t packet_length) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that sets and starts stopwatch for watching packet timeout - /// @description The function sets the stopwatch by getting current time and the time of packet timeout with msec. - /// @param packet_length Length of the packet expected to be received - //////////////////////////////////////////////////////////////////////////////// - virtual void setPacketTimeout(double msec) = 0; - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that checks whether packet timeout is occurred - /// @description The function checks whether current time is passed by the time of packet timeout from the time set by PortHandlerLinux::setPacketTimeout(). - //////////////////////////////////////////////////////////////////////////////// - virtual bool isPacketTimeout() = 0; +class WINDECLSPEC PortHandler { +public: + static const int DEFAULT_BAUDRATE_ = 57600; ///< Default Baudrate + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that gets PortHandler class inheritance + /// @description The function gets class inheritance (PortHandlerLinux / PortHandlerWindows / PortHandlerMac / PortHandlerArduino. + //////////////////////////////////////////////////////////////////////////////// + static PortHandler* getPortHandler(const char* port_name); + + bool is_using_; ///< shows whether the port is in use + + virtual ~PortHandler() {} + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that opens the port + /// @description The function calls PortHandlerLinux::setBaudRate() to open the port. + /// @return communication results which come from PortHandlerLinux::setBaudRate() + //////////////////////////////////////////////////////////////////////////////// + virtual bool openPort() = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that closes the port + /// @description The function closes the port. + //////////////////////////////////////////////////////////////////////////////// + virtual void closePort() = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that clears the port + /// @description The function clears the port. + //////////////////////////////////////////////////////////////////////////////// + virtual void clearPort() = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets port name into the port handler + /// @description The function sets port name into the port handler. + /// @param port_name Port name + //////////////////////////////////////////////////////////////////////////////// + virtual void setPortName(const char* port_name) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns port name set into the port handler + /// @description The function returns current port name set into the port handler. + /// @return Port name + //////////////////////////////////////////////////////////////////////////////// + virtual char* getPortName() = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets baudrate into the port handler + /// @description The function sets baudrate into the port handler. + /// @param baudrate Baudrate + /// @return false + /// @return when error was occurred during port opening + /// @return or true + //////////////////////////////////////////////////////////////////////////////// + virtual bool setBaudRate(const int baudrate) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns current baudrate set into the port handler + /// @description The function returns current baudrate set into the port handler. + /// @return Baudrate + //////////////////////////////////////////////////////////////////////////////// + virtual int getBaudRate() = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that checks how much bytes are able to be read from the port buffer + /// @description The function checks how much bytes are able to be read from the port buffer + /// @description and returns the number. + /// @return Length of read-able bytes in the port buffer + //////////////////////////////////////////////////////////////////////////////// + virtual int getBytesAvailable() = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that reads bytes from the port buffer + /// @description The function gets bytes from the port buffer, + /// @description and returns a number of bytes read. + /// @param packet Buffer for the packet received + /// @param length Length of the buffer for read + /// @return -1 + /// @return when error was occurred + /// @return or Length of bytes read + //////////////////////////////////////////////////////////////////////////////// + virtual int readPort(uint8_t* packet, int length) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that writes bytes on the port buffer + /// @description The function writes bytes on the port buffer, + /// @description and returns a number of bytes which are successfully written. + /// @param packet Buffer which would be written on the port buffer + /// @param length Length of the buffer for write + /// @return -1 + /// @return when error was occurred + /// @return or Length of bytes written + //////////////////////////////////////////////////////////////////////////////// + virtual int writePort(uint8_t* packet, int length) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets and starts stopwatch for watching packet timeout + /// @description The function sets the stopwatch by getting current time and the time of packet timeout with packet_length. + /// @param packet_length Length of the packet expected to be received + //////////////////////////////////////////////////////////////////////////////// + virtual void setPacketTimeout(uint16_t packet_length) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets and starts stopwatch for watching packet timeout + /// @description The function sets the stopwatch by getting current time and the time of packet timeout with msec. + /// @param packet_length Length of the packet expected to be received + //////////////////////////////////////////////////////////////////////////////// + virtual void setPacketTimeout(double msec) = 0; + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that checks whether packet timeout is occurred + /// @description The function checks whether current time is passed by the time of packet timeout from the time set by PortHandlerLinux::setPacketTimeout(). + //////////////////////////////////////////////////////////////////////////////// + virtual bool isPacketTimeout() = 0; }; -} - +} // namespace dynamixel #endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PORTHANDLER_H_ */ diff --git a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/port_handler_arduino.h b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/port_handler_arduino.h index 78ddea9..ea18dbc 100644 --- a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/port_handler_arduino.h +++ b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/port_handler_arduino.h @@ -1,18 +1,18 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ //////////////////////////////////////////////////////////////////////////////// /// @file The file for port control in Arduino @@ -22,162 +22,162 @@ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ARDUINO_PORTHANDLERARDUINO_H_ #define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ARDUINO_PORTHANDLERARDUINO_H_ -#if defined(ARDUINO) || defined(__OPENCR__) || defined (__OPENCM904__) +#if defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__) #include #endif #include "port_handler.h" -namespace dynamixel -{ +namespace dynamixel { //////////////////////////////////////////////////////////////////////////////// /// @brief The class for control port in Arduino //////////////////////////////////////////////////////////////////////////////// -class PortHandlerArduino : public PortHandler -{ - private: - int socket_fd_; - int baudrate_; - char port_name_[100]; +class PortHandlerArduino : public PortHandler { +private: + int socket_fd_; + int baudrate_; + char port_name_[100]; - double packet_start_time_; - double packet_timeout_; - double tx_time_per_byte; + double packet_start_time_; + double packet_timeout_; + double tx_time_per_byte; #if defined(__OPENCM904__) - UARTClass *p_dxl_serial; + UARTClass* p_dxl_serial; #endif - bool setupPort(const int cflag_baud); - - double getCurrentTime(); - double getTimeSinceStart(); - - int checkBaudrateAvailable(int baudrate); - - void setPowerOn(); - void setPowerOff(); - void setTxEnable(); - void setTxDisable(); - - public: - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that initializes instance of PortHandler and gets port_name - /// @description The function initializes instance of PortHandler and gets port_name. - //////////////////////////////////////////////////////////////////////////////// - PortHandlerArduino(const char *port_name); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that closes the port - /// @description The function calls PortHandlerArduino::closePort() to close the port. - //////////////////////////////////////////////////////////////////////////////// - virtual ~PortHandlerArduino() { closePort(); } - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that opens the port - /// @description The function calls PortHandlerArduino::setBaudRate() to open the port. - /// @return communication results which come from PortHandlerArduino::setBaudRate() - //////////////////////////////////////////////////////////////////////////////// - bool openPort(); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that closes the port - /// @description The function closes the port. - //////////////////////////////////////////////////////////////////////////////// - void closePort(); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that clears the port - /// @description The function clears the port. - //////////////////////////////////////////////////////////////////////////////// - void clearPort(); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that sets port name into the port handler - /// @description The function sets port name into the port handler. - /// @param port_name Port name - //////////////////////////////////////////////////////////////////////////////// - void setPortName(const char *port_name); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that returns port name set into the port handler - /// @description The function returns current port name set into the port handler. - /// @return Port name - //////////////////////////////////////////////////////////////////////////////// - char *getPortName(); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that sets baudrate into the port handler - /// @description The function sets baudrate into the port handler. - /// @param baudrate Baudrate - /// @return false - /// @return when error was occurred during port opening - /// @return or true - //////////////////////////////////////////////////////////////////////////////// - bool setBaudRate(const int baudrate); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that returns current baudrate set into the port handler - /// @description The function returns current baudrate set into the port handler. - /// @return Baudrate - //////////////////////////////////////////////////////////////////////////////// - int getBaudRate(); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that checks how much bytes are able to be read from the port buffer - /// @description The function checks how much bytes are able to be read from the port buffer - /// @description and returns the number. - /// @return Length of read-able bytes in the port buffer - //////////////////////////////////////////////////////////////////////////////// - int getBytesAvailable(); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that reads bytes from the port buffer - /// @description The function gets bytes from the port buffer, - /// @description and returns a number of bytes read. - /// @param packet Buffer for the packet received - /// @param length Length of the buffer for read - /// @return -1 - /// @return when error was occurred - /// @return or Length of bytes read - //////////////////////////////////////////////////////////////////////////////// - int readPort(uint8_t *packet, int length); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that writes bytes on the port buffer - /// @description The function writes bytes on the port buffer, - /// @description and returns a number of bytes which are successfully written. - /// @param packet Buffer which would be written on the port buffer - /// @param length Length of the buffer for write - /// @return -1 - /// @return when error was occurred - /// @return or Length of bytes written - //////////////////////////////////////////////////////////////////////////////// - int writePort(uint8_t *packet, int length); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that sets and starts stopwatch for watching packet timeout - /// @description The function sets the stopwatch by getting current time and the time of packet timeout with packet_length. - /// @param packet_length Length of the packet expected to be received - //////////////////////////////////////////////////////////////////////////////// - void setPacketTimeout(uint16_t packet_length); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that sets and starts stopwatch for watching packet timeout - /// @description The function sets the stopwatch by getting current time and the time of packet timeout with msec. - /// @param packet_length Length of the packet expected to be received - //////////////////////////////////////////////////////////////////////////////// - void setPacketTimeout(double msec); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that checks whether packet timeout is occurred - /// @description The function checks whether current time is passed by the time of packet timeout from the time set by PortHandlerArduino::setPacketTimeout(). - //////////////////////////////////////////////////////////////////////////////// - bool isPacketTimeout(); + bool setupPort(const int cflag_baud); + + double getCurrentTime(); + double getTimeSinceStart(); + + int checkBaudrateAvailable(int baudrate); + + void setPowerOn(); + void setPowerOff(); + void setTxEnable(); + void setTxDisable(); + +public: + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that initializes instance of PortHandler and gets port_name + /// @description The function initializes instance of PortHandler and gets port_name. + //////////////////////////////////////////////////////////////////////////////// + PortHandlerArduino(const char* port_name); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that closes the port + /// @description The function calls PortHandlerArduino::closePort() to close the port. + //////////////////////////////////////////////////////////////////////////////// + virtual ~PortHandlerArduino() + { + closePort(); + } + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that opens the port + /// @description The function calls PortHandlerArduino::setBaudRate() to open the port. + /// @return communication results which come from PortHandlerArduino::setBaudRate() + //////////////////////////////////////////////////////////////////////////////// + bool openPort(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that closes the port + /// @description The function closes the port. + //////////////////////////////////////////////////////////////////////////////// + void closePort(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that clears the port + /// @description The function clears the port. + //////////////////////////////////////////////////////////////////////////////// + void clearPort(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets port name into the port handler + /// @description The function sets port name into the port handler. + /// @param port_name Port name + //////////////////////////////////////////////////////////////////////////////// + void setPortName(const char* port_name); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns port name set into the port handler + /// @description The function returns current port name set into the port handler. + /// @return Port name + //////////////////////////////////////////////////////////////////////////////// + char* getPortName(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets baudrate into the port handler + /// @description The function sets baudrate into the port handler. + /// @param baudrate Baudrate + /// @return false + /// @return when error was occurred during port opening + /// @return or true + //////////////////////////////////////////////////////////////////////////////// + bool setBaudRate(const int baudrate); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns current baudrate set into the port handler + /// @description The function returns current baudrate set into the port handler. + /// @return Baudrate + //////////////////////////////////////////////////////////////////////////////// + int getBaudRate(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that checks how much bytes are able to be read from the port buffer + /// @description The function checks how much bytes are able to be read from the port buffer + /// @description and returns the number. + /// @return Length of read-able bytes in the port buffer + //////////////////////////////////////////////////////////////////////////////// + int getBytesAvailable(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that reads bytes from the port buffer + /// @description The function gets bytes from the port buffer, + /// @description and returns a number of bytes read. + /// @param packet Buffer for the packet received + /// @param length Length of the buffer for read + /// @return -1 + /// @return when error was occurred + /// @return or Length of bytes read + //////////////////////////////////////////////////////////////////////////////// + int readPort(uint8_t* packet, int length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that writes bytes on the port buffer + /// @description The function writes bytes on the port buffer, + /// @description and returns a number of bytes which are successfully written. + /// @param packet Buffer which would be written on the port buffer + /// @param length Length of the buffer for write + /// @return -1 + /// @return when error was occurred + /// @return or Length of bytes written + //////////////////////////////////////////////////////////////////////////////// + int writePort(uint8_t* packet, int length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets and starts stopwatch for watching packet timeout + /// @description The function sets the stopwatch by getting current time and the time of packet timeout with packet_length. + /// @param packet_length Length of the packet expected to be received + //////////////////////////////////////////////////////////////////////////////// + void setPacketTimeout(uint16_t packet_length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets and starts stopwatch for watching packet timeout + /// @description The function sets the stopwatch by getting current time and the time of packet timeout with msec. + /// @param packet_length Length of the packet expected to be received + //////////////////////////////////////////////////////////////////////////////// + void setPacketTimeout(double msec); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that checks whether packet timeout is occurred + /// @description The function checks whether current time is passed by the time of packet timeout from the time set by PortHandlerArduino::setPacketTimeout(). + //////////////////////////////////////////////////////////////////////////////// + bool isPacketTimeout(); }; -} - +} // namespace dynamixel #endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_ARDUINO_PORTHANDLERARDUINO_H_ */ diff --git a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/port_handler_linux.h b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/port_handler_linux.h index a690e43..c07df88 100644 --- a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/port_handler_linux.h +++ b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/port_handler_linux.h @@ -1,18 +1,18 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ //////////////////////////////////////////////////////////////////////////////// /// @file The file for port control in Linux @@ -22,150 +22,146 @@ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ #define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ - #include "port_handler.h" -namespace dynamixel -{ +namespace dynamixel { //////////////////////////////////////////////////////////////////////////////// /// @brief The class for control port in Linux //////////////////////////////////////////////////////////////////////////////// -class PortHandlerLinux : public PortHandler -{ - private: - int socket_fd_; - int baudrate_; - char port_name_[100]; - - double packet_start_time_; - double packet_timeout_; - double tx_time_per_byte; - - bool setupPort(const int cflag_baud); - bool setCustomBaudrate(int speed); - int getCFlagBaud(const int baudrate); - - double getCurrentTime(); - double getTimeSinceStart(); - - public: - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that initializes instance of PortHandler and gets port_name - /// @description The function initializes instance of PortHandler and gets port_name. - //////////////////////////////////////////////////////////////////////////////// - PortHandlerLinux(const char *port_name); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that closes the port - /// @description The function calls PortHandlerLinux::closePort() to close the port. - //////////////////////////////////////////////////////////////////////////////// - virtual ~PortHandlerLinux() { closePort(); } - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that opens the port - /// @description The function calls PortHandlerLinux::setBaudRate() to open the port. - /// @return communication results which come from PortHandlerLinux::setBaudRate() - //////////////////////////////////////////////////////////////////////////////// - bool openPort(); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that closes the port - /// @description The function closes the port. - //////////////////////////////////////////////////////////////////////////////// - void closePort(); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that clears the port - /// @description The function clears the port. - //////////////////////////////////////////////////////////////////////////////// - void clearPort(); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that sets port name into the port handler - /// @description The function sets port name into the port handler. - /// @param port_name Port name - //////////////////////////////////////////////////////////////////////////////// - void setPortName(const char *port_name); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that returns port name set into the port handler - /// @description The function returns current port name set into the port handler. - /// @return Port name - //////////////////////////////////////////////////////////////////////////////// - char *getPortName(); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that sets baudrate into the port handler - /// @description The function sets baudrate into the port handler. - /// @param baudrate Baudrate - /// @return false - /// @return when error was occurred during port opening - /// @return or true - //////////////////////////////////////////////////////////////////////////////// - bool setBaudRate(const int baudrate); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that returns current baudrate set into the port handler - /// @description The function returns current baudrate set into the port handler. - /// @return Baudrate - //////////////////////////////////////////////////////////////////////////////// - int getBaudRate(); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that checks how much bytes are able to be read from the port buffer - /// @description The function checks how much bytes are able to be read from the port buffer - /// @description and returns the number. - /// @return Length of read-able bytes in the port buffer - //////////////////////////////////////////////////////////////////////////////// - int getBytesAvailable(); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that reads bytes from the port buffer - /// @description The function gets bytes from the port buffer, - /// @description and returns a number of bytes read. - /// @param packet Buffer for the packet received - /// @param length Length of the buffer for read - /// @return -1 - /// @return when error was occurred - /// @return or Length of bytes read - //////////////////////////////////////////////////////////////////////////////// - int readPort(uint8_t *packet, int length); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that writes bytes on the port buffer - /// @description The function writes bytes on the port buffer, - /// @description and returns a number of bytes which are successfully written. - /// @param packet Buffer which would be written on the port buffer - /// @param length Length of the buffer for write - /// @return -1 - /// @return when error was occurred - /// @return or Length of bytes written - //////////////////////////////////////////////////////////////////////////////// - int writePort(uint8_t *packet, int length); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that sets and starts stopwatch for watching packet timeout - /// @description The function sets the stopwatch by getting current time and the time of packet timeout with packet_length. - /// @param packet_length Length of the packet expected to be received - //////////////////////////////////////////////////////////////////////////////// - void setPacketTimeout(uint16_t packet_length); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that sets and starts stopwatch for watching packet timeout - /// @description The function sets the stopwatch by getting current time and the time of packet timeout with msec. - /// @param packet_length Length of the packet expected to be received - //////////////////////////////////////////////////////////////////////////////// - void setPacketTimeout(double msec); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that checks whether packet timeout is occurred - /// @description The function checks whether current time is passed by the time of packet timeout from the time set by PortHandlerLinux::setPacketTimeout(). - //////////////////////////////////////////////////////////////////////////////// - bool isPacketTimeout(); +class PortHandlerLinux : public PortHandler { +private: + int socket_fd_; + int baudrate_; + char port_name_[100]; + + double packet_start_time_; + double packet_timeout_; + double tx_time_per_byte; + + bool setupPort(const int cflag_baud); + bool setCustomBaudrate(int speed); + int getCFlagBaud(const int baudrate); + + double getCurrentTime(); + double getTimeSinceStart(); + +public: + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that initializes instance of PortHandler and gets port_name + /// @description The function initializes instance of PortHandler and gets port_name. + //////////////////////////////////////////////////////////////////////////////// + PortHandlerLinux(const char* port_name); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that closes the port + /// @description The function calls PortHandlerLinux::closePort() to close the port. + //////////////////////////////////////////////////////////////////////////////// + virtual ~PortHandlerLinux() { closePort(); } + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that opens the port + /// @description The function calls PortHandlerLinux::setBaudRate() to open the port. + /// @return communication results which come from PortHandlerLinux::setBaudRate() + //////////////////////////////////////////////////////////////////////////////// + bool openPort(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that closes the port + /// @description The function closes the port. + //////////////////////////////////////////////////////////////////////////////// + void closePort(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that clears the port + /// @description The function clears the port. + //////////////////////////////////////////////////////////////////////////////// + void clearPort(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets port name into the port handler + /// @description The function sets port name into the port handler. + /// @param port_name Port name + //////////////////////////////////////////////////////////////////////////////// + void setPortName(const char* port_name); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns port name set into the port handler + /// @description The function returns current port name set into the port handler. + /// @return Port name + //////////////////////////////////////////////////////////////////////////////// + char* getPortName(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets baudrate into the port handler + /// @description The function sets baudrate into the port handler. + /// @param baudrate Baudrate + /// @return false + /// @return when error was occurred during port opening + /// @return or true + //////////////////////////////////////////////////////////////////////////////// + bool setBaudRate(const int baudrate); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns current baudrate set into the port handler + /// @description The function returns current baudrate set into the port handler. + /// @return Baudrate + //////////////////////////////////////////////////////////////////////////////// + int getBaudRate(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that checks how much bytes are able to be read from the port buffer + /// @description The function checks how much bytes are able to be read from the port buffer + /// @description and returns the number. + /// @return Length of read-able bytes in the port buffer + //////////////////////////////////////////////////////////////////////////////// + int getBytesAvailable(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that reads bytes from the port buffer + /// @description The function gets bytes from the port buffer, + /// @description and returns a number of bytes read. + /// @param packet Buffer for the packet received + /// @param length Length of the buffer for read + /// @return -1 + /// @return when error was occurred + /// @return or Length of bytes read + //////////////////////////////////////////////////////////////////////////////// + int readPort(uint8_t* packet, int length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that writes bytes on the port buffer + /// @description The function writes bytes on the port buffer, + /// @description and returns a number of bytes which are successfully written. + /// @param packet Buffer which would be written on the port buffer + /// @param length Length of the buffer for write + /// @return -1 + /// @return when error was occurred + /// @return or Length of bytes written + //////////////////////////////////////////////////////////////////////////////// + int writePort(uint8_t* packet, int length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets and starts stopwatch for watching packet timeout + /// @description The function sets the stopwatch by getting current time and the time of packet timeout with packet_length. + /// @param packet_length Length of the packet expected to be received + //////////////////////////////////////////////////////////////////////////////// + void setPacketTimeout(uint16_t packet_length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets and starts stopwatch for watching packet timeout + /// @description The function sets the stopwatch by getting current time and the time of packet timeout with msec. + /// @param packet_length Length of the packet expected to be received + //////////////////////////////////////////////////////////////////////////////// + void setPacketTimeout(double msec); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that checks whether packet timeout is occurred + /// @description The function checks whether current time is passed by the time of packet timeout from the time set by PortHandlerLinux::setPacketTimeout(). + //////////////////////////////////////////////////////////////////////////////// + bool isPacketTimeout(); }; -} - +} // namespace dynamixel #endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_LINUX_PORTHANDLERLINUX_H_ */ diff --git a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/port_handler_mac.h b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/port_handler_mac.h index 9739084..23f0b19 100644 --- a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/port_handler_mac.h +++ b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/port_handler_mac.h @@ -1,18 +1,18 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ //////////////////////////////////////////////////////////////////////////////// /// @file The file for port control in Mac OS @@ -22,151 +22,147 @@ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_MAC_PORTHANDLERMAC_H_ #define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_MAC_PORTHANDLERMAC_H_ - #include "port_handler.h" -namespace dynamixel -{ +namespace dynamixel { //////////////////////////////////////////////////////////////////////////////// /// @brief The class for control port in Mac OS //////////////////////////////////////////////////////////////////////////////// -class PortHandlerMac : public PortHandler -{ - private: - int socket_fd_; - int baudrate_; - char port_name_[100]; - - double packet_start_time_; - double packet_timeout_; - double tx_time_per_byte; - - bool setupPort(const int cflag_baud); - bool setCustomBaudrate(int speed); - int getCFlagBaud(const int baudrate); - - double getCurrentTime(); - double getTimeSinceStart(); - - public: - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that initializes instance of PortHandler and gets port_name - /// @description The function initializes instance of PortHandler and gets port_name. - //////////////////////////////////////////////////////////////////////////////// - PortHandlerMac(const char *port_name); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that closes the port - /// @description The function calls PortHandlerMac::closePort() to close the port. - //////////////////////////////////////////////////////////////////////////////// - virtual ~PortHandlerMac() { closePort(); } - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that opens the port - /// @description The function calls PortHandlerMac::setBaudRate() to open the port. - /// @return communication results which come from PortHandlerMac::setBaudRate() - //////////////////////////////////////////////////////////////////////////////// - bool openPort(); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that closes the port - /// @description The function closes the port. - //////////////////////////////////////////////////////////////////////////////// - void closePort(); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that clears the port - /// @description The function clears the port. - //////////////////////////////////////////////////////////////////////////////// - void clearPort(); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that sets port name into the port handler - /// @description The function sets port name into the port handler. - /// @param port_name Port name - //////////////////////////////////////////////////////////////////////////////// - void setPortName(const char *port_name); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that returns port name set into the port handler - /// @description The function returns current port name set into the port handler. - /// @return Port name - //////////////////////////////////////////////////////////////////////////////// - char *getPortName(); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that sets baudrate into the port handler - /// @description The function sets baudrate into the port handler. - /// @param baudrate Baudrate - /// @return false - /// @return when error was occurred during port opening - /// @return or true - //////////////////////////////////////////////////////////////////////////////// - bool setBaudRate(const int baudrate); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that returns current baudrate set into the port handler - /// @description The function returns current baudrate set into the port handler. - /// @warning Mac OS doesn't support over 230400 bps - /// @return Baudrate - //////////////////////////////////////////////////////////////////////////////// - int getBaudRate(); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that checks how much bytes are able to be read from the port buffer - /// @description The function checks how much bytes are able to be read from the port buffer - /// @description and returns the number. - /// @return Length of read-able bytes in the port buffer - //////////////////////////////////////////////////////////////////////////////// - int getBytesAvailable(); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that reads bytes from the port buffer - /// @description The function gets bytes from the port buffer, - /// @description and returns a number of bytes read. - /// @param packet Buffer for the packet received - /// @param length Length of the buffer for read - /// @return -1 - /// @return when error was occurred - /// @return or Length of bytes read - //////////////////////////////////////////////////////////////////////////////// - int readPort(uint8_t *packet, int length); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that writes bytes on the port buffer - /// @description The function writes bytes on the port buffer, - /// @description and returns a number of bytes which are successfully written. - /// @param packet Buffer which would be written on the port buffer - /// @param length Length of the buffer for write - /// @return -1 - /// @return when error was occurred - /// @return or Length of bytes written - //////////////////////////////////////////////////////////////////////////////// - int writePort(uint8_t *packet, int length); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that sets and starts stopwatch for watching packet timeout - /// @description The function sets the stopwatch by getting current time and the time of packet timeout with packet_length. - /// @param packet_length Length of the packet expected to be received - //////////////////////////////////////////////////////////////////////////////// - void setPacketTimeout(uint16_t packet_length); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that sets and starts stopwatch for watching packet timeout - /// @description The function sets the stopwatch by getting current time and the time of packet timeout with msec. - /// @param packet_length Length of the packet expected to be received - //////////////////////////////////////////////////////////////////////////////// - void setPacketTimeout(double msec); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that checks whether packet timeout is occurred - /// @description The function checks whether current time is passed by the time of packet timeout from the time set by PortHandlerMac::setPacketTimeout(). - //////////////////////////////////////////////////////////////////////////////// - bool isPacketTimeout(); +class PortHandlerMac : public PortHandler { +private: + int socket_fd_; + int baudrate_; + char port_name_[100]; + + double packet_start_time_; + double packet_timeout_; + double tx_time_per_byte; + + bool setupPort(const int cflag_baud); + bool setCustomBaudrate(int speed); + int getCFlagBaud(const int baudrate); + + double getCurrentTime(); + double getTimeSinceStart(); + +public: + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that initializes instance of PortHandler and gets port_name + /// @description The function initializes instance of PortHandler and gets port_name. + //////////////////////////////////////////////////////////////////////////////// + PortHandlerMac(const char* port_name); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that closes the port + /// @description The function calls PortHandlerMac::closePort() to close the port. + //////////////////////////////////////////////////////////////////////////////// + virtual ~PortHandlerMac() { closePort(); } + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that opens the port + /// @description The function calls PortHandlerMac::setBaudRate() to open the port. + /// @return communication results which come from PortHandlerMac::setBaudRate() + //////////////////////////////////////////////////////////////////////////////// + bool openPort(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that closes the port + /// @description The function closes the port. + //////////////////////////////////////////////////////////////////////////////// + void closePort(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that clears the port + /// @description The function clears the port. + //////////////////////////////////////////////////////////////////////////////// + void clearPort(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets port name into the port handler + /// @description The function sets port name into the port handler. + /// @param port_name Port name + //////////////////////////////////////////////////////////////////////////////// + void setPortName(const char* port_name); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns port name set into the port handler + /// @description The function returns current port name set into the port handler. + /// @return Port name + //////////////////////////////////////////////////////////////////////////////// + char* getPortName(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets baudrate into the port handler + /// @description The function sets baudrate into the port handler. + /// @param baudrate Baudrate + /// @return false + /// @return when error was occurred during port opening + /// @return or true + //////////////////////////////////////////////////////////////////////////////// + bool setBaudRate(const int baudrate); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns current baudrate set into the port handler + /// @description The function returns current baudrate set into the port handler. + /// @warning Mac OS doesn't support over 230400 bps + /// @return Baudrate + //////////////////////////////////////////////////////////////////////////////// + int getBaudRate(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that checks how much bytes are able to be read from the port buffer + /// @description The function checks how much bytes are able to be read from the port buffer + /// @description and returns the number. + /// @return Length of read-able bytes in the port buffer + //////////////////////////////////////////////////////////////////////////////// + int getBytesAvailable(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that reads bytes from the port buffer + /// @description The function gets bytes from the port buffer, + /// @description and returns a number of bytes read. + /// @param packet Buffer for the packet received + /// @param length Length of the buffer for read + /// @return -1 + /// @return when error was occurred + /// @return or Length of bytes read + //////////////////////////////////////////////////////////////////////////////// + int readPort(uint8_t* packet, int length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that writes bytes on the port buffer + /// @description The function writes bytes on the port buffer, + /// @description and returns a number of bytes which are successfully written. + /// @param packet Buffer which would be written on the port buffer + /// @param length Length of the buffer for write + /// @return -1 + /// @return when error was occurred + /// @return or Length of bytes written + //////////////////////////////////////////////////////////////////////////////// + int writePort(uint8_t* packet, int length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets and starts stopwatch for watching packet timeout + /// @description The function sets the stopwatch by getting current time and the time of packet timeout with packet_length. + /// @param packet_length Length of the packet expected to be received + //////////////////////////////////////////////////////////////////////////////// + void setPacketTimeout(uint16_t packet_length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets and starts stopwatch for watching packet timeout + /// @description The function sets the stopwatch by getting current time and the time of packet timeout with msec. + /// @param packet_length Length of the packet expected to be received + //////////////////////////////////////////////////////////////////////////////// + void setPacketTimeout(double msec); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that checks whether packet timeout is occurred + /// @description The function checks whether current time is passed by the time of packet timeout from the time set by PortHandlerMac::setPacketTimeout(). + //////////////////////////////////////////////////////////////////////////////// + bool isPacketTimeout(); }; -} - +} // namespace dynamixel #endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_MAC_PORTHANDLERMAC_H_ */ diff --git a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/port_handler_windows.h b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/port_handler_windows.h index 0743b2e..964add0 100644 --- a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/port_handler_windows.h +++ b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/port_handler_windows.h @@ -1,18 +1,18 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ //////////////////////////////////////////////////////////////////////////////// /// @file The file for port control in Windows @@ -26,147 +26,144 @@ #include "port_handler.h" -namespace dynamixel -{ +namespace dynamixel { //////////////////////////////////////////////////////////////////////////////// /// @brief The class for control port in Windows //////////////////////////////////////////////////////////////////////////////// -class WINDECLSPEC PortHandlerWindows : public PortHandler -{ - private: - HANDLE serial_handle_; - LARGE_INTEGER freq_, counter_; - - int baudrate_; - char port_name_[100]; - - double packet_start_time_; - double packet_timeout_; - double tx_time_per_byte_; - - bool setupPort(const int baudrate); - - double getCurrentTime(); - double getTimeSinceStart(); - - public: - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that initializes instance of PortHandler and gets port_name - /// @description The function initializes instance of PortHandler and gets port_name. - //////////////////////////////////////////////////////////////////////////////// - PortHandlerWindows(const char *port_name); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that closes the port - /// @description The function calls PortHandlerWindows::closePort() to close the port. - //////////////////////////////////////////////////////////////////////////////// - virtual ~PortHandlerWindows() { closePort(); } - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that opens the port - /// @description The function calls PortHandlerWindows::setBaudRate() to open the port. - /// @return communication results which come from PortHandlerWindows::setBaudRate() - //////////////////////////////////////////////////////////////////////////////// - bool openPort(); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that closes the port - /// @description The function closes the port. - //////////////////////////////////////////////////////////////////////////////// - void closePort(); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that clears the port - /// @description The function clears the port. - //////////////////////////////////////////////////////////////////////////////// - void clearPort(); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that sets port name into the port handler - /// @description The function sets port name into the port handler. - /// @param port_name Port name - //////////////////////////////////////////////////////////////////////////////// - void setPortName(const char *port_name); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that returns port name set into the port handler - /// @description The function returns current port name set into the port handler. - /// @return Port name - //////////////////////////////////////////////////////////////////////////////// - char *getPortName(); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that sets baudrate into the port handler - /// @description The function sets baudrate into the port handler. - /// @param baudrate Baudrate - /// @return false - /// @return when error was occurred during port opening - /// @return or true - //////////////////////////////////////////////////////////////////////////////// - bool setBaudRate(const int baudrate); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that returns current baudrate set into the port handler - /// @description The function returns current baudrate set into the port handler. - /// @return Baudrate - //////////////////////////////////////////////////////////////////////////////// - int getBaudRate(); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that checks how much bytes are able to be read from the port buffer - /// @description The function checks how much bytes are able to be read from the port buffer - /// @description and returns the number. - /// @return Length of read-able bytes in the port buffer - //////////////////////////////////////////////////////////////////////////////// - int getBytesAvailable(); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that reads bytes from the port buffer - /// @description The function gets bytes from the port buffer, - /// @description and returns a number of bytes read. - /// @param packet Buffer for the packet received - /// @param length Length of the buffer for read - /// @return -1 - /// @return when error was occurred - /// @return or Length of bytes read - //////////////////////////////////////////////////////////////////////////////// - int readPort(uint8_t *packet, int length); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that writes bytes on the port buffer - /// @description The function writes bytes on the port buffer, - /// @description and returns a number of bytes which are successfully written. - /// @param packet Buffer which would be written on the port buffer - /// @param length Length of the buffer for write - /// @return -1 - /// @return when error was occurred - /// @return or Length of bytes written - //////////////////////////////////////////////////////////////////////////////// - int writePort(uint8_t *packet, int length); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that sets and starts stopwatch for watching packet timeout - /// @description The function sets the stopwatch by getting current time and the time of packet timeout with packet_length. - /// @param packet_length Length of the packet expected to be received - //////////////////////////////////////////////////////////////////////////////// - void setPacketTimeout(uint16_t packet_length); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that sets and starts stopwatch for watching packet timeout - /// @description The function sets the stopwatch by getting current time and the time of packet timeout with msec. - /// @param packet_length Length of the packet expected to be received - //////////////////////////////////////////////////////////////////////////////// - void setPacketTimeout(double msec); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that checks whether packet timeout is occurred - /// @description The function checks whether current time is passed by the time of packet timeout from the time set by PortHandlerWindows::setPacketTimeout(). - //////////////////////////////////////////////////////////////////////////////// - bool isPacketTimeout(); +class WINDECLSPEC PortHandlerWindows : public PortHandler { +private: + HANDLE serial_handle_; + LARGE_INTEGER freq_, counter_; + + int baudrate_; + char port_name_[100]; + + double packet_start_time_; + double packet_timeout_; + double tx_time_per_byte_; + + bool setupPort(const int baudrate); + + double getCurrentTime(); + double getTimeSinceStart(); + +public: + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that initializes instance of PortHandler and gets port_name + /// @description The function initializes instance of PortHandler and gets port_name. + //////////////////////////////////////////////////////////////////////////////// + PortHandlerWindows(const char* port_name); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that closes the port + /// @description The function calls PortHandlerWindows::closePort() to close the port. + //////////////////////////////////////////////////////////////////////////////// + virtual ~PortHandlerWindows() { closePort(); } + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that opens the port + /// @description The function calls PortHandlerWindows::setBaudRate() to open the port. + /// @return communication results which come from PortHandlerWindows::setBaudRate() + //////////////////////////////////////////////////////////////////////////////// + bool openPort(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that closes the port + /// @description The function closes the port. + //////////////////////////////////////////////////////////////////////////////// + void closePort(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that clears the port + /// @description The function clears the port. + //////////////////////////////////////////////////////////////////////////////// + void clearPort(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets port name into the port handler + /// @description The function sets port name into the port handler. + /// @param port_name Port name + //////////////////////////////////////////////////////////////////////////////// + void setPortName(const char* port_name); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns port name set into the port handler + /// @description The function returns current port name set into the port handler. + /// @return Port name + //////////////////////////////////////////////////////////////////////////////// + char* getPortName(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets baudrate into the port handler + /// @description The function sets baudrate into the port handler. + /// @param baudrate Baudrate + /// @return false + /// @return when error was occurred during port opening + /// @return or true + //////////////////////////////////////////////////////////////////////////////// + bool setBaudRate(const int baudrate); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns current baudrate set into the port handler + /// @description The function returns current baudrate set into the port handler. + /// @return Baudrate + //////////////////////////////////////////////////////////////////////////////// + int getBaudRate(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that checks how much bytes are able to be read from the port buffer + /// @description The function checks how much bytes are able to be read from the port buffer + /// @description and returns the number. + /// @return Length of read-able bytes in the port buffer + //////////////////////////////////////////////////////////////////////////////// + int getBytesAvailable(); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that reads bytes from the port buffer + /// @description The function gets bytes from the port buffer, + /// @description and returns a number of bytes read. + /// @param packet Buffer for the packet received + /// @param length Length of the buffer for read + /// @return -1 + /// @return when error was occurred + /// @return or Length of bytes read + //////////////////////////////////////////////////////////////////////////////// + int readPort(uint8_t* packet, int length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that writes bytes on the port buffer + /// @description The function writes bytes on the port buffer, + /// @description and returns a number of bytes which are successfully written. + /// @param packet Buffer which would be written on the port buffer + /// @param length Length of the buffer for write + /// @return -1 + /// @return when error was occurred + /// @return or Length of bytes written + //////////////////////////////////////////////////////////////////////////////// + int writePort(uint8_t* packet, int length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets and starts stopwatch for watching packet timeout + /// @description The function sets the stopwatch by getting current time and the time of packet timeout with packet_length. + /// @param packet_length Length of the packet expected to be received + //////////////////////////////////////////////////////////////////////////////// + void setPacketTimeout(uint16_t packet_length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that sets and starts stopwatch for watching packet timeout + /// @description The function sets the stopwatch by getting current time and the time of packet timeout with msec. + /// @param packet_length Length of the packet expected to be received + //////////////////////////////////////////////////////////////////////////////// + void setPacketTimeout(double msec); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that checks whether packet timeout is occurred + /// @description The function checks whether current time is passed by the time of packet timeout from the time set by PortHandlerWindows::setPacketTimeout(). + //////////////////////////////////////////////////////////////////////////////// + bool isPacketTimeout(); }; -} - +} // namespace dynamixel #endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_WINDOWS_PORTHANDLERWINDOWS_H_ */ diff --git a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/protocol1_packet_handler.h b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/protocol1_packet_handler.h index 3dcbadd..6dc017f 100644 --- a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/protocol1_packet_handler.h +++ b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/protocol1_packet_handler.h @@ -1,18 +1,18 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ //////////////////////////////////////////////////////////////////////////////// /// @file The file for Protocol 1.0 Dynamixel packet control @@ -22,516 +22,513 @@ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ #define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ - #include "packet_handler.h" -namespace dynamixel -{ +namespace dynamixel { //////////////////////////////////////////////////////////////////////////////// /// @brief The class for control Dynamixel by using Protocol1.0 //////////////////////////////////////////////////////////////////////////////// -class WINDECLSPEC Protocol1PacketHandler : public PacketHandler -{ - private: - static Protocol1PacketHandler *unique_instance_; - - Protocol1PacketHandler(); - - public: - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that returns Protocol1PacketHandler instance - /// @return Protocol1PacketHandler instance - //////////////////////////////////////////////////////////////////////////////// - static Protocol1PacketHandler *getInstance() { return unique_instance_; } - - virtual ~Protocol1PacketHandler() { } - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that returns Protocol version used in Protocol1PacketHandler (1.0) - /// @return 1.0 - //////////////////////////////////////////////////////////////////////////////// - float getProtocolVersion() { return 1.0; } - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that gets description of communication result - /// @param result Communication result which might be gotten by the tx rx functions - /// @return description of communication result in const char* (string) - //////////////////////////////////////////////////////////////////////////////// - const char *getTxRxResult (int result); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that gets description of hardware error - /// @param error Dynamixel hardware error which might be gotten by the tx rx functions - /// @return description of hardware error in const char* (string) - //////////////////////////////////////////////////////////////////////////////// - const char *getRxPacketError (uint8_t error); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits the instruction packet txpacket via PortHandler port. - /// @description The function clears the port buffer by PortHandler::clearPort() function, - /// @description then transmits txpacket by PortHandler::writePort() function. - /// @description The function activates only when the port is not busy and when the packet is already written on the port buffer - /// @param port PortHandler instance - /// @param txpacket packet for transmission - /// @return COMM_PORT_BUSY - /// @return when the port is already in use - /// @return COMM_TX_ERROR - /// @return when txpacket is out of range described by TXPACKET_MAX_LEN - /// @return COMM_TX_FAIL - /// @return when written packet is shorter than expected - /// @return or COMM_SUCCESS - //////////////////////////////////////////////////////////////////////////////// - int txPacket (PortHandler *port, uint8_t *txpacket); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that receives packet (rxpacket) during designated time via PortHandler port - /// @description The function repeatedly tries to receive rxpacket by PortHandler::readPort() function. - /// @description It breaks out - /// @description when PortHandler::isPacketTimeout() shows the timeout, - /// @description when rxpacket seemed as corrupted, or - /// @description when nothing received - /// @param port PortHandler instance - /// @param rxpacket received packet - /// @return COMM_RX_CORRUPT - /// @return when it received the packet but it couldn't find header in the packet - /// @return when it found header in the packet but the id, length or error value is out of range - /// @return when it received the packet but it is shorted than expected - /// @return COMM_RX_TIMEOUT - /// @return when there is no rxpacket received until PortHandler::isPacketTimeout() shows the timeout - /// @return COMM_SUCCESS - /// @return when rxpacket passes checksum test - /// @return or COMM_RX_FAIL - //////////////////////////////////////////////////////////////////////////////// - int rxPacket (PortHandler *port, uint8_t *rxpacket); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits packet (txpacket) and receives packet (rxpacket) during designated time via PortHandler port - /// @description The function calls Protocol1PacketHandler::txPacket(), - /// @description and calls Protocol1PacketHandler::rxPacket() if it succeeds Protocol1PacketHandler::txPacket(). - /// @description It breaks out - /// @description when it fails Protocol1PacketHandler::txPacket(), - /// @description when txpacket is called by Protocol1PacketHandler::broadcastPing() / Protocol1PacketHandler::syncWriteTxOnly() / Protocol1PacketHandler::regWriteTxOnly / Protocol1PacketHandler::action - /// @param port PortHandler instance - /// @param txpacket packet for transmission - /// @param rxpacket received packet - /// @return COMM_SUCCESS - /// @return when it succeeds Protocol1PacketHandler::txPacket() and Protocol1PacketHandler::rxPacket() - /// @return or the other communication results which come from Protocol1PacketHandler::txPacket() and Protocol1PacketHandler::rxPacket() - //////////////////////////////////////////////////////////////////////////////// - int txRxPacket (PortHandler *port, uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that pings Dynamixel but doesn't take its model number - /// @description The function calls Protocol1PacketHandler::ping() which gets Dynamixel model number, - /// @description but doesn't carry the model number - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param error Dynamixel hardware error - /// @return communication results which come from Protocol1PacketHandler::ping() - //////////////////////////////////////////////////////////////////////////////// - int ping (PortHandler *port, uint8_t id, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that pings Dynamixel and takes its model number - /// @description The function makes an instruction packet with INST_PING, - /// @description transmits the packet with Protocol1PacketHandler::txRxPacket(), - /// @description and call Protocol1PacketHandler::readTxRx to read model_number in the rx buffer. - /// @description It breaks out - /// @description when it tries to transmit to BROADCAST_ID - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param error Dynamixel hardware error - /// @return COMM_NOT_AVAILABLE - /// @return when it tries to transmit to BROADCAST_ID - /// @return COMM_SUCCESS - /// @return when it succeeds to ping Dynamixel and get model_number from it - /// @return or the other communication results which come from Protocol1PacketHandler::txRxPacket() and Protocol1PacketHandler::readTxRx() - //////////////////////////////////////////////////////////////////////////////// - int ping (PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief (Available only in Protocol 2.0) The function that pings all connected Dynamixel - /// @param port PortHandler instance - /// @param id_list ID list of Dynamixels which are found by broadcast ping - /// @return COMM_NOT_AVAILABLE - //////////////////////////////////////////////////////////////////////////////// - int broadcastPing (PortHandler *port, std::vector &id_list); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that makes Dynamixels run as written in the Dynamixel register - /// @description The function makes an instruction packet with INST_ACTION, - /// @description transmits the packet with Protocol1PacketHandler::txRxPacket(). - /// @description To use this function, Dynamixel register should be set by Protocol1PacketHandler::regWriteTxOnly() or Protocol1PacketHandler::regWriteTxRx() - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @return communication results which come from Protocol1PacketHandler::txRxPacket() - //////////////////////////////////////////////////////////////////////////////// - int action (PortHandler *port, uint8_t id); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief (Available only in Protocol 2.0) The function that makes Dynamixel reboot - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param error Dynamixel hardware error - /// @return COMM_NOT_AVAILABLE - //////////////////////////////////////////////////////////////////////////////// - int reboot (PortHandler *port, uint8_t id, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief (Available only in Protocol 2.0) The function that reset multi-turn revolution information of Dynamixel - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param error Dynamixel hardware error - /// @return COMM_NOT_AVAILABLE - //////////////////////////////////////////////////////////////////////////////// - int clearMultiTurn (PortHandler *port, uint8_t id, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that makes Dynamixel reset as it was produced in the factory - /// @description The function makes an instruction packet with INST_FACTORY_RESET, - /// @description transmits the packet with Protocol1PacketHandler::txRxPacket(). - /// @description Be careful of the use. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param option (Not available in Protocol 1.0) Reset option - /// @param error Dynamixel hardware error - /// @return communication results which come from Protocol1PacketHandler::txRxPacket() - //////////////////////////////////////////////////////////////////////////////// - int factoryReset (PortHandler *port, uint8_t id, uint8_t option, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits INST_READ instruction packet - /// @description The function makes an instruction packet with INST_READ, - /// @description transmits the packet with Protocol1PacketHandler::txPacket(). - /// @description It breaks out - /// @description when it tries to transmit to BROADCAST_ID - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for read - /// @param length Length of the data for read - /// @return COMM_NOT_AVAILABLE - /// @return when it tries to transmit to BROADCAST_ID - /// @return or the other communication results which come from Protocol1PacketHandler::txPacket() - //////////////////////////////////////////////////////////////////////////////// - int readTx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that receives the packet and reads the data in the packet - /// @description The function receives the packet which might be come by previous INST_READ instruction packet transmission, - /// @description gets the data from the packet. - /// @param port PortHandler instance - /// @param length Length of the data for read - /// @param data Data extracted from the packet - /// @param error Dynamixel hardware error - /// @return communication results which come from Protocol1PacketHandler::rxPacket() - //////////////////////////////////////////////////////////////////////////////// - int readRx (PortHandler *port, uint8_t id, uint16_t length, uint8_t *data, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits INST_READ instruction packet, and read data from received packet - /// @description The function makes an instruction packet with INST_READ, - /// @description transmits and receives the packet with Protocol1PacketHandler::txRxPacket(), - /// @description gets the data from the packet. - /// @description It breaks out - /// @description when it tries to transmit to BROADCAST_ID - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for read - /// @param length Length of the data for read - /// @param data Data extracted from the packet - /// @param error Dynamixel hardware error - /// @return COMM_NOT_AVAILABLE - /// @return when it tries to transmit to BROADCAST_ID - /// @return or the other communication results which come from Protocol1PacketHandler::txRxPacket() - //////////////////////////////////////////////////////////////////////////////// - int readTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls Protocol1PacketHandler::readTx() function for reading 1 byte data - /// @description The function calls Protocol1PacketHandler::readTx() function for reading 1 byte data - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for read - /// @return communication results which come from Protocol1PacketHandler::readTx() - //////////////////////////////////////////////////////////////////////////////// - int read1ByteTx (PortHandler *port, uint8_t id, uint16_t address); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls Protocol1PacketHandler::readRx() function and reads 1 byte data on the packet - /// @description The function calls Protocol1PacketHandler::readRx() function, - /// @description gets 1 byte data from the packet. - /// @param port PortHandler instance - /// @param data Data extracted from the packet - /// @param error Dynamixel hardware error - /// @return communication results which come from Protocol1PacketHandler::readRx() - //////////////////////////////////////////////////////////////////////////////// - int read1ByteRx (PortHandler *port, uint8_t id, uint8_t *data, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls Protocol1PacketHandler::readTxRx() function for reading 1 byte data - /// @description The function calls Protocol1PacketHandler::readTxRx(), - /// @description gets 1 byte data from the packet. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for read - /// @param length Length of the data for read - /// @param data Data extracted from the packet - /// @param error Dynamixel hardware error - /// @return communication results which come from Protocol1PacketHandler::txRxPacket() - //////////////////////////////////////////////////////////////////////////////// - int read1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls Protocol1PacketHandler::readTx() function for reading 2 byte data - /// @description The function calls Protocol1PacketHandler::readTx() function for reading 2 byte data - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for read - /// @return communication results which come from Protocol1PacketHandler::readTx() - //////////////////////////////////////////////////////////////////////////////// - int read2ByteTx (PortHandler *port, uint8_t id, uint16_t address); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls Protocol1PacketHandler::readRx() function and reads 2 byte data on the packet - /// @description The function calls Protocol1PacketHandler::readRx() function, - /// @description gets 2 byte data from the packet. - /// @param port PortHandler instance - /// @param data Data extracted from the packet - /// @param error Dynamixel hardware error - /// @return communication results which come from Protocol1PacketHandler::readRx() - //////////////////////////////////////////////////////////////////////////////// - int read2ByteRx (PortHandler *port, uint8_t id, uint16_t *data, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls Protocol1PacketHandler::readTxRx() function for reading 2 byte data - /// @description The function calls Protocol1PacketHandler::readTxRx(), - /// @description gets 2 byte data from the packet. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for read - /// @param length Length of the data for read - /// @param data Data extracted from the packet - /// @param error Dynamixel hardware error - /// @return communication results which come from Protocol1PacketHandler::txRxPacket() - //////////////////////////////////////////////////////////////////////////////// - int read2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls Protocol1PacketHandler::readTx() function for reading 4 byte data - /// @description The function calls Protocol1PacketHandler::readTx() function for reading 4 byte data - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for read - /// @return communication results which come from Protocol1PacketHandler::readTx() - //////////////////////////////////////////////////////////////////////////////// - int read4ByteTx (PortHandler *port, uint8_t id, uint16_t address); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls Protocol1PacketHandler::readRx() function and reads 4 byte data on the packet - /// @description The function calls Protocol1PacketHandler::readRx() function, - /// @description gets 4 byte data from the packet. - /// @param port PortHandler instance - /// @param data Data extracted from the packet - /// @param error Dynamixel hardware error - /// @return communication results which come from Protocol1PacketHandler::readRx() - //////////////////////////////////////////////////////////////////////////////// - int read4ByteRx (PortHandler *port, uint8_t id, uint32_t *data, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls Protocol1PacketHandler::readTxRx() function for reading 4 byte data - /// @description The function calls Protocol1PacketHandler::readTxRx(), - /// @description gets 4 byte data from the packet. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for read - /// @param length Length of the data for read - /// @param data Data extracted from the packet - /// @param error Dynamixel hardware error - /// @return communication results which come from Protocol1PacketHandler::txRxPacket() - //////////////////////////////////////////////////////////////////////////////// - int read4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits INST_WRITE instruction packet with the data for write - /// @description The function makes an instruction packet with INST_WRITE and the data for write, - /// @description transmits the packet with Protocol1PacketHandler::txPacket(). - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for write - /// @param length Length of the data for write - /// @param data Data for write - /// @return communication results which come from Protocol1PacketHandler::txPacket() - //////////////////////////////////////////////////////////////////////////////// - int writeTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits INST_WRITE instruction packet with the data for write, and receives the packet - /// @description The function makes an instruction packet with INST_WRITE and the data for write, - /// @description transmits and receives the packet with Protocol1PacketHandler::txRxPacket(), - /// @description gets the error from the packet. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for write - /// @param length Length of the data for write - /// @param data Data for write - /// @param error Dynamixel hardware error - /// @return communication results which come from Protocol1PacketHandler::txRxPacket() - //////////////////////////////////////////////////////////////////////////////// - int writeTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls Protocol1PacketHandler::writeTxOnly() for writing 1 byte data - /// @description The function calls Protocol1PacketHandler::writeTxOnly() for writing 1 byte data. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for write - /// @param data Data for write - /// @return communication results which come from Protocol1PacketHandler::writeTxOnly() - //////////////////////////////////////////////////////////////////////////////// - int write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls Protocol1PacketHandler::writeTxRx() for writing 1 byte data and receives the packet - /// @description The function calls Protocol1PacketHandler::writeTxRx() for writing 1 byte data and receves the packet, - /// @description gets the error from the packet. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for write - /// @param data Data for write - /// @param error Dynamixel hardware error - /// @return communication results which come from Protocol1PacketHandler::writeTxRx() - //////////////////////////////////////////////////////////////////////////////// - int write1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls Protocol1PacketHandler::writeTxOnly() for writing 2 byte data - /// @description The function calls Protocol1PacketHandler::writeTxOnly() for writing 2 byte data. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for write - /// @param data Data for write - /// @return communication results which come from Protocol1PacketHandler::writeTxOnly() - //////////////////////////////////////////////////////////////////////////////// - int write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls Protocol1PacketHandler::writeTxRx() for writing 2 byte data and receives the packet - /// @description The function calls Protocol1PacketHandler::writeTxRx() for writing 2 byte data and receves the packet, - /// @description gets the error from the packet. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for write - /// @param data Data for write - /// @param error Dynamixel hardware error - /// @return communication results which come from Protocol1PacketHandler::writeTxRx() - //////////////////////////////////////////////////////////////////////////////// - int write2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls Protocol1PacketHandler::writeTxOnly() for writing 4 byte data - /// @description The function calls Protocol1PacketHandler::writeTxOnly() for writing 4 byte data. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for write - /// @param data Data for write - /// @return communication results which come from Protocol1PacketHandler::writeTxOnly() - //////////////////////////////////////////////////////////////////////////////// - int write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls Protocol1PacketHandler::writeTxRx() for writing 4 byte data and receives the packet - /// @description The function calls Protocol1PacketHandler::writeTxRx() for writing 4 byte data and receves the packet, - /// @description gets the error from the packet. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for write - /// @param data Data for write - /// @param error Dynamixel hardware error - /// @return communication results which come from Protocol1PacketHandler::writeTxRx() - //////////////////////////////////////////////////////////////////////////////// - int write4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits INST_REG_WRITE instruction packet with the data for writing on the Dynamixel register - /// @description The function makes an instruction packet with INST_REG_WRITE and the data for writing on the Dynamixel register, - /// @description transmits the packet with Protocol1PacketHandler::txPacket(). - /// @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamixel. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for write - /// @param length Length of the data for write - /// @param data Data for write - /// @return communication results which come from Protocol1PacketHandler::txPacket() - //////////////////////////////////////////////////////////////////////////////// - int regWriteTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits INST_REG_WRITE instruction packet with the data for writing on the Dynamixel register, and receives the packet - /// @description The function makes an instruction packet with INST_REG_WRITE and the data for writing on the Dynamixel register, - /// @description transmits and receives the packet with Protocol1PacketHandler::txRxPacket(), - /// @description gets the error from the packet. - /// @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamixel. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for write - /// @param length Length of the data for write - /// @param data Data for write - /// @param error Dynamixel hardware error - /// @return communication results which come from Protocol1PacketHandler::txRxPacket() - //////////////////////////////////////////////////////////////////////////////// - int regWriteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief (Available only in Protocol 2.0) The function that transmits Sync Read instruction packet - /// @param port PortHandler instance - /// @param start_address Address of the data for Sync Read - /// @param data_length Length of the data for Sync Read - /// @param param Parameter for Sync Read - /// @param param_length Length of the data for Sync Read - /// @return COMM_NOT_AVAILABLE - //////////////////////////////////////////////////////////////////////////////// - int syncReadTx (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length); - // SyncReadRx -> GroupSyncRead class - // SyncReadTxRx -> GroupSyncRead class - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits Sync Write instruction packet - /// @description The function makes an instruction packet with INST_SYNC_WRITE, - /// @description transmits the packet with Protocol1PacketHandler::txRxPacket(). - /// @param port PortHandler instance - /// @param start_address Address of the data for Sync Write - /// @param data_length Length of the data for Sync Write - /// @param param Parameter for Sync Write {ID1, DATA0, DATA1, ..., DATAn, ID2, DATA0, DATA1, ..., DATAn, ID3, DATA0, DATA1, ..., DATAn} - /// @param param_length Length of the data for Sync Write - /// @return communication results which come from Protocol1PacketHandler::txRxPacket() - //////////////////////////////////////////////////////////////////////////////// - int syncWriteTxOnly (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief (Available only on Dynamixel MX / X series) The function that transmits Bulk Read instruction packet - /// @description The function makes an instruction packet with INST_BULK_READ, - /// @description transmits the packet with Protocol1PacketHandler::txPacket(). - /// @param port PortHandler instance - /// @param param Parameter for Bulk Read {LEN1, ID1, ADDR1, LEN2, ID2, ADDR2, ...} - /// @param param_length Length of the data for Bulk Read - /// @return communication results which come from Protocol1PacketHandler::txPacket() - //////////////////////////////////////////////////////////////////////////////// - int bulkReadTx (PortHandler *port, uint8_t *param, uint16_t param_length); - // BulkReadRx -> GroupBulkRead class - // BulkReadTxRx -> GroupBulkRead class - - //////////////////////////////////////////////////////////////////////////////// - /// @brief (Available only in Protocol 2.0) The function that transmits Bulk Write instruction packet - /// @param port PortHandler instance - /// @param param Parameter for Bulk Write - /// @param param_length Length of the data for Bulk Write - /// @return COMM_NOT_AVAILABLE - //////////////////////////////////////////////////////////////////////////////// - int bulkWriteTxOnly (PortHandler *port, uint8_t *param, uint16_t param_length); +class WINDECLSPEC Protocol1PacketHandler : public PacketHandler { +private: + static Protocol1PacketHandler* unique_instance_; + + Protocol1PacketHandler(); + +public: + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns Protocol1PacketHandler instance + /// @return Protocol1PacketHandler instance + //////////////////////////////////////////////////////////////////////////////// + static Protocol1PacketHandler* getInstance() { return unique_instance_; } + + virtual ~Protocol1PacketHandler() {} + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns Protocol version used in Protocol1PacketHandler (1.0) + /// @return 1.0 + //////////////////////////////////////////////////////////////////////////////// + float getProtocolVersion() { return 1.0; } + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that gets description of communication result + /// @param result Communication result which might be gotten by the tx rx functions + /// @return description of communication result in const char* (string) + //////////////////////////////////////////////////////////////////////////////// + const char* getTxRxResult(int result); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that gets description of hardware error + /// @param error Dynamixel hardware error which might be gotten by the tx rx functions + /// @return description of hardware error in const char* (string) + //////////////////////////////////////////////////////////////////////////////// + const char* getRxPacketError(uint8_t error); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits the instruction packet txpacket via PortHandler port. + /// @description The function clears the port buffer by PortHandler::clearPort() function, + /// @description then transmits txpacket by PortHandler::writePort() function. + /// @description The function activates only when the port is not busy and when the packet is already written on the port buffer + /// @param port PortHandler instance + /// @param txpacket packet for transmission + /// @return COMM_PORT_BUSY + /// @return when the port is already in use + /// @return COMM_TX_ERROR + /// @return when txpacket is out of range described by TXPACKET_MAX_LEN + /// @return COMM_TX_FAIL + /// @return when written packet is shorter than expected + /// @return or COMM_SUCCESS + //////////////////////////////////////////////////////////////////////////////// + int txPacket(PortHandler* port, uint8_t* txpacket); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that receives packet (rxpacket) during designated time via PortHandler port + /// @description The function repeatedly tries to receive rxpacket by PortHandler::readPort() function. + /// @description It breaks out + /// @description when PortHandler::isPacketTimeout() shows the timeout, + /// @description when rxpacket seemed as corrupted, or + /// @description when nothing received + /// @param port PortHandler instance + /// @param rxpacket received packet + /// @return COMM_RX_CORRUPT + /// @return when it received the packet but it couldn't find header in the packet + /// @return when it found header in the packet but the id, length or error value is out of range + /// @return when it received the packet but it is shorted than expected + /// @return COMM_RX_TIMEOUT + /// @return when there is no rxpacket received until PortHandler::isPacketTimeout() shows the timeout + /// @return COMM_SUCCESS + /// @return when rxpacket passes checksum test + /// @return or COMM_RX_FAIL + //////////////////////////////////////////////////////////////////////////////// + int rxPacket(PortHandler* port, uint8_t* rxpacket); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits packet (txpacket) and receives packet (rxpacket) during designated time via PortHandler port + /// @description The function calls Protocol1PacketHandler::txPacket(), + /// @description and calls Protocol1PacketHandler::rxPacket() if it succeeds Protocol1PacketHandler::txPacket(). + /// @description It breaks out + /// @description when it fails Protocol1PacketHandler::txPacket(), + /// @description when txpacket is called by Protocol1PacketHandler::broadcastPing() / Protocol1PacketHandler::syncWriteTxOnly() / Protocol1PacketHandler::regWriteTxOnly / + /// Protocol1PacketHandler::action + /// @param port PortHandler instance + /// @param txpacket packet for transmission + /// @param rxpacket received packet + /// @return COMM_SUCCESS + /// @return when it succeeds Protocol1PacketHandler::txPacket() and Protocol1PacketHandler::rxPacket() + /// @return or the other communication results which come from Protocol1PacketHandler::txPacket() and Protocol1PacketHandler::rxPacket() + //////////////////////////////////////////////////////////////////////////////// + int txRxPacket(PortHandler* port, uint8_t* txpacket, uint8_t* rxpacket, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that pings Dynamixel but doesn't take its model number + /// @description The function calls Protocol1PacketHandler::ping() which gets Dynamixel model number, + /// @description but doesn't carry the model number + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param error Dynamixel hardware error + /// @return communication results which come from Protocol1PacketHandler::ping() + //////////////////////////////////////////////////////////////////////////////// + int ping(PortHandler* port, uint8_t id, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that pings Dynamixel and takes its model number + /// @description The function makes an instruction packet with INST_PING, + /// @description transmits the packet with Protocol1PacketHandler::txRxPacket(), + /// @description and call Protocol1PacketHandler::readTxRx to read model_number in the rx buffer. + /// @description It breaks out + /// @description when it tries to transmit to BROADCAST_ID + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param error Dynamixel hardware error + /// @return COMM_NOT_AVAILABLE + /// @return when it tries to transmit to BROADCAST_ID + /// @return COMM_SUCCESS + /// @return when it succeeds to ping Dynamixel and get model_number from it + /// @return or the other communication results which come from Protocol1PacketHandler::txRxPacket() and Protocol1PacketHandler::readTxRx() + //////////////////////////////////////////////////////////////////////////////// + int ping(PortHandler* port, uint8_t id, uint16_t* model_number, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief (Available only in Protocol 2.0) The function that pings all connected Dynamixel + /// @param port PortHandler instance + /// @param id_list ID list of Dynamixels which are found by broadcast ping + /// @return COMM_NOT_AVAILABLE + //////////////////////////////////////////////////////////////////////////////// + int broadcastPing(PortHandler* port, std::vector& id_list); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that makes Dynamixels run as written in the Dynamixel register + /// @description The function makes an instruction packet with INST_ACTION, + /// @description transmits the packet with Protocol1PacketHandler::txRxPacket(). + /// @description To use this function, Dynamixel register should be set by Protocol1PacketHandler::regWriteTxOnly() or Protocol1PacketHandler::regWriteTxRx() + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @return communication results which come from Protocol1PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int action(PortHandler* port, uint8_t id); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief (Available only in Protocol 2.0) The function that makes Dynamixel reboot + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param error Dynamixel hardware error + /// @return COMM_NOT_AVAILABLE + //////////////////////////////////////////////////////////////////////////////// + int reboot(PortHandler* port, uint8_t id, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief (Available only in Protocol 2.0) The function that reset multi-turn revolution information of Dynamixel + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param error Dynamixel hardware error + /// @return COMM_NOT_AVAILABLE + //////////////////////////////////////////////////////////////////////////////// + int clearMultiTurn(PortHandler* port, uint8_t id, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that makes Dynamixel reset as it was produced in the factory + /// @description The function makes an instruction packet with INST_FACTORY_RESET, + /// @description transmits the packet with Protocol1PacketHandler::txRxPacket(). + /// @description Be careful of the use. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param option (Not available in Protocol 1.0) Reset option + /// @param error Dynamixel hardware error + /// @return communication results which come from Protocol1PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int factoryReset(PortHandler* port, uint8_t id, uint8_t option, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_READ instruction packet + /// @description The function makes an instruction packet with INST_READ, + /// @description transmits the packet with Protocol1PacketHandler::txPacket(). + /// @description It breaks out + /// @description when it tries to transmit to BROADCAST_ID + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @param length Length of the data for read + /// @return COMM_NOT_AVAILABLE + /// @return when it tries to transmit to BROADCAST_ID + /// @return or the other communication results which come from Protocol1PacketHandler::txPacket() + //////////////////////////////////////////////////////////////////////////////// + int readTx(PortHandler* port, uint8_t id, uint16_t address, uint16_t length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that receives the packet and reads the data in the packet + /// @description The function receives the packet which might be come by previous INST_READ instruction packet transmission, + /// @description gets the data from the packet. + /// @param port PortHandler instance + /// @param length Length of the data for read + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from Protocol1PacketHandler::rxPacket() + //////////////////////////////////////////////////////////////////////////////// + int readRx(PortHandler* port, uint8_t id, uint16_t length, uint8_t* data, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_READ instruction packet, and read data from received packet + /// @description The function makes an instruction packet with INST_READ, + /// @description transmits and receives the packet with Protocol1PacketHandler::txRxPacket(), + /// @description gets the data from the packet. + /// @description It breaks out + /// @description when it tries to transmit to BROADCAST_ID + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @param length Length of the data for read + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return COMM_NOT_AVAILABLE + /// @return when it tries to transmit to BROADCAST_ID + /// @return or the other communication results which come from Protocol1PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int readTxRx(PortHandler* port, uint8_t id, uint16_t address, uint16_t length, uint8_t* data, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol1PacketHandler::readTx() function for reading 1 byte data + /// @description The function calls Protocol1PacketHandler::readTx() function for reading 1 byte data + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @return communication results which come from Protocol1PacketHandler::readTx() + //////////////////////////////////////////////////////////////////////////////// + int read1ByteTx(PortHandler* port, uint8_t id, uint16_t address); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol1PacketHandler::readRx() function and reads 1 byte data on the packet + /// @description The function calls Protocol1PacketHandler::readRx() function, + /// @description gets 1 byte data from the packet. + /// @param port PortHandler instance + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from Protocol1PacketHandler::readRx() + //////////////////////////////////////////////////////////////////////////////// + int read1ByteRx(PortHandler* port, uint8_t id, uint8_t* data, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol1PacketHandler::readTxRx() function for reading 1 byte data + /// @description The function calls Protocol1PacketHandler::readTxRx(), + /// @description gets 1 byte data from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @param length Length of the data for read + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from Protocol1PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int read1ByteTxRx(PortHandler* port, uint8_t id, uint16_t address, uint8_t* data, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol1PacketHandler::readTx() function for reading 2 byte data + /// @description The function calls Protocol1PacketHandler::readTx() function for reading 2 byte data + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @return communication results which come from Protocol1PacketHandler::readTx() + //////////////////////////////////////////////////////////////////////////////// + int read2ByteTx(PortHandler* port, uint8_t id, uint16_t address); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol1PacketHandler::readRx() function and reads 2 byte data on the packet + /// @description The function calls Protocol1PacketHandler::readRx() function, + /// @description gets 2 byte data from the packet. + /// @param port PortHandler instance + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from Protocol1PacketHandler::readRx() + //////////////////////////////////////////////////////////////////////////////// + int read2ByteRx(PortHandler* port, uint8_t id, uint16_t* data, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol1PacketHandler::readTxRx() function for reading 2 byte data + /// @description The function calls Protocol1PacketHandler::readTxRx(), + /// @description gets 2 byte data from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @param length Length of the data for read + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from Protocol1PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int read2ByteTxRx(PortHandler* port, uint8_t id, uint16_t address, uint16_t* data, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol1PacketHandler::readTx() function for reading 4 byte data + /// @description The function calls Protocol1PacketHandler::readTx() function for reading 4 byte data + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @return communication results which come from Protocol1PacketHandler::readTx() + //////////////////////////////////////////////////////////////////////////////// + int read4ByteTx(PortHandler* port, uint8_t id, uint16_t address); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol1PacketHandler::readRx() function and reads 4 byte data on the packet + /// @description The function calls Protocol1PacketHandler::readRx() function, + /// @description gets 4 byte data from the packet. + /// @param port PortHandler instance + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from Protocol1PacketHandler::readRx() + //////////////////////////////////////////////////////////////////////////////// + int read4ByteRx(PortHandler* port, uint8_t id, uint32_t* data, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol1PacketHandler::readTxRx() function for reading 4 byte data + /// @description The function calls Protocol1PacketHandler::readTxRx(), + /// @description gets 4 byte data from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @param length Length of the data for read + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from Protocol1PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int read4ByteTxRx(PortHandler* port, uint8_t id, uint16_t address, uint32_t* data, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_WRITE instruction packet with the data for write + /// @description The function makes an instruction packet with INST_WRITE and the data for write, + /// @description transmits the packet with Protocol1PacketHandler::txPacket(). + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param length Length of the data for write + /// @param data Data for write + /// @return communication results which come from Protocol1PacketHandler::txPacket() + //////////////////////////////////////////////////////////////////////////////// + int writeTxOnly(PortHandler* port, uint8_t id, uint16_t address, uint16_t length, uint8_t* data); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_WRITE instruction packet with the data for write, and receives the packet + /// @description The function makes an instruction packet with INST_WRITE and the data for write, + /// @description transmits and receives the packet with Protocol1PacketHandler::txRxPacket(), + /// @description gets the error from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param length Length of the data for write + /// @param data Data for write + /// @param error Dynamixel hardware error + /// @return communication results which come from Protocol1PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int writeTxRx(PortHandler* port, uint8_t id, uint16_t address, uint16_t length, uint8_t* data, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol1PacketHandler::writeTxOnly() for writing 1 byte data + /// @description The function calls Protocol1PacketHandler::writeTxOnly() for writing 1 byte data. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param data Data for write + /// @return communication results which come from Protocol1PacketHandler::writeTxOnly() + //////////////////////////////////////////////////////////////////////////////// + int write1ByteTxOnly(PortHandler* port, uint8_t id, uint16_t address, uint8_t data); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol1PacketHandler::writeTxRx() for writing 1 byte data and receives the packet + /// @description The function calls Protocol1PacketHandler::writeTxRx() for writing 1 byte data and receves the packet, + /// @description gets the error from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param data Data for write + /// @param error Dynamixel hardware error + /// @return communication results which come from Protocol1PacketHandler::writeTxRx() + //////////////////////////////////////////////////////////////////////////////// + int write1ByteTxRx(PortHandler* port, uint8_t id, uint16_t address, uint8_t data, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol1PacketHandler::writeTxOnly() for writing 2 byte data + /// @description The function calls Protocol1PacketHandler::writeTxOnly() for writing 2 byte data. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param data Data for write + /// @return communication results which come from Protocol1PacketHandler::writeTxOnly() + //////////////////////////////////////////////////////////////////////////////// + int write2ByteTxOnly(PortHandler* port, uint8_t id, uint16_t address, uint16_t data); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol1PacketHandler::writeTxRx() for writing 2 byte data and receives the packet + /// @description The function calls Protocol1PacketHandler::writeTxRx() for writing 2 byte data and receves the packet, + /// @description gets the error from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param data Data for write + /// @param error Dynamixel hardware error + /// @return communication results which come from Protocol1PacketHandler::writeTxRx() + //////////////////////////////////////////////////////////////////////////////// + int write2ByteTxRx(PortHandler* port, uint8_t id, uint16_t address, uint16_t data, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol1PacketHandler::writeTxOnly() for writing 4 byte data + /// @description The function calls Protocol1PacketHandler::writeTxOnly() for writing 4 byte data. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param data Data for write + /// @return communication results which come from Protocol1PacketHandler::writeTxOnly() + //////////////////////////////////////////////////////////////////////////////// + int write4ByteTxOnly(PortHandler* port, uint8_t id, uint16_t address, uint32_t data); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol1PacketHandler::writeTxRx() for writing 4 byte data and receives the packet + /// @description The function calls Protocol1PacketHandler::writeTxRx() for writing 4 byte data and receves the packet, + /// @description gets the error from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param data Data for write + /// @param error Dynamixel hardware error + /// @return communication results which come from Protocol1PacketHandler::writeTxRx() + //////////////////////////////////////////////////////////////////////////////// + int write4ByteTxRx(PortHandler* port, uint8_t id, uint16_t address, uint32_t data, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_REG_WRITE instruction packet with the data for writing on the Dynamixel register + /// @description The function makes an instruction packet with INST_REG_WRITE and the data for writing on the Dynamixel register, + /// @description transmits the packet with Protocol1PacketHandler::txPacket(). + /// @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamixel. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param length Length of the data for write + /// @param data Data for write + /// @return communication results which come from Protocol1PacketHandler::txPacket() + //////////////////////////////////////////////////////////////////////////////// + int regWriteTxOnly(PortHandler* port, uint8_t id, uint16_t address, uint16_t length, uint8_t* data); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_REG_WRITE instruction packet with the data for writing on the Dynamixel register, and receives the packet + /// @description The function makes an instruction packet with INST_REG_WRITE and the data for writing on the Dynamixel register, + /// @description transmits and receives the packet with Protocol1PacketHandler::txRxPacket(), + /// @description gets the error from the packet. + /// @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamixel. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param length Length of the data for write + /// @param data Data for write + /// @param error Dynamixel hardware error + /// @return communication results which come from Protocol1PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int regWriteTxRx(PortHandler* port, uint8_t id, uint16_t address, uint16_t length, uint8_t* data, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief (Available only in Protocol 2.0) The function that transmits Sync Read instruction packet + /// @param port PortHandler instance + /// @param start_address Address of the data for Sync Read + /// @param data_length Length of the data for Sync Read + /// @param param Parameter for Sync Read + /// @param param_length Length of the data for Sync Read + /// @return COMM_NOT_AVAILABLE + //////////////////////////////////////////////////////////////////////////////// + int syncReadTx(PortHandler* port, uint16_t start_address, uint16_t data_length, uint8_t* param, uint16_t param_length); + // SyncReadRx -> GroupSyncRead class + // SyncReadTxRx -> GroupSyncRead class + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits Sync Write instruction packet + /// @description The function makes an instruction packet with INST_SYNC_WRITE, + /// @description transmits the packet with Protocol1PacketHandler::txRxPacket(). + /// @param port PortHandler instance + /// @param start_address Address of the data for Sync Write + /// @param data_length Length of the data for Sync Write + /// @param param Parameter for Sync Write {ID1, DATA0, DATA1, ..., DATAn, ID2, DATA0, DATA1, ..., DATAn, ID3, DATA0, DATA1, ..., DATAn} + /// @param param_length Length of the data for Sync Write + /// @return communication results which come from Protocol1PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int syncWriteTxOnly(PortHandler* port, uint16_t start_address, uint16_t data_length, uint8_t* param, uint16_t param_length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief (Available only on Dynamixel MX / X series) The function that transmits Bulk Read instruction packet + /// @description The function makes an instruction packet with INST_BULK_READ, + /// @description transmits the packet with Protocol1PacketHandler::txPacket(). + /// @param port PortHandler instance + /// @param param Parameter for Bulk Read {LEN1, ID1, ADDR1, LEN2, ID2, ADDR2, ...} + /// @param param_length Length of the data for Bulk Read + /// @return communication results which come from Protocol1PacketHandler::txPacket() + //////////////////////////////////////////////////////////////////////////////// + int bulkReadTx(PortHandler* port, uint8_t* param, uint16_t param_length); + // BulkReadRx -> GroupBulkRead class + // BulkReadTxRx -> GroupBulkRead class + + //////////////////////////////////////////////////////////////////////////////// + /// @brief (Available only in Protocol 2.0) The function that transmits Bulk Write instruction packet + /// @param port PortHandler instance + /// @param param Parameter for Bulk Write + /// @param param_length Length of the data for Bulk Write + /// @return COMM_NOT_AVAILABLE + //////////////////////////////////////////////////////////////////////////////// + int bulkWriteTxOnly(PortHandler* port, uint8_t* param, uint16_t param_length); }; -} - +} // namespace dynamixel #endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL1PACKETHANDLER_H_ */ diff --git a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/protocol2_packet_handler.h b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/protocol2_packet_handler.h index b8bd885..5a2a86c 100644 --- a/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/protocol2_packet_handler.h +++ b/DynamixelSDK/dynamixel_sdk/include/dynamixel_sdk/protocol2_packet_handler.h @@ -1,18 +1,18 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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. + *******************************************************************************/ //////////////////////////////////////////////////////////////////////////////// /// @file The file for Protocol 2.0 Dynamixel packet control @@ -22,533 +22,530 @@ #ifndef DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ #define DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ - #include "packet_handler.h" -namespace dynamixel -{ +namespace dynamixel { //////////////////////////////////////////////////////////////////////////////// /// @brief The class for control Dynamixel by using Protocol2.0 //////////////////////////////////////////////////////////////////////////////// -class WINDECLSPEC Protocol2PacketHandler : public PacketHandler -{ - private: - static Protocol2PacketHandler *unique_instance_; - - Protocol2PacketHandler(); - - uint16_t updateCRC(uint16_t crc_accum, uint8_t *data_blk_ptr, uint16_t data_blk_size); - void addStuffing(uint8_t *packet); - void removeStuffing(uint8_t *packet); - - public: - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that returns Protocol2PacketHandler instance - /// @return Protocol2PacketHandler instance - //////////////////////////////////////////////////////////////////////////////// - static Protocol2PacketHandler *getInstance() { return unique_instance_; } - - virtual ~Protocol2PacketHandler() { } - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that returns Protocol version used in Protocol2PacketHandler (2.0) - /// @return 2.0 - //////////////////////////////////////////////////////////////////////////////// - float getProtocolVersion() { return 2.0; } - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that gets description of communication result - /// @param result Communication result which might be gotten by the tx rx functions - /// @return description of communication result in const char* (string) - //////////////////////////////////////////////////////////////////////////////// - const char *getTxRxResult (int result); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that gets description of hardware error - /// @param error Dynamixel hardware error which might be gotten by the tx rx functions - /// @return description of hardware error in const char* (string) - //////////////////////////////////////////////////////////////////////////////// - const char *getRxPacketError (uint8_t error); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits the instruction packet txpacket via PortHandler port. - /// @description The function clears the port buffer by PortHandler::clearPort() function, - /// @description then transmits txpacket by PortHandler::writePort() function. - /// @description The function activates only when the port is not busy and when the packet is already written on the port buffer - /// @param port PortHandler instance - /// @param txpacket packet for transmission - /// @return COMM_PORT_BUSY - /// @return when the port is already in use - /// @return COMM_TX_ERROR - /// @return when txpacket is out of range described by TXPACKET_MAX_LEN - /// @return COMM_TX_FAIL - /// @return when written packet is shorter than expected - /// @return or COMM_SUCCESS - //////////////////////////////////////////////////////////////////////////////// - int txPacket (PortHandler *port, uint8_t *txpacket); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that receives packet (rxpacket) during designated time via PortHandler port - /// @description The function repeatedly tries to receive rxpacket by PortHandler::readPort() function. - /// @description It breaks out - /// @description when PortHandler::isPacketTimeout() shows the timeout, - /// @description when rxpacket seemed as corrupted, or - /// @description when nothing received - /// @param port PortHandler instance - /// @param rxpacket received packet - /// @return COMM_RX_CORRUPT - /// @return when it received the packet but it couldn't find header in the packet - /// @return when it found header in the packet but the id, length or error value is out of range - /// @return when it received the packet but it is shorted than expected - /// @return COMM_RX_TIMEOUT - /// @return when there is no rxpacket received until PortHandler::isPacketTimeout() shows the timeout - /// @return COMM_SUCCESS - /// @return when rxpacket passes checksum test - /// @return or COMM_RX_FAIL - //////////////////////////////////////////////////////////////////////////////// - int rxPacket (PortHandler *port, uint8_t *rxpacket); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits packet (txpacket) and receives packet (rxpacket) during designated time via PortHandler port - /// @description The function calls Protocol2PacketHandler::txPacket(), - /// @description and calls Protocol2PacketHandler::rxPacket() if it succeeds Protocol2PacketHandler::txPacket(). - /// @description It breaks out - /// @description when it fails Protocol2PacketHandler::txPacket(), - /// @description when txpacket is called by Protocol2PacketHandler::broadcastPing() / Protocol2PacketHandler::syncWriteTxOnly() / Protocol2PacketHandler::regWriteTxOnly / Protocol2PacketHandler::action - /// @param port PortHandler instance - /// @param txpacket packet for transmission - /// @param rxpacket received packet - /// @return COMM_SUCCESS - /// @return when it succeeds Protocol2PacketHandler::txPacket() and Protocol2PacketHandler::rxPacket() - /// @return or the other communication results which come from Protocol2PacketHandler::txPacket() and Protocol2PacketHandler::rxPacket() - //////////////////////////////////////////////////////////////////////////////// - int txRxPacket (PortHandler *port, uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that pings Dynamixel but doesn't take its model number - /// @description The function calls Protocol2PacketHandler::ping() which gets Dynamixel model number, - /// @description but doesn't carry the model number - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param error Dynamixel hardware error - /// @return communication results which come from Protocol2PacketHandler::ping() - //////////////////////////////////////////////////////////////////////////////// - int ping (PortHandler *port, uint8_t id, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that pings Dynamixel and takes its model number - /// @description The function makes an instruction packet with INST_PING, - /// @description transmits the packet with Protocol2PacketHandler::txRxPacket(), - /// @description and call Protocol2PacketHandler::readTxRx to read model_number in the rx buffer. - /// @description It breaks out - /// @description when it tries to transmit to BROADCAST_ID - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param error Dynamixel hardware error - /// @return COMM_NOT_AVAILABLE - /// @return when it tries to transmit to BROADCAST_ID - /// @return COMM_SUCCESS - /// @return when it succeeds to ping Dynamixel and get model_number from it - /// @return or the other communication results which come from Protocol2PacketHandler::txRxPacket() and Protocol2PacketHandler::readTxRx() - //////////////////////////////////////////////////////////////////////////////// - int ping (PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief (Available only in Protocol 2.0) The function that pings all connected Dynamixel - /// @param port PortHandler instance - /// @param id_list ID list of Dynamixels which are found by broadcast ping - /// @return COMM_NOT_AVAILABLE - //////////////////////////////////////////////////////////////////////////////// - int broadcastPing (PortHandler *port, std::vector &id_list); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that makes Dynamixels run as written in the Dynamixel register - /// @description The function makes an instruction packet with INST_ACTION, - /// @description transmits the packet with Protocol2PacketHandler::txRxPacket(). - /// @description To use this function, Dynamixel register should be set by Protocol2PacketHandler::regWriteTxOnly() or Protocol2PacketHandler::regWriteTxRx() - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @return communication results which come from Protocol2PacketHandler::txRxPacket() - //////////////////////////////////////////////////////////////////////////////// - int action (PortHandler *port, uint8_t id); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that makes Dynamixel reboot - /// @description The function makes an instruction packet with INST_REBOOT, - /// @description transmits the packet with Protocol2PacketHandler::txRxPacket(), - /// @description then Dynamixel reboots. - /// @description During reboot, its LED will blink. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param error Dynamixel hardware error - /// @return COMM_NOT_AVAILABLE - //////////////////////////////////////////////////////////////////////////////// - int reboot (PortHandler *port, uint8_t id, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that reset multi-turn revolution information of Dynamixel - /// @description The function makes an instruction packet with INST_CLEAR, - /// @description transmits the packet with Protocol2PacketHandler::txRxPacket(). - /// @description Applied Products : MX with Protocol 2.0 (Firmware v42 or above), - /// @description Dynamixel X-series (Firmware v42 or above). - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param error Dynamixel hardware error - /// @return communication results which come from Protocol2PacketHandler::txRxPacket() - //////////////////////////////////////////////////////////////////////////////// - int clearMultiTurn (PortHandler *port, uint8_t id, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that makes Dynamixel reset as it was produced in the factory - /// @description The function makes an instruction packet with INST_FACTORY_RESET, - /// @description transmits the packet with Protocol2PacketHandler::txRxPacket(). - /// @description Be careful of the use. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param option Reset option (0xFF for reset all values / 0x01 for reset all values except ID / 0x02 for reset all values except ID and Baudrate) - /// @param error Dynamixel hardware error - /// @return communication results which come from Protocol2PacketHandler::txRxPacket() - //////////////////////////////////////////////////////////////////////////////// - int factoryReset (PortHandler *port, uint8_t id, uint8_t option, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits INST_READ instruction packet - /// @description The function makes an instruction packet with INST_READ, - /// @description transmits the packet with Protocol2PacketHandler::txPacket(). - /// @description It breaks out - /// @description when it tries to transmit to BROADCAST_ID - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for read - /// @param length Length of the data for read - /// @return COMM_NOT_AVAILABLE - /// @return when it tries to transmit to BROADCAST_ID - /// @return or the other communication results which come from Protocol2PacketHandler::txPacket() - //////////////////////////////////////////////////////////////////////////////// - int readTx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that receives the packet and reads the data in the packet - /// @description The function receives the packet which might be come by previous INST_READ instruction packet transmission, - /// @description gets the data from the packet. - /// @param port PortHandler instance - /// @param length Length of the data for read - /// @param data Data extracted from the packet - /// @param error Dynamixel hardware error - /// @return communication results which come from Protocol2PacketHandler::rxPacket() - //////////////////////////////////////////////////////////////////////////////// - int readRx (PortHandler *port, uint8_t id, uint16_t length, uint8_t *data, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits INST_READ instruction packet, and read data from received packet - /// @description The function makes an instruction packet with INST_READ, - /// @description transmits and receives the packet with Protocol2PacketHandler::txRxPacket(), - /// @description gets the data from the packet. - /// @description It breaks out - /// @description when it tries to transmit to BROADCAST_ID - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for read - /// @param length Length of the data for read - /// @param data Data extracted from the packet - /// @param error Dynamixel hardware error - /// @return COMM_NOT_AVAILABLE - /// @return when it tries to transmit to BROADCAST_ID - /// @return or the other communication results which come from Protocol2PacketHandler::txRxPacket() - //////////////////////////////////////////////////////////////////////////////// - int readTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls Protocol2PacketHandler::readTx() function for reading 1 byte data - /// @description The function calls Protocol2PacketHandler::readTx() function for reading 1 byte data - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for read - /// @return communication results which come from Protocol2PacketHandler::readTx() - //////////////////////////////////////////////////////////////////////////////// - int read1ByteTx (PortHandler *port, uint8_t id, uint16_t address); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls Protocol2PacketHandler::readRx() function and reads 1 byte data on the packet - /// @description The function calls Protocol2PacketHandler::readRx() function, - /// @description gets 1 byte data from the packet. - /// @param port PortHandler instance - /// @param data Data extracted from the packet - /// @param error Dynamixel hardware error - /// @return communication results which come from Protocol2PacketHandler::readRx() - //////////////////////////////////////////////////////////////////////////////// - int read1ByteRx (PortHandler *port, uint8_t id, uint8_t *data, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls Protocol2PacketHandler::readTxRx() function for reading 1 byte data - /// @description The function calls Protocol2PacketHandler::readTxRx(), - /// @description gets 1 byte data from the packet. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for read - /// @param length Length of the data for read - /// @param data Data extracted from the packet - /// @param error Dynamixel hardware error - /// @return communication results which come from Protocol2PacketHandler::txRxPacket() - //////////////////////////////////////////////////////////////////////////////// - int read1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls Protocol2PacketHandler::readTx() function for reading 2 byte data - /// @description The function calls Protocol2PacketHandler::readTx() function for reading 2 byte data - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for read - /// @return communication results which come from Protocol2PacketHandler::readTx() - //////////////////////////////////////////////////////////////////////////////// - int read2ByteTx (PortHandler *port, uint8_t id, uint16_t address); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls Protocol2PacketHandler::readRx() function and reads 2 byte data on the packet - /// @description The function calls Protocol2PacketHandler::readRx() function, - /// @description gets 2 byte data from the packet. - /// @param port PortHandler instance - /// @param data Data extracted from the packet - /// @param error Dynamixel hardware error - /// @return communication results which come from Protocol2PacketHandler::readRx() - //////////////////////////////////////////////////////////////////////////////// - int read2ByteRx (PortHandler *port, uint8_t id, uint16_t *data, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls Protocol2PacketHandler::readTxRx() function for reading 2 byte data - /// @description The function calls Protocol2PacketHandler::readTxRx(), - /// @description gets 2 byte data from the packet. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for read - /// @param length Length of the data for read - /// @param data Data extracted from the packet - /// @param error Dynamixel hardware error - /// @return communication results which come from Protocol2PacketHandler::txRxPacket() - //////////////////////////////////////////////////////////////////////////////// - int read2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls Protocol2PacketHandler::readTx() function for reading 4 byte data - /// @description The function calls Protocol2PacketHandler::readTx() function for reading 4 byte data - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for read - /// @return communication results which come from Protocol2PacketHandler::readTx() - //////////////////////////////////////////////////////////////////////////////// - int read4ByteTx (PortHandler *port, uint8_t id, uint16_t address); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls Protocol2PacketHandler::readRx() function and reads 4 byte data on the packet - /// @description The function calls Protocol2PacketHandler::readRx() function, - /// @description gets 4 byte data from the packet. - /// @param port PortHandler instance - /// @param data Data extracted from the packet - /// @param error Dynamixel hardware error - /// @return communication results which come from Protocol2PacketHandler::readRx() - //////////////////////////////////////////////////////////////////////////////// - int read4ByteRx (PortHandler *port, uint8_t id, uint32_t *data, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls Protocol2PacketHandler::readTxRx() function for reading 4 byte data - /// @description The function calls Protocol2PacketHandler::readTxRx(), - /// @description gets 4 byte data from the packet. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for read - /// @param length Length of the data for read - /// @param data Data extracted from the packet - /// @param error Dynamixel hardware error - /// @return communication results which come from Protocol2PacketHandler::txRxPacket() - //////////////////////////////////////////////////////////////////////////////// - int read4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits INST_WRITE instruction packet with the data for write - /// @description The function makes an instruction packet with INST_WRITE and the data for write, - /// @description transmits the packet with Protocol2PacketHandler::txPacket(). - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for write - /// @param length Length of the data for write - /// @param data Data for write - /// @return communication results which come from Protocol2PacketHandler::txPacket() - //////////////////////////////////////////////////////////////////////////////// - int writeTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits INST_WRITE instruction packet with the data for write, and receives the packet - /// @description The function makes an instruction packet with INST_WRITE and the data for write, - /// @description transmits and receives the packet with Protocol2PacketHandler::txRxPacket(), - /// @description gets the error from the packet. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for write - /// @param length Length of the data for write - /// @param data Data for write - /// @param error Dynamixel hardware error - /// @return communication results which come from Protocol2PacketHandler::txRxPacket() - //////////////////////////////////////////////////////////////////////////////// - int writeTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls Protocol2PacketHandler::writeTxOnly() for writing 1 byte data - /// @description The function calls Protocol2PacketHandler::writeTxOnly() for writing 1 byte data. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for write - /// @param data Data for write - /// @return communication results which come from Protocol2PacketHandler::writeTxOnly() - //////////////////////////////////////////////////////////////////////////////// - int write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls Protocol2PacketHandler::writeTxRx() for writing 1 byte data and receives the packet - /// @description The function calls Protocol2PacketHandler::writeTxRx() for writing 1 byte data and receves the packet, - /// @description gets the error from the packet. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for write - /// @param data Data for write - /// @param error Dynamixel hardware error - /// @return communication results which come from Protocol2PacketHandler::writeTxRx() - //////////////////////////////////////////////////////////////////////////////// - int write1ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls Protocol2PacketHandler::writeTxOnly() for writing 2 byte data - /// @description The function calls Protocol2PacketHandler::writeTxOnly() for writing 2 byte data. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for write - /// @param data Data for write - /// @return communication results which come from Protocol2PacketHandler::writeTxOnly() - //////////////////////////////////////////////////////////////////////////////// - int write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls Protocol2PacketHandler::writeTxRx() for writing 2 byte data and receives the packet - /// @description The function calls Protocol2PacketHandler::writeTxRx() for writing 2 byte data and receves the packet, - /// @description gets the error from the packet. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for write - /// @param data Data for write - /// @param error Dynamixel hardware error - /// @return communication results which come from Protocol2PacketHandler::writeTxRx() - //////////////////////////////////////////////////////////////////////////////// - int write2ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls Protocol2PacketHandler::writeTxOnly() for writing 4 byte data - /// @description The function calls Protocol2PacketHandler::writeTxOnly() for writing 4 byte data. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for write - /// @param data Data for write - /// @return communication results which come from Protocol2PacketHandler::writeTxOnly() - //////////////////////////////////////////////////////////////////////////////// - int write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that calls Protocol2PacketHandler::writeTxRx() for writing 4 byte data and receives the packet - /// @description The function calls Protocol2PacketHandler::writeTxRx() for writing 4 byte data and receves the packet, - /// @description gets the error from the packet. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for write - /// @param data Data for write - /// @param error Dynamixel hardware error - /// @return communication results which come from Protocol2PacketHandler::writeTxRx() - //////////////////////////////////////////////////////////////////////////////// - int write4ByteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error = 0); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits INST_REG_WRITE instruction packet with the data for writing on the Dynamixel register - /// @description The function makes an instruction packet with INST_REG_WRITE and the data for writing on the Dynamixel register, - /// @description transmits the packet with Protocol2PacketHandler::txPacket(). - /// @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamixel. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for write - /// @param length Length of the data for write - /// @param data Data for write - /// @return communication results which come from Protocol2PacketHandler::txPacket() - //////////////////////////////////////////////////////////////////////////////// - int regWriteTxOnly (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits INST_REG_WRITE instruction packet with the data for writing on the Dynamixel register, and receives the packet - /// @description The function makes an instruction packet with INST_REG_WRITE and the data for writing on the Dynamixel register, - /// @description transmits and receives the packet with Protocol2PacketHandler::txRxPacket(), - /// @description gets the error from the packet. - /// @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamixel. - /// @param port PortHandler instance - /// @param id Dynamixel ID - /// @param address Address of the data for write - /// @param length Length of the data for write - /// @param data Data for write - /// @param error Dynamixel hardware error - /// @return communication results which come from Protocol2PacketHandler::txRxPacket() - //////////////////////////////////////////////////////////////////////////////// - int regWriteTxRx (PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error = 0); - - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits INST_SYNC_READ instruction packet - /// @description The function makes an instruction packet with INST_SYNC_READ, - /// @description transmits the packet with Protocol2PacketHandler::txPacket(). - /// @param port PortHandler instance - /// @param start_address Address of the data for Sync Read - /// @param data_length Length of the data for Sync Read - /// @param param Parameter for Sync Read - /// @param param_length Length of the data for Sync Read - /// @return communication results which come from Protocol2PacketHandler::txPacket() - //////////////////////////////////////////////////////////////////////////////// - int syncReadTx (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length); - // SyncReadRx -> GroupSyncRead class - // SyncReadTxRx -> GroupSyncRead class - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits INST_SYNC_WRITE instruction packet - /// @description The function makes an instruction packet with INST_SYNC_WRITE, - /// @description transmits the packet with Protocol2PacketHandler::txRxPacket(). - /// @param port PortHandler instance - /// @param start_address Address of the data for Sync Write - /// @param data_length Length of the data for Sync Write - /// @param param Parameter for Sync Write {ID1, DATA0, DATA1, ..., DATAn, ID2, DATA0, DATA1, ..., DATAn, ID3, DATA0, DATA1, ..., DATAn} - /// @param param_length Length of the data for Sync Write - /// @return communication results which come from Protocol2PacketHandler::txRxPacket() - //////////////////////////////////////////////////////////////////////////////// - int syncWriteTxOnly (PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length); - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits INST_BULK_READ instruction packet - /// @description The function makes an instruction packet with INST_BULK_READ, - /// @description transmits the packet with Protocol2PacketHandler::txPacket(). - /// @param port PortHandler instance - /// @param param Parameter for Bulk Read {ID1, ADDR_L1, ADDR_H1, LEN_L1, LEN_H1, ID2, ADDR_L2, ADDR_H2, LEN_L2, LEN_H2, ...} - /// @param param_length Length of the data for Bulk Read - /// @return communication results which come from Protocol2PacketHandler::txPacket() - //////////////////////////////////////////////////////////////////////////////// - int bulkReadTx (PortHandler *port, uint8_t *param, uint16_t param_length); - // BulkReadRx -> GroupBulkRead class - // BulkReadTxRx -> GroupBulkRead class - - //////////////////////////////////////////////////////////////////////////////// - /// @brief The function that transmits INST_BULK_WRITE instruction packet - /// @description The function makes an instruction packet with INST_BULK_WRITE, - /// @description transmits the packet with Protocol2PacketHandler::txRxPacket(). - /// @param port PortHandler instance - /// @param param Parameter for Bulk Write {ID1, START_ADDR_L, START_ADDR_H, DATA_LEN_L, DATA_LEN_H, DATA0, DATA1, ..., DATAn, ID2, START_ADDR_L, START_ADDR_H, DATA_LEN_L, DATA_LEN_H, DATA0, DATA1, ..., DATAn} - /// @param param_length Length of the data for Bulk Write - /// @return communication results which come from Protocol2PacketHandler::txRxPacket() - //////////////////////////////////////////////////////////////////////////////// - int bulkWriteTxOnly (PortHandler *port, uint8_t *param, uint16_t param_length); +class WINDECLSPEC Protocol2PacketHandler : public PacketHandler { +private: + static Protocol2PacketHandler* unique_instance_; + + Protocol2PacketHandler(); + + uint16_t updateCRC(uint16_t crc_accum, uint8_t* data_blk_ptr, uint16_t data_blk_size); + void addStuffing(uint8_t* packet); + void removeStuffing(uint8_t* packet); + +public: + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns Protocol2PacketHandler instance + /// @return Protocol2PacketHandler instance + //////////////////////////////////////////////////////////////////////////////// + static Protocol2PacketHandler* getInstance() { return unique_instance_; } + + virtual ~Protocol2PacketHandler() {} + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that returns Protocol version used in Protocol2PacketHandler (2.0) + /// @return 2.0 + //////////////////////////////////////////////////////////////////////////////// + float getProtocolVersion() { return 2.0; } + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that gets description of communication result + /// @param result Communication result which might be gotten by the tx rx functions + /// @return description of communication result in const char* (string) + //////////////////////////////////////////////////////////////////////////////// + const char* getTxRxResult(int result); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that gets description of hardware error + /// @param error Dynamixel hardware error which might be gotten by the tx rx functions + /// @return description of hardware error in const char* (string) + //////////////////////////////////////////////////////////////////////////////// + const char* getRxPacketError(uint8_t error); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits the instruction packet txpacket via PortHandler port. + /// @description The function clears the port buffer by PortHandler::clearPort() function, + /// @description then transmits txpacket by PortHandler::writePort() function. + /// @description The function activates only when the port is not busy and when the packet is already written on the port buffer + /// @param port PortHandler instance + /// @param txpacket packet for transmission + /// @return COMM_PORT_BUSY + /// @return when the port is already in use + /// @return COMM_TX_ERROR + /// @return when txpacket is out of range described by TXPACKET_MAX_LEN + /// @return COMM_TX_FAIL + /// @return when written packet is shorter than expected + /// @return or COMM_SUCCESS + //////////////////////////////////////////////////////////////////////////////// + int txPacket(PortHandler* port, uint8_t* txpacket); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that receives packet (rxpacket) during designated time via PortHandler port + /// @description The function repeatedly tries to receive rxpacket by PortHandler::readPort() function. + /// @description It breaks out + /// @description when PortHandler::isPacketTimeout() shows the timeout, + /// @description when rxpacket seemed as corrupted, or + /// @description when nothing received + /// @param port PortHandler instance + /// @param rxpacket received packet + /// @return COMM_RX_CORRUPT + /// @return when it received the packet but it couldn't find header in the packet + /// @return when it found header in the packet but the id, length or error value is out of range + /// @return when it received the packet but it is shorted than expected + /// @return COMM_RX_TIMEOUT + /// @return when there is no rxpacket received until PortHandler::isPacketTimeout() shows the timeout + /// @return COMM_SUCCESS + /// @return when rxpacket passes checksum test + /// @return or COMM_RX_FAIL + //////////////////////////////////////////////////////////////////////////////// + int rxPacket(PortHandler* port, uint8_t* rxpacket); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits packet (txpacket) and receives packet (rxpacket) during designated time via PortHandler port + /// @description The function calls Protocol2PacketHandler::txPacket(), + /// @description and calls Protocol2PacketHandler::rxPacket() if it succeeds Protocol2PacketHandler::txPacket(). + /// @description It breaks out + /// @description when it fails Protocol2PacketHandler::txPacket(), + /// @description when txpacket is called by Protocol2PacketHandler::broadcastPing() / Protocol2PacketHandler::syncWriteTxOnly() / Protocol2PacketHandler::regWriteTxOnly / + /// Protocol2PacketHandler::action + /// @param port PortHandler instance + /// @param txpacket packet for transmission + /// @param rxpacket received packet + /// @return COMM_SUCCESS + /// @return when it succeeds Protocol2PacketHandler::txPacket() and Protocol2PacketHandler::rxPacket() + /// @return or the other communication results which come from Protocol2PacketHandler::txPacket() and Protocol2PacketHandler::rxPacket() + //////////////////////////////////////////////////////////////////////////////// + int txRxPacket(PortHandler* port, uint8_t* txpacket, uint8_t* rxpacket, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that pings Dynamixel but doesn't take its model number + /// @description The function calls Protocol2PacketHandler::ping() which gets Dynamixel model number, + /// @description but doesn't carry the model number + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param error Dynamixel hardware error + /// @return communication results which come from Protocol2PacketHandler::ping() + //////////////////////////////////////////////////////////////////////////////// + int ping(PortHandler* port, uint8_t id, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that pings Dynamixel and takes its model number + /// @description The function makes an instruction packet with INST_PING, + /// @description transmits the packet with Protocol2PacketHandler::txRxPacket(), + /// @description and call Protocol2PacketHandler::readTxRx to read model_number in the rx buffer. + /// @description It breaks out + /// @description when it tries to transmit to BROADCAST_ID + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param error Dynamixel hardware error + /// @return COMM_NOT_AVAILABLE + /// @return when it tries to transmit to BROADCAST_ID + /// @return COMM_SUCCESS + /// @return when it succeeds to ping Dynamixel and get model_number from it + /// @return or the other communication results which come from Protocol2PacketHandler::txRxPacket() and Protocol2PacketHandler::readTxRx() + //////////////////////////////////////////////////////////////////////////////// + int ping(PortHandler* port, uint8_t id, uint16_t* model_number, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief (Available only in Protocol 2.0) The function that pings all connected Dynamixel + /// @param port PortHandler instance + /// @param id_list ID list of Dynamixels which are found by broadcast ping + /// @return COMM_NOT_AVAILABLE + //////////////////////////////////////////////////////////////////////////////// + int broadcastPing(PortHandler* port, std::vector& id_list); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that makes Dynamixels run as written in the Dynamixel register + /// @description The function makes an instruction packet with INST_ACTION, + /// @description transmits the packet with Protocol2PacketHandler::txRxPacket(). + /// @description To use this function, Dynamixel register should be set by Protocol2PacketHandler::regWriteTxOnly() or Protocol2PacketHandler::regWriteTxRx() + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @return communication results which come from Protocol2PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int action(PortHandler* port, uint8_t id); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that makes Dynamixel reboot + /// @description The function makes an instruction packet with INST_REBOOT, + /// @description transmits the packet with Protocol2PacketHandler::txRxPacket(), + /// @description then Dynamixel reboots. + /// @description During reboot, its LED will blink. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param error Dynamixel hardware error + /// @return COMM_NOT_AVAILABLE + //////////////////////////////////////////////////////////////////////////////// + int reboot(PortHandler* port, uint8_t id, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that reset multi-turn revolution information of Dynamixel + /// @description The function makes an instruction packet with INST_CLEAR, + /// @description transmits the packet with Protocol2PacketHandler::txRxPacket(). + /// @description Applied Products : MX with Protocol 2.0 (Firmware v42 or above), + /// @description Dynamixel X-series (Firmware v42 or above). + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param error Dynamixel hardware error + /// @return communication results which come from Protocol2PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int clearMultiTurn(PortHandler* port, uint8_t id, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that makes Dynamixel reset as it was produced in the factory + /// @description The function makes an instruction packet with INST_FACTORY_RESET, + /// @description transmits the packet with Protocol2PacketHandler::txRxPacket(). + /// @description Be careful of the use. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param option Reset option (0xFF for reset all values / 0x01 for reset all values except ID / 0x02 for reset all values except ID and Baudrate) + /// @param error Dynamixel hardware error + /// @return communication results which come from Protocol2PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int factoryReset(PortHandler* port, uint8_t id, uint8_t option, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_READ instruction packet + /// @description The function makes an instruction packet with INST_READ, + /// @description transmits the packet with Protocol2PacketHandler::txPacket(). + /// @description It breaks out + /// @description when it tries to transmit to BROADCAST_ID + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @param length Length of the data for read + /// @return COMM_NOT_AVAILABLE + /// @return when it tries to transmit to BROADCAST_ID + /// @return or the other communication results which come from Protocol2PacketHandler::txPacket() + //////////////////////////////////////////////////////////////////////////////// + int readTx(PortHandler* port, uint8_t id, uint16_t address, uint16_t length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that receives the packet and reads the data in the packet + /// @description The function receives the packet which might be come by previous INST_READ instruction packet transmission, + /// @description gets the data from the packet. + /// @param port PortHandler instance + /// @param length Length of the data for read + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from Protocol2PacketHandler::rxPacket() + //////////////////////////////////////////////////////////////////////////////// + int readRx(PortHandler* port, uint8_t id, uint16_t length, uint8_t* data, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_READ instruction packet, and read data from received packet + /// @description The function makes an instruction packet with INST_READ, + /// @description transmits and receives the packet with Protocol2PacketHandler::txRxPacket(), + /// @description gets the data from the packet. + /// @description It breaks out + /// @description when it tries to transmit to BROADCAST_ID + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @param length Length of the data for read + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return COMM_NOT_AVAILABLE + /// @return when it tries to transmit to BROADCAST_ID + /// @return or the other communication results which come from Protocol2PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int readTxRx(PortHandler* port, uint8_t id, uint16_t address, uint16_t length, uint8_t* data, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol2PacketHandler::readTx() function for reading 1 byte data + /// @description The function calls Protocol2PacketHandler::readTx() function for reading 1 byte data + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @return communication results which come from Protocol2PacketHandler::readTx() + //////////////////////////////////////////////////////////////////////////////// + int read1ByteTx(PortHandler* port, uint8_t id, uint16_t address); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol2PacketHandler::readRx() function and reads 1 byte data on the packet + /// @description The function calls Protocol2PacketHandler::readRx() function, + /// @description gets 1 byte data from the packet. + /// @param port PortHandler instance + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from Protocol2PacketHandler::readRx() + //////////////////////////////////////////////////////////////////////////////// + int read1ByteRx(PortHandler* port, uint8_t id, uint8_t* data, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol2PacketHandler::readTxRx() function for reading 1 byte data + /// @description The function calls Protocol2PacketHandler::readTxRx(), + /// @description gets 1 byte data from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @param length Length of the data for read + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from Protocol2PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int read1ByteTxRx(PortHandler* port, uint8_t id, uint16_t address, uint8_t* data, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol2PacketHandler::readTx() function for reading 2 byte data + /// @description The function calls Protocol2PacketHandler::readTx() function for reading 2 byte data + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @return communication results which come from Protocol2PacketHandler::readTx() + //////////////////////////////////////////////////////////////////////////////// + int read2ByteTx(PortHandler* port, uint8_t id, uint16_t address); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol2PacketHandler::readRx() function and reads 2 byte data on the packet + /// @description The function calls Protocol2PacketHandler::readRx() function, + /// @description gets 2 byte data from the packet. + /// @param port PortHandler instance + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from Protocol2PacketHandler::readRx() + //////////////////////////////////////////////////////////////////////////////// + int read2ByteRx(PortHandler* port, uint8_t id, uint16_t* data, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol2PacketHandler::readTxRx() function for reading 2 byte data + /// @description The function calls Protocol2PacketHandler::readTxRx(), + /// @description gets 2 byte data from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @param length Length of the data for read + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from Protocol2PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int read2ByteTxRx(PortHandler* port, uint8_t id, uint16_t address, uint16_t* data, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol2PacketHandler::readTx() function for reading 4 byte data + /// @description The function calls Protocol2PacketHandler::readTx() function for reading 4 byte data + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @return communication results which come from Protocol2PacketHandler::readTx() + //////////////////////////////////////////////////////////////////////////////// + int read4ByteTx(PortHandler* port, uint8_t id, uint16_t address); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol2PacketHandler::readRx() function and reads 4 byte data on the packet + /// @description The function calls Protocol2PacketHandler::readRx() function, + /// @description gets 4 byte data from the packet. + /// @param port PortHandler instance + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from Protocol2PacketHandler::readRx() + //////////////////////////////////////////////////////////////////////////////// + int read4ByteRx(PortHandler* port, uint8_t id, uint32_t* data, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol2PacketHandler::readTxRx() function for reading 4 byte data + /// @description The function calls Protocol2PacketHandler::readTxRx(), + /// @description gets 4 byte data from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for read + /// @param length Length of the data for read + /// @param data Data extracted from the packet + /// @param error Dynamixel hardware error + /// @return communication results which come from Protocol2PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int read4ByteTxRx(PortHandler* port, uint8_t id, uint16_t address, uint32_t* data, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_WRITE instruction packet with the data for write + /// @description The function makes an instruction packet with INST_WRITE and the data for write, + /// @description transmits the packet with Protocol2PacketHandler::txPacket(). + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param length Length of the data for write + /// @param data Data for write + /// @return communication results which come from Protocol2PacketHandler::txPacket() + //////////////////////////////////////////////////////////////////////////////// + int writeTxOnly(PortHandler* port, uint8_t id, uint16_t address, uint16_t length, uint8_t* data); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_WRITE instruction packet with the data for write, and receives the packet + /// @description The function makes an instruction packet with INST_WRITE and the data for write, + /// @description transmits and receives the packet with Protocol2PacketHandler::txRxPacket(), + /// @description gets the error from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param length Length of the data for write + /// @param data Data for write + /// @param error Dynamixel hardware error + /// @return communication results which come from Protocol2PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int writeTxRx(PortHandler* port, uint8_t id, uint16_t address, uint16_t length, uint8_t* data, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol2PacketHandler::writeTxOnly() for writing 1 byte data + /// @description The function calls Protocol2PacketHandler::writeTxOnly() for writing 1 byte data. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param data Data for write + /// @return communication results which come from Protocol2PacketHandler::writeTxOnly() + //////////////////////////////////////////////////////////////////////////////// + int write1ByteTxOnly(PortHandler* port, uint8_t id, uint16_t address, uint8_t data); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol2PacketHandler::writeTxRx() for writing 1 byte data and receives the packet + /// @description The function calls Protocol2PacketHandler::writeTxRx() for writing 1 byte data and receves the packet, + /// @description gets the error from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param data Data for write + /// @param error Dynamixel hardware error + /// @return communication results which come from Protocol2PacketHandler::writeTxRx() + //////////////////////////////////////////////////////////////////////////////// + int write1ByteTxRx(PortHandler* port, uint8_t id, uint16_t address, uint8_t data, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol2PacketHandler::writeTxOnly() for writing 2 byte data + /// @description The function calls Protocol2PacketHandler::writeTxOnly() for writing 2 byte data. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param data Data for write + /// @return communication results which come from Protocol2PacketHandler::writeTxOnly() + //////////////////////////////////////////////////////////////////////////////// + int write2ByteTxOnly(PortHandler* port, uint8_t id, uint16_t address, uint16_t data); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol2PacketHandler::writeTxRx() for writing 2 byte data and receives the packet + /// @description The function calls Protocol2PacketHandler::writeTxRx() for writing 2 byte data and receves the packet, + /// @description gets the error from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param data Data for write + /// @param error Dynamixel hardware error + /// @return communication results which come from Protocol2PacketHandler::writeTxRx() + //////////////////////////////////////////////////////////////////////////////// + int write2ByteTxRx(PortHandler* port, uint8_t id, uint16_t address, uint16_t data, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol2PacketHandler::writeTxOnly() for writing 4 byte data + /// @description The function calls Protocol2PacketHandler::writeTxOnly() for writing 4 byte data. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param data Data for write + /// @return communication results which come from Protocol2PacketHandler::writeTxOnly() + //////////////////////////////////////////////////////////////////////////////// + int write4ByteTxOnly(PortHandler* port, uint8_t id, uint16_t address, uint32_t data); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that calls Protocol2PacketHandler::writeTxRx() for writing 4 byte data and receives the packet + /// @description The function calls Protocol2PacketHandler::writeTxRx() for writing 4 byte data and receves the packet, + /// @description gets the error from the packet. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param data Data for write + /// @param error Dynamixel hardware error + /// @return communication results which come from Protocol2PacketHandler::writeTxRx() + //////////////////////////////////////////////////////////////////////////////// + int write4ByteTxRx(PortHandler* port, uint8_t id, uint16_t address, uint32_t data, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_REG_WRITE instruction packet with the data for writing on the Dynamixel register + /// @description The function makes an instruction packet with INST_REG_WRITE and the data for writing on the Dynamixel register, + /// @description transmits the packet with Protocol2PacketHandler::txPacket(). + /// @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamixel. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param length Length of the data for write + /// @param data Data for write + /// @return communication results which come from Protocol2PacketHandler::txPacket() + //////////////////////////////////////////////////////////////////////////////// + int regWriteTxOnly(PortHandler* port, uint8_t id, uint16_t address, uint16_t length, uint8_t* data); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_REG_WRITE instruction packet with the data for writing on the Dynamixel register, and receives the packet + /// @description The function makes an instruction packet with INST_REG_WRITE and the data for writing on the Dynamixel register, + /// @description transmits and receives the packet with Protocol2PacketHandler::txRxPacket(), + /// @description gets the error from the packet. + /// @description The data written in the register will act when INST_ACTION instruction packet is transmitted to the Dynamixel. + /// @param port PortHandler instance + /// @param id Dynamixel ID + /// @param address Address of the data for write + /// @param length Length of the data for write + /// @param data Data for write + /// @param error Dynamixel hardware error + /// @return communication results which come from Protocol2PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int regWriteTxRx(PortHandler* port, uint8_t id, uint16_t address, uint16_t length, uint8_t* data, uint8_t* error = 0); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_SYNC_READ instruction packet + /// @description The function makes an instruction packet with INST_SYNC_READ, + /// @description transmits the packet with Protocol2PacketHandler::txPacket(). + /// @param port PortHandler instance + /// @param start_address Address of the data for Sync Read + /// @param data_length Length of the data for Sync Read + /// @param param Parameter for Sync Read + /// @param param_length Length of the data for Sync Read + /// @return communication results which come from Protocol2PacketHandler::txPacket() + //////////////////////////////////////////////////////////////////////////////// + int syncReadTx(PortHandler* port, uint16_t start_address, uint16_t data_length, uint8_t* param, uint16_t param_length); + // SyncReadRx -> GroupSyncRead class + // SyncReadTxRx -> GroupSyncRead class + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_SYNC_WRITE instruction packet + /// @description The function makes an instruction packet with INST_SYNC_WRITE, + /// @description transmits the packet with Protocol2PacketHandler::txRxPacket(). + /// @param port PortHandler instance + /// @param start_address Address of the data for Sync Write + /// @param data_length Length of the data for Sync Write + /// @param param Parameter for Sync Write {ID1, DATA0, DATA1, ..., DATAn, ID2, DATA0, DATA1, ..., DATAn, ID3, DATA0, DATA1, ..., DATAn} + /// @param param_length Length of the data for Sync Write + /// @return communication results which come from Protocol2PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int syncWriteTxOnly(PortHandler* port, uint16_t start_address, uint16_t data_length, uint8_t* param, uint16_t param_length); + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_BULK_READ instruction packet + /// @description The function makes an instruction packet with INST_BULK_READ, + /// @description transmits the packet with Protocol2PacketHandler::txPacket(). + /// @param port PortHandler instance + /// @param param Parameter for Bulk Read {ID1, ADDR_L1, ADDR_H1, LEN_L1, LEN_H1, ID2, ADDR_L2, ADDR_H2, LEN_L2, LEN_H2, ...} + /// @param param_length Length of the data for Bulk Read + /// @return communication results which come from Protocol2PacketHandler::txPacket() + //////////////////////////////////////////////////////////////////////////////// + int bulkReadTx(PortHandler* port, uint8_t* param, uint16_t param_length); + // BulkReadRx -> GroupBulkRead class + // BulkReadTxRx -> GroupBulkRead class + + //////////////////////////////////////////////////////////////////////////////// + /// @brief The function that transmits INST_BULK_WRITE instruction packet + /// @description The function makes an instruction packet with INST_BULK_WRITE, + /// @description transmits the packet with Protocol2PacketHandler::txRxPacket(). + /// @param port PortHandler instance + /// @param param Parameter for Bulk Write {ID1, START_ADDR_L, START_ADDR_H, DATA_LEN_L, DATA_LEN_H, DATA0, DATA1, ..., DATAn, ID2, START_ADDR_L, START_ADDR_H, DATA_LEN_L, DATA_LEN_H, DATA0, DATA1, + /// ..., DATAn} + /// @param param_length Length of the data for Bulk Write + /// @return communication results which come from Protocol2PacketHandler::txRxPacket() + //////////////////////////////////////////////////////////////////////////////// + int bulkWriteTxOnly(PortHandler* port, uint8_t* param, uint16_t param_length); }; -} - +} // namespace dynamixel #endif /* DYNAMIXEL_SDK_INCLUDE_DYNAMIXEL_SDK_PROTOCOL2PACKETHANDLER_H_ */ diff --git a/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp b/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp index 5f9d913..8e32f34 100644 --- a/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp +++ b/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/group_bulk_read.cpp @@ -1,23 +1,23 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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: zerom, Ryu Woon Jung (Leon) */ -#include #include +#include #if defined(__linux__) #include "group_bulk_read.h" @@ -32,205 +32,190 @@ using namespace dynamixel; -GroupBulkRead::GroupBulkRead(PortHandler *port, PacketHandler *ph) - : port_(port), - ph_(ph), - last_result_(false), - is_param_changed_(false), - param_(0) +GroupBulkRead::GroupBulkRead(PortHandler* port, PacketHandler* ph) : port_(port), ph_(ph), last_result_(false), is_param_changed_(false), param_(0) { - clearParam(); + clearParam(); } void GroupBulkRead::makeParam() { - if (id_list_.size() == 0) - return; - - if (param_ != 0) - delete[] param_; - param_ = 0; - - if (ph_->getProtocolVersion() == 1.0) - { - param_ = new uint8_t[id_list_.size() * 3]; // ID(1) + ADDR(1) + LENGTH(1) - } - else // 2.0 - { - param_ = new uint8_t[id_list_.size() * 5]; // ID(1) + ADDR(2) + LENGTH(2) - } - - int idx = 0; - for (unsigned int i = 0; i < id_list_.size(); i++) - { - uint8_t id = id_list_[i]; - if (ph_->getProtocolVersion() == 1.0) - { - param_[idx++] = (uint8_t)length_list_[id]; // LEN - param_[idx++] = id; // ID - param_[idx++] = (uint8_t)address_list_[id]; // ADDR + if (id_list_.size() == 0) + return; + + if (param_ != 0) + delete[] param_; + param_ = 0; + + if (ph_->getProtocolVersion() == 1.0) { + param_ = new uint8_t[id_list_.size() * 3]; // ID(1) + ADDR(1) + LENGTH(1) } - else // 2.0 + else // 2.0 { - param_[idx++] = id; // ID - param_[idx++] = DXL_LOBYTE(address_list_[id]); // ADDR_L - param_[idx++] = DXL_HIBYTE(address_list_[id]); // ADDR_H - param_[idx++] = DXL_LOBYTE(length_list_[id]); // LEN_L - param_[idx++] = DXL_HIBYTE(length_list_[id]); // LEN_H + param_ = new uint8_t[id_list_.size() * 5]; // ID(1) + ADDR(2) + LENGTH(2) + } + + int idx = 0; + for (unsigned int i = 0; i < id_list_.size(); i++) { + uint8_t id = id_list_[i]; + if (ph_->getProtocolVersion() == 1.0) { + param_[idx++] = (uint8_t)length_list_[id]; // LEN + param_[idx++] = id; // ID + param_[idx++] = (uint8_t)address_list_[id]; // ADDR + } + else // 2.0 + { + param_[idx++] = id; // ID + param_[idx++] = DXL_LOBYTE(address_list_[id]); // ADDR_L + param_[idx++] = DXL_HIBYTE(address_list_[id]); // ADDR_H + param_[idx++] = DXL_LOBYTE(length_list_[id]); // LEN_L + param_[idx++] = DXL_HIBYTE(length_list_[id]); // LEN_H + } } - } } bool GroupBulkRead::addParam(uint8_t id, uint16_t start_address, uint16_t data_length) { - if (std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist - return false; + if (std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist + return false; - id_list_.push_back(id); - length_list_[id] = data_length; - address_list_[id] = start_address; - data_list_[id] = new uint8_t[data_length]; - error_list_[id] = new uint8_t[1]; + id_list_.push_back(id); + length_list_[id] = data_length; + address_list_[id] = start_address; + data_list_[id] = new uint8_t[data_length]; + error_list_[id] = new uint8_t[1]; - is_param_changed_ = true; - return true; + is_param_changed_ = true; + return true; } void GroupBulkRead::removeParam(uint8_t id) { - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if (it == id_list_.end()) // NOT exist - return; - - id_list_.erase(it); - address_list_.erase(id); - length_list_.erase(id); - delete[] data_list_[id]; - delete[] error_list_[id]; - data_list_.erase(id); - error_list_.erase(id); - - is_param_changed_ = true; + std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); + if (it == id_list_.end()) // NOT exist + return; + + id_list_.erase(it); + address_list_.erase(id); + length_list_.erase(id); + delete[] data_list_[id]; + delete[] error_list_[id]; + data_list_.erase(id); + error_list_.erase(id); + + is_param_changed_ = true; } void GroupBulkRead::clearParam() { - if (id_list_.size() == 0) - return; - - for (unsigned int i = 0; i < id_list_.size(); i++) - { - delete[] data_list_[id_list_[i]]; - delete[] error_list_[id_list_[i]]; - } - - id_list_.clear(); - address_list_.clear(); - length_list_.clear(); - data_list_.clear(); - error_list_.clear(); - if (param_ != 0) - delete[] param_; - param_ = 0; + if (id_list_.size() == 0) + return; + + for (unsigned int i = 0; i < id_list_.size(); i++) { + delete[] data_list_[id_list_[i]]; + delete[] error_list_[id_list_[i]]; + } + + id_list_.clear(); + address_list_.clear(); + length_list_.clear(); + data_list_.clear(); + error_list_.clear(); + if (param_ != 0) + delete[] param_; + param_ = 0; } int GroupBulkRead::txPacket() { - if (id_list_.size() == 0) - return COMM_NOT_AVAILABLE; - - if (is_param_changed_ == true || param_ == 0) - makeParam(); - - if (ph_->getProtocolVersion() == 1.0) - { - return ph_->bulkReadTx(port_, param_, id_list_.size() * 3); - } - else // 2.0 - { - return ph_->bulkReadTx(port_, param_, id_list_.size() * 5); - } + if (id_list_.size() == 0) + return COMM_NOT_AVAILABLE; + + if (is_param_changed_ == true || param_ == 0) + makeParam(); + + if (ph_->getProtocolVersion() == 1.0) { + return ph_->bulkReadTx(port_, param_, id_list_.size() * 3); + } + else // 2.0 + { + return ph_->bulkReadTx(port_, param_, id_list_.size() * 5); + } } int GroupBulkRead::rxPacket() { - int cnt = id_list_.size(); - int result = COMM_RX_FAIL; + int cnt = id_list_.size(); + int result = COMM_RX_FAIL; - last_result_ = false; + last_result_ = false; - if (cnt == 0) - return COMM_NOT_AVAILABLE; + if (cnt == 0) + return COMM_NOT_AVAILABLE; - for (int i = 0; i < cnt; i++) - { - uint8_t id = id_list_[i]; + for (int i = 0; i < cnt; i++) { + uint8_t id = id_list_[i]; - result = ph_->readRx(port_, id, length_list_[id], data_list_[id], error_list_[id]); - if (result != COMM_SUCCESS) - return result; - } + result = ph_->readRx(port_, id, length_list_[id], data_list_[id], error_list_[id]); + if (result != COMM_SUCCESS) + return result; + } - if (result == COMM_SUCCESS) - last_result_ = true; + if (result == COMM_SUCCESS) + last_result_ = true; - return result; + return result; } int GroupBulkRead::txRxPacket() { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - result = txPacket(); - if (result != COMM_SUCCESS) - return result; + result = txPacket(); + if (result != COMM_SUCCESS) + return result; - return rxPacket(); + return rxPacket(); } bool GroupBulkRead::isAvailable(uint8_t id, uint16_t address, uint16_t data_length) { - uint16_t start_addr; + uint16_t start_addr; - if (last_result_ == false || data_list_.find(id) == data_list_.end()) - return false; + if (last_result_ == false || data_list_.find(id) == data_list_.end()) + return false; - start_addr = address_list_[id]; + start_addr = address_list_[id]; - if (address < start_addr || start_addr + length_list_[id] - data_length < address) - return false; + if (address < start_addr || start_addr + length_list_[id] - data_length < address) + return false; - return true; + return true; } uint32_t GroupBulkRead::getData(uint8_t id, uint16_t address, uint16_t data_length) { - if (isAvailable(id, address, data_length) == false) - return 0; + if (isAvailable(id, address, data_length) == false) + return 0; - uint16_t start_addr = address_list_[id]; + uint16_t start_addr = address_list_[id]; - switch(data_length) - { - case 1: - return data_list_[id][address - start_addr]; + switch (data_length) { + case 1: return data_list_[id][address - start_addr]; - case 2: - return DXL_MAKEWORD(data_list_[id][address - start_addr], data_list_[id][address - start_addr + 1]); + case 2: return DXL_MAKEWORD(data_list_[id][address - start_addr], data_list_[id][address - start_addr + 1]); case 4: - return DXL_MAKEDWORD(DXL_MAKEWORD(data_list_[id][address - start_addr + 0], data_list_[id][address - start_addr + 1]), - DXL_MAKEWORD(data_list_[id][address - start_addr + 2], data_list_[id][address - start_addr + 3])); + return DXL_MAKEDWORD(DXL_MAKEWORD(data_list_[id][address - start_addr + 0], data_list_[id][address - start_addr + 1]), + DXL_MAKEWORD(data_list_[id][address - start_addr + 2], data_list_[id][address - start_addr + 3])); - default: - return 0; - } + default: return 0; + } } bool GroupBulkRead::getError(uint8_t id, uint8_t* error) { - // TODO : check protocol version, last_result_, data_list - // if (last_result_ == false || error_list_.find(id) == error_list_.end()) + // TODO : check protocol version, last_result_, data_list + // if (last_result_ == false || error_list_.find(id) == error_list_.end()) - return (error[0] = error_list_[id][0]); + return (error[0] = error_list_[id][0]); } \ No newline at end of file diff --git a/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp b/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp index 665378f..1287e0d 100644 --- a/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp +++ b/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/group_bulk_write.cpp @@ -1,18 +1,18 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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: zerom, Ryu Woon Jung (Leon) */ @@ -31,125 +31,119 @@ using namespace dynamixel; -GroupBulkWrite::GroupBulkWrite(PortHandler *port, PacketHandler *ph) - : port_(port), - ph_(ph), - is_param_changed_(false), - param_(0), - param_length_(0) +GroupBulkWrite::GroupBulkWrite(PortHandler* port, PacketHandler* ph) : port_(port), ph_(ph), is_param_changed_(false), param_(0), param_length_(0) { - clearParam(); + clearParam(); } void GroupBulkWrite::makeParam() { - if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) - return; - - if (param_ != 0) - delete[] param_; - param_ = 0; - - param_length_ = 0; - for (unsigned int i = 0; i < id_list_.size(); i++) - param_length_ += 1 + 2 + 2 + length_list_[id_list_[i]]; - - param_ = new uint8_t[param_length_]; - - int idx = 0; - for (unsigned int i = 0; i < id_list_.size(); i++) - { - uint8_t id = id_list_[i]; - if (data_list_[id] == 0) - return; - - param_[idx++] = id; - param_[idx++] = DXL_LOBYTE(address_list_[id]); - param_[idx++] = DXL_HIBYTE(address_list_[id]); - param_[idx++] = DXL_LOBYTE(length_list_[id]); - param_[idx++] = DXL_HIBYTE(length_list_[id]); - for (int c = 0; c < length_list_[id]; c++) - param_[idx++] = (data_list_[id])[c]; - } + if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) + return; + + if (param_ != 0) + delete[] param_; + param_ = 0; + + param_length_ = 0; + for (unsigned int i = 0; i < id_list_.size(); i++) + param_length_ += 1 + 2 + 2 + length_list_[id_list_[i]]; + + param_ = new uint8_t[param_length_]; + + int idx = 0; + for (unsigned int i = 0; i < id_list_.size(); i++) { + uint8_t id = id_list_[i]; + if (data_list_[id] == 0) + return; + + param_[idx++] = id; + param_[idx++] = DXL_LOBYTE(address_list_[id]); + param_[idx++] = DXL_HIBYTE(address_list_[id]); + param_[idx++] = DXL_LOBYTE(length_list_[id]); + param_[idx++] = DXL_HIBYTE(length_list_[id]); + for (int c = 0; c < length_list_[id]; c++) + param_[idx++] = (data_list_[id])[c]; + } } -bool GroupBulkWrite::addParam(uint8_t id, uint16_t start_address, uint16_t data_length, uint8_t *data) +bool GroupBulkWrite::addParam(uint8_t id, uint16_t start_address, uint16_t data_length, uint8_t* data) { - if (ph_->getProtocolVersion() == 1.0) - return false; + if (ph_->getProtocolVersion() == 1.0) + return false; - if (std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist - return false; + if (std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist + return false; - id_list_.push_back(id); - address_list_[id] = start_address; - length_list_[id] = data_length; - data_list_[id] = new uint8_t[data_length]; - for (int c = 0; c < data_length; c++) - data_list_[id][c] = data[c]; + id_list_.push_back(id); + address_list_[id] = start_address; + length_list_[id] = data_length; + data_list_[id] = new uint8_t[data_length]; + for (int c = 0; c < data_length; c++) + data_list_[id][c] = data[c]; - is_param_changed_ = true; - return true; + is_param_changed_ = true; + return true; } void GroupBulkWrite::removeParam(uint8_t id) { - if (ph_->getProtocolVersion() == 1.0) - return; + if (ph_->getProtocolVersion() == 1.0) + return; - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if (it == id_list_.end()) // NOT exist - return; + std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); + if (it == id_list_.end()) // NOT exist + return; - id_list_.erase(it); - address_list_.erase(id); - length_list_.erase(id); - delete[] data_list_[id]; - data_list_.erase(id); + id_list_.erase(it); + address_list_.erase(id); + length_list_.erase(id); + delete[] data_list_[id]; + data_list_.erase(id); - is_param_changed_ = true; + is_param_changed_ = true; } -bool GroupBulkWrite::changeParam(uint8_t id, uint16_t start_address, uint16_t data_length, uint8_t *data) +bool GroupBulkWrite::changeParam(uint8_t id, uint16_t start_address, uint16_t data_length, uint8_t* data) { - if (ph_->getProtocolVersion() == 1.0) - return false; - - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if (it == id_list_.end()) // NOT exist - return false; - - address_list_[id] = start_address; - length_list_[id] = data_length; - delete[] data_list_[id]; - data_list_[id] = new uint8_t[data_length]; - for (int c = 0; c < data_length; c++) - data_list_[id][c] = data[c]; - - is_param_changed_ = true; - return true; + if (ph_->getProtocolVersion() == 1.0) + return false; + + std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); + if (it == id_list_.end()) // NOT exist + return false; + + address_list_[id] = start_address; + length_list_[id] = data_length; + delete[] data_list_[id]; + data_list_[id] = new uint8_t[data_length]; + for (int c = 0; c < data_length; c++) + data_list_[id][c] = data[c]; + + is_param_changed_ = true; + return true; } void GroupBulkWrite::clearParam() { - if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) - return; - - for (unsigned int i = 0; i < id_list_.size(); i++) - delete[] data_list_[id_list_[i]]; - - id_list_.clear(); - address_list_.clear(); - length_list_.clear(); - data_list_.clear(); - if (param_ != 0) - delete[] param_; - param_ = 0; + if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) + return; + + for (unsigned int i = 0; i < id_list_.size(); i++) + delete[] data_list_[id_list_[i]]; + + id_list_.clear(); + address_list_.clear(); + length_list_.clear(); + data_list_.clear(); + if (param_ != 0) + delete[] param_; + param_ = 0; } int GroupBulkWrite::txPacket() { - if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) - return COMM_NOT_AVAILABLE; + if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) + return COMM_NOT_AVAILABLE; - if (is_param_changed_ == true || param_ == 0) - makeParam(); + if (is_param_changed_ == true || param_ == 0) + makeParam(); - return ph_->bulkWriteTxOnly(port_, param_, param_length_); + return ph_->bulkWriteTxOnly(port_, param_, param_length_); } diff --git a/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp b/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp index 124f206..f1c246f 100644 --- a/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp +++ b/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/group_sync_read.cpp @@ -1,18 +1,18 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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: zerom, Ryu Woon Jung (Leon) */ @@ -31,175 +31,163 @@ using namespace dynamixel; -GroupSyncRead::GroupSyncRead(PortHandler *port, PacketHandler *ph, uint16_t start_address, uint16_t data_length) - : port_(port), - ph_(ph), - last_result_(false), - is_param_changed_(false), - param_(0), - start_address_(start_address), - data_length_(data_length) +GroupSyncRead::GroupSyncRead(PortHandler* port, PacketHandler* ph, uint16_t start_address, uint16_t data_length) + : port_(port), ph_(ph), last_result_(false), is_param_changed_(false), param_(0), start_address_(start_address), data_length_(data_length) { - clearParam(); + clearParam(); } void GroupSyncRead::makeParam() { - if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) - return; + if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) + return; - if (param_ != 0) - delete[] param_; - param_ = 0; + if (param_ != 0) + delete[] param_; + param_ = 0; - param_ = new uint8_t[id_list_.size() * 1]; // ID(1) + param_ = new uint8_t[id_list_.size() * 1]; // ID(1) - int idx = 0; - for (unsigned int i = 0; i < id_list_.size(); i++) - param_[idx++] = id_list_[i]; + int idx = 0; + for (unsigned int i = 0; i < id_list_.size(); i++) + param_[idx++] = id_list_[i]; } bool GroupSyncRead::addParam(uint8_t id) { - if (ph_->getProtocolVersion() == 1.0) - return false; + if (ph_->getProtocolVersion() == 1.0) + return false; - if (std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist - return false; + if (std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist + return false; - id_list_.push_back(id); - data_list_[id] = new uint8_t[data_length_]; - error_list_[id] = new uint8_t[1]; + id_list_.push_back(id); + data_list_[id] = new uint8_t[data_length_]; + error_list_[id] = new uint8_t[1]; - is_param_changed_ = true; - return true; + is_param_changed_ = true; + return true; } void GroupSyncRead::removeParam(uint8_t id) { - if (ph_->getProtocolVersion() == 1.0) - return; + if (ph_->getProtocolVersion() == 1.0) + return; - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if (it == id_list_.end()) // NOT exist - return; + std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); + if (it == id_list_.end()) // NOT exist + return; - id_list_.erase(it); - delete[] data_list_[id]; - delete[] error_list_[id]; - data_list_.erase(id); - error_list_.erase(id); + id_list_.erase(it); + delete[] data_list_[id]; + delete[] error_list_[id]; + data_list_.erase(id); + error_list_.erase(id); - is_param_changed_ = true; + is_param_changed_ = true; } void GroupSyncRead::clearParam() { - if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) - return; - - for (unsigned int i = 0; i < id_list_.size(); i++) - { - delete[] data_list_[id_list_[i]]; - delete[] error_list_[id_list_[i]]; - } - - id_list_.clear(); - data_list_.clear(); - error_list_.clear(); - if (param_ != 0) - delete[] param_; - param_ = 0; + if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) + return; + + for (unsigned int i = 0; i < id_list_.size(); i++) { + delete[] data_list_[id_list_[i]]; + delete[] error_list_[id_list_[i]]; + } + + id_list_.clear(); + data_list_.clear(); + error_list_.clear(); + if (param_ != 0) + delete[] param_; + param_ = 0; } int GroupSyncRead::txPacket() { - if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) - return COMM_NOT_AVAILABLE; + if (ph_->getProtocolVersion() == 1.0 || id_list_.size() == 0) + return COMM_NOT_AVAILABLE; - if (is_param_changed_ == true || param_ == 0) - makeParam(); + if (is_param_changed_ == true || param_ == 0) + makeParam(); - return ph_->syncReadTx(port_, start_address_, data_length_, param_, (uint16_t)id_list_.size() * 1); + return ph_->syncReadTx(port_, start_address_, data_length_, param_, (uint16_t)id_list_.size() * 1); } int GroupSyncRead::rxPacket() { - last_result_ = false; + last_result_ = false; - if (ph_->getProtocolVersion() == 1.0) - return COMM_NOT_AVAILABLE; + if (ph_->getProtocolVersion() == 1.0) + return COMM_NOT_AVAILABLE; - int cnt = id_list_.size(); - int result = COMM_RX_FAIL; + int cnt = id_list_.size(); + int result = COMM_RX_FAIL; - if (cnt == 0) - return COMM_NOT_AVAILABLE; + if (cnt == 0) + return COMM_NOT_AVAILABLE; - for (int i = 0; i < cnt; i++) - { - uint8_t id = id_list_[i]; + for (int i = 0; i < cnt; i++) { + uint8_t id = id_list_[i]; - result = ph_->readRx(port_, id, data_length_, data_list_[id], error_list_[id]); - if (result != COMM_SUCCESS) - return result; - } + result = ph_->readRx(port_, id, data_length_, data_list_[id], error_list_[id]); + if (result != COMM_SUCCESS) + return result; + } - if (result == COMM_SUCCESS) - last_result_ = true; + if (result == COMM_SUCCESS) + last_result_ = true; - return result; + return result; } int GroupSyncRead::txRxPacket() { - if (ph_->getProtocolVersion() == 1.0) - return COMM_NOT_AVAILABLE; + if (ph_->getProtocolVersion() == 1.0) + return COMM_NOT_AVAILABLE; - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - result = txPacket(); - if (result != COMM_SUCCESS) - return result; + result = txPacket(); + if (result != COMM_SUCCESS) + return result; - return rxPacket(); + return rxPacket(); } bool GroupSyncRead::isAvailable(uint8_t id, uint16_t address, uint16_t data_length) { - if (ph_->getProtocolVersion() == 1.0 || last_result_ == false || data_list_.find(id) == data_list_.end()) - return false; + if (ph_->getProtocolVersion() == 1.0 || last_result_ == false || data_list_.find(id) == data_list_.end()) + return false; - if (address < start_address_ || start_address_ + data_length_ - data_length < address) - return false; + if (address < start_address_ || start_address_ + data_length_ - data_length < address) + return false; - return true; + return true; } uint32_t GroupSyncRead::getData(uint8_t id, uint16_t address, uint16_t data_length) { - if (isAvailable(id, address, data_length) == false) - return 0; + if (isAvailable(id, address, data_length) == false) + return 0; - switch(data_length) - { - case 1: - return data_list_[id][address - start_address_]; + switch (data_length) { + case 1: return data_list_[id][address - start_address_]; - case 2: - return DXL_MAKEWORD(data_list_[id][address - start_address_], data_list_[id][address - start_address_ + 1]); + case 2: return DXL_MAKEWORD(data_list_[id][address - start_address_], data_list_[id][address - start_address_ + 1]); case 4: - return DXL_MAKEDWORD(DXL_MAKEWORD(data_list_[id][address - start_address_ + 0], data_list_[id][address - start_address_ + 1]), - DXL_MAKEWORD(data_list_[id][address - start_address_ + 2], data_list_[id][address - start_address_ + 3])); + return DXL_MAKEDWORD(DXL_MAKEWORD(data_list_[id][address - start_address_ + 0], data_list_[id][address - start_address_ + 1]), + DXL_MAKEWORD(data_list_[id][address - start_address_ + 2], data_list_[id][address - start_address_ + 3])); - default: - return 0; - } + default: return 0; + } } bool GroupSyncRead::getError(uint8_t id, uint8_t* error) { - // TODO : check protocol version, last_result_, data_list - // if (ph_->getProtocolVersion() == 1.0 || last_result_ == false || error_list_.find(id) == error_list_.end()) + // TODO : check protocol version, last_result_, data_list + // if (ph_->getProtocolVersion() == 1.0 || last_result_ == false || error_list_.find(id) == error_list_.end()) - return (error[0] = error_list_[id][0]); + return (error[0] = error_list_[id][0]); } \ No newline at end of file diff --git a/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp b/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp index 1202d47..95ee58d 100644 --- a/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp +++ b/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/group_sync_write.cpp @@ -1,18 +1,18 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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: zerom, Ryu Woon Jung (Leon) */ @@ -31,104 +31,99 @@ using namespace dynamixel; -GroupSyncWrite::GroupSyncWrite(PortHandler *port, PacketHandler *ph, uint16_t start_address, uint16_t data_length) - : port_(port), - ph_(ph), - is_param_changed_(false), - param_(0), - start_address_(start_address), - data_length_(data_length) +GroupSyncWrite::GroupSyncWrite(PortHandler* port, PacketHandler* ph, uint16_t start_address, uint16_t data_length) + : port_(port), ph_(ph), is_param_changed_(false), param_(0), start_address_(start_address), data_length_(data_length) { - clearParam(); + clearParam(); } void GroupSyncWrite::makeParam() { - if (id_list_.size() == 0) return; + if (id_list_.size() == 0) + return; - if (param_ != 0) - delete[] param_; - param_ = 0; + if (param_ != 0) + delete[] param_; + param_ = 0; - param_ = new uint8_t[id_list_.size() * (1 + data_length_)]; // ID(1) + DATA(data_length) + param_ = new uint8_t[id_list_.size() * (1 + data_length_)]; // ID(1) + DATA(data_length) - int idx = 0; - for (unsigned int i = 0; i < id_list_.size(); i++) - { - uint8_t id = id_list_[i]; - if (data_list_[id] == 0) - return; + int idx = 0; + for (unsigned int i = 0; i < id_list_.size(); i++) { + uint8_t id = id_list_[i]; + if (data_list_[id] == 0) + return; - param_[idx++] = id; - for (int c = 0; c < data_length_; c++) - param_[idx++] = (data_list_[id])[c]; - } + param_[idx++] = id; + for (int c = 0; c < data_length_; c++) + param_[idx++] = (data_list_[id])[c]; + } } -bool GroupSyncWrite::addParam(uint8_t id, uint8_t *data) +bool GroupSyncWrite::addParam(uint8_t id, uint8_t* data) { - if (std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist - return false; + if (std::find(id_list_.begin(), id_list_.end(), id) != id_list_.end()) // id already exist + return false; - id_list_.push_back(id); - data_list_[id] = new uint8_t[data_length_]; - for (int c = 0; c < data_length_; c++) - data_list_[id][c] = data[c]; + id_list_.push_back(id); + data_list_[id] = new uint8_t[data_length_]; + for (int c = 0; c < data_length_; c++) + data_list_[id][c] = data[c]; - is_param_changed_ = true; - return true; + is_param_changed_ = true; + return true; } void GroupSyncWrite::removeParam(uint8_t id) { - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if (it == id_list_.end()) // NOT exist - return; + std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); + if (it == id_list_.end()) // NOT exist + return; - id_list_.erase(it); - delete[] data_list_[id]; - data_list_.erase(id); + id_list_.erase(it); + delete[] data_list_[id]; + data_list_.erase(id); - is_param_changed_ = true; + is_param_changed_ = true; } -bool GroupSyncWrite::changeParam(uint8_t id, uint8_t *data) +bool GroupSyncWrite::changeParam(uint8_t id, uint8_t* data) { - std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); - if (it == id_list_.end()) // NOT exist - return false; + std::vector::iterator it = std::find(id_list_.begin(), id_list_.end(), id); + if (it == id_list_.end()) // NOT exist + return false; - delete[] data_list_[id]; - data_list_[id] = new uint8_t[data_length_]; - for (int c = 0; c < data_length_; c++) - data_list_[id][c] = data[c]; + delete[] data_list_[id]; + data_list_[id] = new uint8_t[data_length_]; + for (int c = 0; c < data_length_; c++) + data_list_[id][c] = data[c]; - is_param_changed_ = true; - return true; + is_param_changed_ = true; + return true; } void GroupSyncWrite::clearParam() { - if (id_list_.size() == 0) - return; + if (id_list_.size() == 0) + return; - for (unsigned int i = 0; i < id_list_.size(); i++) - delete[] data_list_[id_list_[i]]; + for (unsigned int i = 0; i < id_list_.size(); i++) + delete[] data_list_[id_list_[i]]; - id_list_.clear(); - data_list_.clear(); - if (param_ != 0) - delete[] param_; - param_ = 0; + id_list_.clear(); + data_list_.clear(); + if (param_ != 0) + delete[] param_; + param_ = 0; } int GroupSyncWrite::txPacket() { - if (id_list_.size() == 0) - return COMM_NOT_AVAILABLE; + if (id_list_.size() == 0) + return COMM_NOT_AVAILABLE; - if (is_param_changed_ == true || param_ == 0) - makeParam(); + if (is_param_changed_ == true || param_ == 0) + makeParam(); - return ph_->syncWriteTxOnly(port_, start_address_, data_length_, param_, id_list_.size() * (1 + data_length_)); + return ph_->syncWriteTxOnly(port_, start_address_, data_length_, param_, id_list_.size() * (1 + data_length_)); } diff --git a/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/packet_handler.cpp b/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/packet_handler.cpp index c926715..e646aa8 100644 --- a/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/packet_handler.cpp +++ b/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/packet_handler.cpp @@ -1,18 +1,18 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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: zerom, Ryu Woon Jung (Leon) */ @@ -37,16 +37,14 @@ using namespace dynamixel; -PacketHandler *PacketHandler::getPacketHandler(float protocol_version) +PacketHandler* PacketHandler::getPacketHandler(float protocol_version) { - if (protocol_version == 1.0) - { - return (PacketHandler *)(Protocol1PacketHandler::getInstance()); - } - else if (protocol_version == 2.0) - { - return (PacketHandler *)(Protocol2PacketHandler::getInstance()); - } + if (protocol_version == 1.0) { + return (PacketHandler*)(Protocol1PacketHandler::getInstance()); + } + else if (protocol_version == 2.0) { + return (PacketHandler*)(Protocol2PacketHandler::getInstance()); + } - return (PacketHandler *)(Protocol2PacketHandler::getInstance()); + return (PacketHandler*)(Protocol2PacketHandler::getInstance()); } diff --git a/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/port_handler.cpp b/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/port_handler.cpp index 3054c85..9d86b16 100644 --- a/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/port_handler.cpp +++ b/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/port_handler.cpp @@ -1,18 +1,18 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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: zerom, Ryu Woon Jung (Leon) */ @@ -33,15 +33,15 @@ using namespace dynamixel; -PortHandler *PortHandler::getPortHandler(const char *port_name) +PortHandler* PortHandler::getPortHandler(const char* port_name) { #if defined(__linux__) - return (PortHandler *)(new PortHandlerLinux(port_name)); + return (PortHandler*)(new PortHandlerLinux(port_name)); #elif defined(__APPLE__) - return (PortHandler *)(new PortHandlerMac(port_name)); + return (PortHandler*)(new PortHandlerMac(port_name)); #elif defined(_WIN32) || defined(_WIN64) - return (PortHandler *)(new PortHandlerWindows(port_name)); + return (PortHandler*)(new PortHandlerWindows(port_name)); #elif defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__) - return (PortHandler *)(new PortHandlerArduino(port_name)); + return (PortHandler*)(new PortHandlerArduino(port_name)); #endif } diff --git a/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/port_handler_arduino.cpp b/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/port_handler_arduino.cpp index 07add9a..edccd4b 100644 --- a/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/port_handler_arduino.cpp +++ b/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/port_handler_arduino.cpp @@ -1,18 +1,18 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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: Ryu Woon Jung (Leon) */ @@ -20,272 +20,249 @@ #include - #include "../../include/dynamixel_sdk/port_handler_arduino.h" -#if defined (__OPENCR__) -#define DYNAMIXEL_SERIAL Serial3 +#if defined(__OPENCR__) +#define DYNAMIXEL_SERIAL Serial3 #endif -#define LATENCY_TIMER 4 // msec (USB latency timer) +#define LATENCY_TIMER 4 // msec (USB latency timer) using namespace dynamixel; -PortHandlerArduino::PortHandlerArduino(const char *port_name) - : baudrate_(DEFAULT_BAUDRATE_), - packet_start_time_(0.0), - packet_timeout_(0.0), - tx_time_per_byte(0.0) +PortHandlerArduino::PortHandlerArduino(const char* port_name) : baudrate_(DEFAULT_BAUDRATE_), packet_start_time_(0.0), packet_timeout_(0.0), tx_time_per_byte(0.0) { - is_using_ = false; - setPortName(port_name); + is_using_ = false; + setPortName(port_name); #if defined(__OPENCR__) - pinMode(BDPIN_DXL_PWR_EN, OUTPUT); + pinMode(BDPIN_DXL_PWR_EN, OUTPUT); - setPowerOff(); + setPowerOff(); #elif defined(__OPENCM904__) - if (port_name[0] == '1') - { - socket_fd_ = 0; - p_dxl_serial = &Serial1; - } - else if (port_name[0] == '2') - { - socket_fd_ = 1; - p_dxl_serial = &Serial2; - } - else if (port_name[0] == '3') - { - socket_fd_ = 2; - p_dxl_serial = &Serial3; - } - else - { - socket_fd_ = 0; - p_dxl_serial = &Serial1; - } - - drv_dxl_begin(socket_fd_); + if (port_name[0] == '1') { + socket_fd_ = 0; + p_dxl_serial = &Serial1; + } + else if (port_name[0] == '2') { + socket_fd_ = 1; + p_dxl_serial = &Serial2; + } + else if (port_name[0] == '3') { + socket_fd_ = 2; + p_dxl_serial = &Serial3; + } + else { + socket_fd_ = 0; + p_dxl_serial = &Serial1; + } + + drv_dxl_begin(socket_fd_); #endif - setTxDisable(); + setTxDisable(); } bool PortHandlerArduino::openPort() { #if defined(__OPENCR__) - pinMode(BDPIN_DXL_PWR_EN, OUTPUT); + pinMode(BDPIN_DXL_PWR_EN, OUTPUT); - setPowerOn(); - delay(1000); + setPowerOn(); + delay(1000); #endif - return setBaudRate(baudrate_); + return setBaudRate(baudrate_); } void PortHandlerArduino::closePort() { - setPowerOff(); + setPowerOff(); } void PortHandlerArduino::clearPort() { - int temp __attribute__((unused)); + int temp __attribute__((unused)); #if defined(__OPENCR__) - while (DYNAMIXEL_SERIAL.available()) - { - temp = DYNAMIXEL_SERIAL.read(); - } + while (DYNAMIXEL_SERIAL.available()) { + temp = DYNAMIXEL_SERIAL.read(); + } #elif defined(__OPENCM904__) - while (p_dxl_serial->available()) - { - temp = p_dxl_serial->read(); - } + while (p_dxl_serial->available()) { + temp = p_dxl_serial->read(); + } #endif } -void PortHandlerArduino::setPortName(const char *port_name) +void PortHandlerArduino::setPortName(const char* port_name) { - strcpy(port_name_, port_name); + strcpy(port_name_, port_name); } -char *PortHandlerArduino::getPortName() +char* PortHandlerArduino::getPortName() { - return port_name_; + return port_name_; } bool PortHandlerArduino::setBaudRate(const int baudrate) { - baudrate_ = checkBaudrateAvailable(baudrate); + baudrate_ = checkBaudrateAvailable(baudrate); - if (baudrate_ == -1) - return false; + if (baudrate_ == -1) + return false; - setupPort(baudrate_); + setupPort(baudrate_); - return true; + return true; } int PortHandlerArduino::getBaudRate() { - return baudrate_; + return baudrate_; } int PortHandlerArduino::getBytesAvailable() { - int bytes_available; + int bytes_available; #if defined(__OPENCR__) - bytes_available = DYNAMIXEL_SERIAL.available(); + bytes_available = DYNAMIXEL_SERIAL.available(); #elif defined(__OPENCM904__) - bytes_available = p_dxl_serial->available(); + bytes_available = p_dxl_serial->available(); #endif - return bytes_available; + return bytes_available; } -int PortHandlerArduino::readPort(uint8_t *packet, int length) +int PortHandlerArduino::readPort(uint8_t* packet, int length) { - int rx_length; + int rx_length; #if defined(__OPENCR__) - rx_length = DYNAMIXEL_SERIAL.available(); + rx_length = DYNAMIXEL_SERIAL.available(); #elif defined(__OPENCM904__) - rx_length = p_dxl_serial->available(); + rx_length = p_dxl_serial->available(); #endif - if (rx_length > length) - rx_length = length; + if (rx_length > length) + rx_length = length; - for (int i = 0; i < rx_length; i++) - { + for (int i = 0; i < rx_length; i++) { #if defined(__OPENCR__) - packet[i] = DYNAMIXEL_SERIAL.read(); + packet[i] = DYNAMIXEL_SERIAL.read(); #elif defined(__OPENCM904__) - packet[i] = p_dxl_serial->read(); + packet[i] = p_dxl_serial->read(); #endif - } + } - return rx_length; + return rx_length; } -int PortHandlerArduino::writePort(uint8_t *packet, int length) +int PortHandlerArduino::writePort(uint8_t* packet, int length) { - int length_written; + int length_written; - setTxEnable(); + setTxEnable(); #if defined(__OPENCR__) - length_written = DYNAMIXEL_SERIAL.write(packet, length); + length_written = DYNAMIXEL_SERIAL.write(packet, length); #elif defined(__OPENCM904__) - length_written = p_dxl_serial->write(packet, length); + length_written = p_dxl_serial->write(packet, length); #endif - setTxDisable(); + setTxDisable(); - return length_written; + return length_written; } void PortHandlerArduino::setPacketTimeout(uint16_t packet_length) { - packet_start_time_ = getCurrentTime(); - packet_timeout_ = (tx_time_per_byte * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0; + packet_start_time_ = getCurrentTime(); + packet_timeout_ = (tx_time_per_byte * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0; } void PortHandlerArduino::setPacketTimeout(double msec) { - packet_start_time_ = getCurrentTime(); - packet_timeout_ = msec; + packet_start_time_ = getCurrentTime(); + packet_timeout_ = msec; } bool PortHandlerArduino::isPacketTimeout() { - if (getTimeSinceStart() > packet_timeout_) - { - packet_timeout_ = 0; - return true; - } + if (getTimeSinceStart() > packet_timeout_) { + packet_timeout_ = 0; + return true; + } - return false; + return false; } double PortHandlerArduino::getCurrentTime() { - return (double)millis(); + return (double)millis(); } double PortHandlerArduino::getTimeSinceStart() { - double elapsed_time; + double elapsed_time; - elapsed_time = getCurrentTime() - packet_start_time_; - if (elapsed_time < 0.0) - packet_start_time_ = getCurrentTime(); + elapsed_time = getCurrentTime() - packet_start_time_; + if (elapsed_time < 0.0) + packet_start_time_ = getCurrentTime(); - return elapsed_time; + return elapsed_time; } bool PortHandlerArduino::setupPort(int baudrate) { #if defined(__OPENCR__) - DYNAMIXEL_SERIAL.begin(baudrate); + DYNAMIXEL_SERIAL.begin(baudrate); #elif defined(__OPENCM904__) - p_dxl_serial->setDxlMode(true); - p_dxl_serial->begin(baudrate); + p_dxl_serial->setDxlMode(true); + p_dxl_serial->begin(baudrate); #endif - delay(100); + delay(100); - tx_time_per_byte = (1000.0 / (double)baudrate) * 10.0; - return true; + tx_time_per_byte = (1000.0 / (double)baudrate) * 10.0; + return true; } int PortHandlerArduino::checkBaudrateAvailable(int baudrate) { - switch(baudrate) - { - case 9600: - return 9600; - case 57600: - return 57600; - case 115200: - return 115200; - case 1000000: - return 1000000; - case 2000000: - return 2000000; - case 3000000: - return 3000000; - case 4000000: - return 4000000; - case 4500000: - return 4500000; - default: - return -1; - } + switch (baudrate) { + case 9600: return 9600; + case 57600: return 57600; + case 115200: return 115200; + case 1000000: return 1000000; + case 2000000: return 2000000; + case 3000000: return 3000000; + case 4000000: return 4000000; + case 4500000: return 4500000; + default: return -1; + } } void PortHandlerArduino::setPowerOn() { #if defined(__OPENCR__) - digitalWrite(BDPIN_DXL_PWR_EN, HIGH); + digitalWrite(BDPIN_DXL_PWR_EN, HIGH); #endif } void PortHandlerArduino::setPowerOff() { #if defined(__OPENCR__) - digitalWrite(BDPIN_DXL_PWR_EN, LOW); + digitalWrite(BDPIN_DXL_PWR_EN, LOW); #endif } void PortHandlerArduino::setTxEnable() { #if defined(__OPENCR__) - drv_dxl_tx_enable(TRUE); + drv_dxl_tx_enable(TRUE); #elif defined(__OPENCM904__) - drv_dxl_tx_enable(socket_fd_, TRUE); + drv_dxl_tx_enable(socket_fd_, TRUE); #endif } @@ -293,15 +270,15 @@ void PortHandlerArduino::setTxDisable() { #if defined(__OPENCR__) #ifdef SERIAL_WRITES_NON_BLOCKING - DYNAMIXEL_SERIAL.flush(); // make sure it completes before we disable... + DYNAMIXEL_SERIAL.flush(); // make sure it completes before we disable... #endif - drv_dxl_tx_enable(FALSE); + drv_dxl_tx_enable(FALSE); #elif defined(__OPENCM904__) #ifdef SERIAL_WRITES_NON_BLOCKING - p_dxl_serial->flush(); + p_dxl_serial->flush(); #endif - drv_dxl_tx_enable(socket_fd_, FALSE); + drv_dxl_tx_enable(socket_fd_, FALSE); #endif } diff --git a/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/port_handler_linux.cpp b/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/port_handler_linux.cpp index 57564a9..618be08 100644 --- a/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/port_handler_linux.cpp +++ b/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/port_handler_linux.cpp @@ -1,283 +1,253 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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: zerom, Ryu Woon Jung (Leon) */ #if defined(__linux__) -#include #include +#include +#include #include -#include +#include +#include #include #include -#include -#include -#include +#include #include "port_handler_linux.h" -#define LATENCY_TIMER 16 // msec (USB latency timer) - // You should adjust the latency timer value. From the version Ubuntu 16.04.2, the default latency timer of the usb serial is '16 msec'. - // When you are going to use sync / bulk read, the latency timer should be loosen. - // the lower latency timer value, the faster communication speed. - - // Note: - // You can check its value by: - // $ cat /sys/bus/usb-serial/devices/ttyUSB0/latency_timer - // - // If you think that the communication is too slow, type following after plugging the usb in to change the latency timer - // - // Method 1. Type following (you should do this everytime when the usb once was plugged out or the connection was dropped) - // $ echo 1 | sudo tee /sys/bus/usb-serial/devices/ttyUSB0/latency_timer - // $ cat /sys/bus/usb-serial/devices/ttyUSB0/latency_timer - // - // Method 2. If you want to set it as be done automatically, and don't want to do above everytime, make rules file in /etc/udev/rules.d/. For example, - // $ echo ACTION==\"add\", SUBSYSTEM==\"usb-serial\", DRIVER==\"ftdi_sio\", ATTR{latency_timer}=\"1\" > 99-dynamixelsdk-usb.rules - // $ sudo cp ./99-dynamixelsdk-usb.rules /etc/udev/rules.d/ - // $ sudo udevadm control --reload-rules - // $ sudo udevadm trigger --action=add - // $ cat /sys/bus/usb-serial/devices/ttyUSB0/latency_timer - // - // or if you have another good idea that can be an alternatives, - // please give us advice via github issue https://github.com/ROBOTIS-GIT/DynamixelSDK/issues +#define LATENCY_TIMER \ + 16 // msec (USB latency timer) + // You should adjust the latency timer value. From the version Ubuntu 16.04.2, the default latency timer of the usb serial is '16 msec'. + // When you are going to use sync / bulk read, the latency timer should be loosen. + // the lower latency timer value, the faster communication speed. + +// Note: +// You can check its value by: +// $ cat /sys/bus/usb-serial/devices/ttyUSB0/latency_timer +// +// If you think that the communication is too slow, type following after plugging the usb in to change the latency timer +// +// Method 1. Type following (you should do this everytime when the usb once was plugged out or the connection was dropped) +// $ echo 1 | sudo tee /sys/bus/usb-serial/devices/ttyUSB0/latency_timer +// $ cat /sys/bus/usb-serial/devices/ttyUSB0/latency_timer +// +// Method 2. If you want to set it as be done automatically, and don't want to do above everytime, make rules file in /etc/udev/rules.d/. For example, +// $ echo ACTION==\"add\", SUBSYSTEM==\"usb-serial\", DRIVER==\"ftdi_sio\", ATTR{latency_timer}=\"1\" > 99-dynamixelsdk-usb.rules +// $ sudo cp ./99-dynamixelsdk-usb.rules /etc/udev/rules.d/ +// $ sudo udevadm control --reload-rules +// $ sudo udevadm trigger --action=add +// $ cat /sys/bus/usb-serial/devices/ttyUSB0/latency_timer +// +// or if you have another good idea that can be an alternatives, +// please give us advice via github issue https://github.com/ROBOTIS-GIT/DynamixelSDK/issues using namespace dynamixel; -PortHandlerLinux::PortHandlerLinux(const char *port_name) - : socket_fd_(-1), - baudrate_(DEFAULT_BAUDRATE_), - packet_start_time_(0.0), - packet_timeout_(0.0), - tx_time_per_byte(0.0) +PortHandlerLinux::PortHandlerLinux(const char* port_name) : socket_fd_(-1), baudrate_(DEFAULT_BAUDRATE_), packet_start_time_(0.0), packet_timeout_(0.0), tx_time_per_byte(0.0) { - is_using_ = false; - setPortName(port_name); + is_using_ = false; + setPortName(port_name); } bool PortHandlerLinux::openPort() { - return setBaudRate(baudrate_); + return setBaudRate(baudrate_); } void PortHandlerLinux::closePort() { - if(socket_fd_ != -1) - close(socket_fd_); - socket_fd_ = -1; + if (socket_fd_ != -1) + close(socket_fd_); + socket_fd_ = -1; } void PortHandlerLinux::clearPort() { - tcflush(socket_fd_, TCIFLUSH); + tcflush(socket_fd_, TCIFLUSH); } -void PortHandlerLinux::setPortName(const char *port_name) +void PortHandlerLinux::setPortName(const char* port_name) { - strcpy(port_name_, port_name); + strcpy(port_name_, port_name); } -char *PortHandlerLinux::getPortName() +char* PortHandlerLinux::getPortName() { - return port_name_; + return port_name_; } // TODO: baud number ?? bool PortHandlerLinux::setBaudRate(const int baudrate) { - int baud = getCFlagBaud(baudrate); - - closePort(); - - if(baud <= 0) // custom baudrate - { - setupPort(B38400); - baudrate_ = baudrate; - return setCustomBaudrate(baudrate); - } - else - { - baudrate_ = baudrate; - return setupPort(baud); - } + int baud = getCFlagBaud(baudrate); + + closePort(); + + if (baud <= 0) // custom baudrate + { + setupPort(B38400); + baudrate_ = baudrate; + return setCustomBaudrate(baudrate); + } + else { + baudrate_ = baudrate; + return setupPort(baud); + } } int PortHandlerLinux::getBaudRate() { - return baudrate_; + return baudrate_; } int PortHandlerLinux::getBytesAvailable() { - int bytes_available; - ioctl(socket_fd_, FIONREAD, &bytes_available); - return bytes_available; + int bytes_available; + ioctl(socket_fd_, FIONREAD, &bytes_available); + return bytes_available; } -int PortHandlerLinux::readPort(uint8_t *packet, int length) +int PortHandlerLinux::readPort(uint8_t* packet, int length) { - return read(socket_fd_, packet, length); + return read(socket_fd_, packet, length); } -int PortHandlerLinux::writePort(uint8_t *packet, int length) +int PortHandlerLinux::writePort(uint8_t* packet, int length) { - return write(socket_fd_, packet, length); + return write(socket_fd_, packet, length); } void PortHandlerLinux::setPacketTimeout(uint16_t packet_length) { - packet_start_time_ = getCurrentTime(); - packet_timeout_ = (tx_time_per_byte * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0; + packet_start_time_ = getCurrentTime(); + packet_timeout_ = (tx_time_per_byte * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0; } void PortHandlerLinux::setPacketTimeout(double msec) { - packet_start_time_ = getCurrentTime(); - packet_timeout_ = msec; + packet_start_time_ = getCurrentTime(); + packet_timeout_ = msec; } bool PortHandlerLinux::isPacketTimeout() { - if(getTimeSinceStart() > packet_timeout_) - { - packet_timeout_ = 0; - return true; - } - return false; + if (getTimeSinceStart() > packet_timeout_) { + packet_timeout_ = 0; + return true; + } + return false; } double PortHandlerLinux::getCurrentTime() { - struct timespec tv; - clock_gettime(CLOCK_REALTIME, &tv); - return ((double)tv.tv_sec * 1000.0 + (double)tv.tv_nsec * 0.001 * 0.001); + struct timespec tv; + clock_gettime(CLOCK_REALTIME, &tv); + return ((double)tv.tv_sec * 1000.0 + (double)tv.tv_nsec * 0.001 * 0.001); } double PortHandlerLinux::getTimeSinceStart() { - double time; + double time; - time = getCurrentTime() - packet_start_time_; - if(time < 0.0) - packet_start_time_ = getCurrentTime(); + time = getCurrentTime() - packet_start_time_; + if (time < 0.0) + packet_start_time_ = getCurrentTime(); - return time; + return time; } bool PortHandlerLinux::setupPort(int cflag_baud) { - struct termios newtio; + struct termios newtio; - socket_fd_ = open(port_name_, O_RDWR|O_NOCTTY|O_NONBLOCK); - if(socket_fd_ < 0) - { - printf("[PortHandlerLinux::SetupPort] Error opening serial port!\n"); - return false; - } + socket_fd_ = open(port_name_, O_RDWR | O_NOCTTY | O_NONBLOCK); + if (socket_fd_ < 0) { + printf("[PortHandlerLinux::SetupPort] Error opening serial port!\n"); + return false; + } - bzero(&newtio, sizeof(newtio)); // clear struct for new port settings + bzero(&newtio, sizeof(newtio)); // clear struct for new port settings - newtio.c_cflag = cflag_baud | CS8 | CLOCAL | CREAD; - newtio.c_iflag = IGNPAR; - newtio.c_oflag = 0; - newtio.c_lflag = 0; - newtio.c_cc[VTIME] = 0; - newtio.c_cc[VMIN] = 0; + newtio.c_cflag = cflag_baud | CS8 | CLOCAL | CREAD; + newtio.c_iflag = IGNPAR; + newtio.c_oflag = 0; + newtio.c_lflag = 0; + newtio.c_cc[VTIME] = 0; + newtio.c_cc[VMIN] = 0; - // clean the buffer and activate the settings for the port - tcflush(socket_fd_, TCIFLUSH); - tcsetattr(socket_fd_, TCSANOW, &newtio); + // clean the buffer and activate the settings for the port + tcflush(socket_fd_, TCIFLUSH); + tcsetattr(socket_fd_, TCSANOW, &newtio); - tx_time_per_byte = (1000.0 / (double)baudrate_) * 10.0; - return true; + tx_time_per_byte = (1000.0 / (double)baudrate_) * 10.0; + return true; } bool PortHandlerLinux::setCustomBaudrate(int speed) { - // try to set a custom divisor - struct serial_struct ss; - if(ioctl(socket_fd_, TIOCGSERIAL, &ss) != 0) - { - printf("[PortHandlerLinux::SetCustomBaudrate] TIOCGSERIAL failed!\n"); - return false; - } + // try to set a custom divisor + struct serial_struct ss; + if (ioctl(socket_fd_, TIOCGSERIAL, &ss) != 0) { + printf("[PortHandlerLinux::SetCustomBaudrate] TIOCGSERIAL failed!\n"); + return false; + } - ss.flags = (ss.flags & ~ASYNC_SPD_MASK) | ASYNC_SPD_CUST; - ss.custom_divisor = (ss.baud_base + (speed / 2)) / speed; - int closest_speed = ss.baud_base / ss.custom_divisor; + ss.flags = (ss.flags & ~ASYNC_SPD_MASK) | ASYNC_SPD_CUST; + ss.custom_divisor = (ss.baud_base + (speed / 2)) / speed; + int closest_speed = ss.baud_base / ss.custom_divisor; - if(closest_speed < speed * 98 / 100 || closest_speed > speed * 102 / 100) - { - printf("[PortHandlerLinux::SetCustomBaudrate] Cannot set speed to %d, closest is %d \n", speed, closest_speed); - return false; - } + if (closest_speed < speed * 98 / 100 || closest_speed > speed * 102 / 100) { + printf("[PortHandlerLinux::SetCustomBaudrate] Cannot set speed to %d, closest is %d \n", speed, closest_speed); + return false; + } - if(ioctl(socket_fd_, TIOCSSERIAL, &ss) < 0) - { - printf("[PortHandlerLinux::SetCustomBaudrate] TIOCSSERIAL failed!\n"); - return false; - } + if (ioctl(socket_fd_, TIOCSSERIAL, &ss) < 0) { + printf("[PortHandlerLinux::SetCustomBaudrate] TIOCSSERIAL failed!\n"); + return false; + } - tx_time_per_byte = (1000.0 / (double)speed) * 10.0; - return true; + tx_time_per_byte = (1000.0 / (double)speed) * 10.0; + return true; } int PortHandlerLinux::getCFlagBaud(int baudrate) { - switch(baudrate) - { - case 9600: - return B9600; - case 19200: - return B19200; - case 38400: - return B38400; - case 57600: - return B57600; - case 115200: - return B115200; - case 230400: - return B230400; - case 460800: - return B460800; - case 500000: - return B500000; - case 576000: - return B576000; - case 921600: - return B921600; - case 1000000: - return B1000000; - case 1152000: - return B1152000; - case 1500000: - return B1500000; - case 2000000: - return B2000000; - case 2500000: - return B2500000; - case 3000000: - return B3000000; - case 3500000: - return B3500000; - case 4000000: - return B4000000; - default: - return -1; - } + switch (baudrate) { + case 9600: return B9600; + case 19200: return B19200; + case 38400: return B38400; + case 57600: return B57600; + case 115200: return B115200; + case 230400: return B230400; + case 460800: return B460800; + case 500000: return B500000; + case 576000: return B576000; + case 921600: return B921600; + case 1000000: return B1000000; + case 1152000: return B1152000; + case 1500000: return B1500000; + case 2000000: return B2000000; + case 2500000: return B2500000; + case 3000000: return B3000000; + case 3500000: return B3500000; + case 4000000: return B4000000; + default: return -1; + } } #endif diff --git a/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/port_handler_mac.cpp b/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/port_handler_mac.cpp index f602d57..6384207 100644 --- a/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/port_handler_mac.cpp +++ b/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/port_handler_mac.cpp @@ -1,31 +1,31 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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: Ryu Woon Jung (Leon) */ #if defined(__APPLE__) -#include #include +#include #include -#include +#include +#include #include #include -#include -#include +#include #ifdef __MACH__ #include @@ -34,199 +34,185 @@ #include "port_handler_mac.h" -#define LATENCY_TIMER 16 // msec (USB latency timer) - // You should adjust the latency timer value. - // When you are going to use sync / bulk read, the latency timer should be loosen. - // the lower latency timer value, the faster communication speed. +#define LATENCY_TIMER \ + 16 // msec (USB latency timer) + // You should adjust the latency timer value. + // When you are going to use sync / bulk read, the latency timer should be loosen. + // the lower latency timer value, the faster communication speed. - // Note: - // You can either change its value by following: - // http://www.ftdichip.com/Support/Documents/TechnicalNotes/TN_105%20Adding%20Support%20for%20New%20FTDI%20Devices%20to%20Mac%20Driver.pdf +// Note: +// You can either change its value by following: +// http://www.ftdichip.com/Support/Documents/TechnicalNotes/TN_105%20Adding%20Support%20for%20New%20FTDI%20Devices%20to%20Mac%20Driver.pdf using namespace dynamixel; -PortHandlerMac::PortHandlerMac(const char *port_name) - : socket_fd_(-1), - baudrate_(DEFAULT_BAUDRATE_), - packet_start_time_(0.0), - packet_timeout_(0.0), - tx_time_per_byte(0.0) +PortHandlerMac::PortHandlerMac(const char* port_name) : socket_fd_(-1), baudrate_(DEFAULT_BAUDRATE_), packet_start_time_(0.0), packet_timeout_(0.0), tx_time_per_byte(0.0) { - is_using_ = false; - setPortName(port_name); + is_using_ = false; + setPortName(port_name); } bool PortHandlerMac::openPort() { - return setBaudRate(baudrate_); + return setBaudRate(baudrate_); } void PortHandlerMac::closePort() { - if(socket_fd_ != -1) - close(socket_fd_); - socket_fd_ = -1; + if (socket_fd_ != -1) + close(socket_fd_); + socket_fd_ = -1; } void PortHandlerMac::clearPort() { - tcflush(socket_fd_, TCIFLUSH); + tcflush(socket_fd_, TCIFLUSH); } -void PortHandlerMac::setPortName(const char *port_name) +void PortHandlerMac::setPortName(const char* port_name) { - strcpy(port_name_, port_name); + strcpy(port_name_, port_name); } -char *PortHandlerMac::getPortName() +char* PortHandlerMac::getPortName() { - return port_name_; + return port_name_; } // TODO: baud number ?? bool PortHandlerMac::setBaudRate(const int baudrate) { - int baud = getCFlagBaud(baudrate); - - closePort(); - - if(baud <= 0) // custom baudrate - { - setupPort(B38400); - baudrate_ = baudrate; - return setCustomBaudrate(baudrate); - } - else - { - baudrate_ = baudrate; - return setupPort(baud); - } + int baud = getCFlagBaud(baudrate); + + closePort(); + + if (baud <= 0) // custom baudrate + { + setupPort(B38400); + baudrate_ = baudrate; + return setCustomBaudrate(baudrate); + } + else { + baudrate_ = baudrate; + return setupPort(baud); + } } int PortHandlerMac::getBaudRate() { - return baudrate_; + return baudrate_; } int PortHandlerMac::getBytesAvailable() { - int bytes_available; - ioctl(socket_fd_, FIONREAD, &bytes_available); - return bytes_available; + int bytes_available; + ioctl(socket_fd_, FIONREAD, &bytes_available); + return bytes_available; } -int PortHandlerMac::readPort(uint8_t *packet, int length) +int PortHandlerMac::readPort(uint8_t* packet, int length) { - return read(socket_fd_, packet, length); + return read(socket_fd_, packet, length); } -int PortHandlerMac::writePort(uint8_t *packet, int length) +int PortHandlerMac::writePort(uint8_t* packet, int length) { - return write(socket_fd_, packet, length); + return write(socket_fd_, packet, length); } void PortHandlerMac::setPacketTimeout(uint16_t packet_length) { - packet_start_time_ = getCurrentTime(); - packet_timeout_ = (tx_time_per_byte * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0; + packet_start_time_ = getCurrentTime(); + packet_timeout_ = (tx_time_per_byte * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0; } void PortHandlerMac::setPacketTimeout(double msec) { - packet_start_time_ = getCurrentTime(); - packet_timeout_ = msec; + packet_start_time_ = getCurrentTime(); + packet_timeout_ = msec; } bool PortHandlerMac::isPacketTimeout() { - if(getTimeSinceStart() > packet_timeout_) - { - packet_timeout_ = 0; - return true; - } - return false; + if (getTimeSinceStart() > packet_timeout_) { + packet_timeout_ = 0; + return true; + } + return false; } double PortHandlerMac::getCurrentTime() { - struct timespec tv; + struct timespec tv; #ifdef __MACH__ // OS X does not have clock_gettime, so here uses clock_get_time - clock_serv_t cclock; - mach_timespec_t mts; - host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &cclock); - clock_get_time(cclock, &mts); - mach_port_deallocate(mach_task_self(), cclock); - tv.tv_sec = mts.tv_sec; - tv.tv_nsec = mts.tv_nsec; + clock_serv_t cclock; + mach_timespec_t mts; + host_get_clock_service(mach_host_self(), CALENDAR_CLOCK, &cclock); + clock_get_time(cclock, &mts); + mach_port_deallocate(mach_task_self(), cclock); + tv.tv_sec = mts.tv_sec; + tv.tv_nsec = mts.tv_nsec; #else - clock_gettime(CLOCK_REALTIME, &tv); + clock_gettime(CLOCK_REALTIME, &tv); #endif - return ((double)tv.tv_sec * 1000.0 + (double)tv.tv_nsec * 0.001 * 0.001); + return ((double)tv.tv_sec * 1000.0 + (double)tv.tv_nsec * 0.001 * 0.001); } double PortHandlerMac::getTimeSinceStart() { - double time; + double time; - time = getCurrentTime() - packet_start_time_; - if(time < 0.0) - packet_start_time_ = getCurrentTime(); + time = getCurrentTime() - packet_start_time_; + if (time < 0.0) + packet_start_time_ = getCurrentTime(); - return time; + return time; } bool PortHandlerMac::setupPort(int cflag_baud) { - struct termios newtio; + struct termios newtio; - socket_fd_ = open(port_name_, O_RDWR|O_NOCTTY|O_NONBLOCK); - if(socket_fd_ < 0) - { - printf("[PortHandlerMac::SetupPort] Error opening serial port!\n"); - return false; - } + socket_fd_ = open(port_name_, O_RDWR | O_NOCTTY | O_NONBLOCK); + if (socket_fd_ < 0) { + printf("[PortHandlerMac::SetupPort] Error opening serial port!\n"); + return false; + } - bzero(&newtio, sizeof(newtio)); // clear struct for new port settings + bzero(&newtio, sizeof(newtio)); // clear struct for new port settings - newtio.c_cflag = CS8 | CLOCAL | CREAD; - newtio.c_iflag = IGNPAR; - newtio.c_oflag = 0; - newtio.c_lflag = 0; - newtio.c_cc[VTIME] = 0; - newtio.c_cc[VMIN] = 0; - cfsetispeed(&newtio, cflag_baud); - cfsetospeed(&newtio, cflag_baud); + newtio.c_cflag = CS8 | CLOCAL | CREAD; + newtio.c_iflag = IGNPAR; + newtio.c_oflag = 0; + newtio.c_lflag = 0; + newtio.c_cc[VTIME] = 0; + newtio.c_cc[VMIN] = 0; + cfsetispeed(&newtio, cflag_baud); + cfsetospeed(&newtio, cflag_baud); - // clean the buffer and activate the settings for the port - tcflush(socket_fd_, TCIFLUSH); - tcsetattr(socket_fd_, TCSANOW, &newtio); + // clean the buffer and activate the settings for the port + tcflush(socket_fd_, TCIFLUSH); + tcsetattr(socket_fd_, TCSANOW, &newtio); - tx_time_per_byte = (1000.0 / (double)baudrate_) * 10.0; - return true; + tx_time_per_byte = (1000.0 / (double)baudrate_) * 10.0; + return true; } bool PortHandlerMac::setCustomBaudrate(int speed) { - printf("[PortHandlerMac::SetCustomBaudrate] Not supported on Mac!\n"); - return false; + printf("[PortHandlerMac::SetCustomBaudrate] Not supported on Mac!\n"); + return false; } int PortHandlerMac::getCFlagBaud(int baudrate) { - switch(baudrate) - { - case 9600: - return B9600; - case 19200: - return B19200; - case 38400: - return B38400; - case 57600: - return B57600; - case 115200: - return B115200; - case 230400: - return B230400; + switch (baudrate) { + case 9600: return B9600; + case 19200: return B19200; + case 38400: return B38400; + case 57600: return B57600; + case 115200: return B115200; + case 230400: return B230400; // Mac OS doesn't support over B230400 // case 460800: // return B460800; @@ -252,9 +238,8 @@ int PortHandlerMac::getCFlagBaud(int baudrate) // return B3500000; // case 4000000: // return B4000000; - default: - return -1; - } + default: return -1; + } } #endif diff --git a/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/port_handler_windows.cpp b/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/port_handler_windows.cpp index d9b0ed5..08aa94e 100644 --- a/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/port_handler_windows.cpp +++ b/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/port_handler_windows.cpp @@ -1,18 +1,18 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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: Ryu Woon Jung (Leon) */ @@ -25,211 +25,206 @@ #include #include -#define LATENCY_TIMER 16 // msec (USB latency timer) - // You should adjust the latency timer value. In Windows, the default latency timer of the usb serial is '16 msec'. - // When you are going to use sync / bulk read, the latency timer should be loosen. - // the lower latency timer value, the faster communication speed. +#define LATENCY_TIMER \ + 16 // msec (USB latency timer) + // You should adjust the latency timer value. In Windows, the default latency timer of the usb serial is '16 msec'. + // When you are going to use sync / bulk read, the latency timer should be loosen. + // the lower latency timer value, the faster communication speed. - // Note: - // You can either checking or changing its value by: - // [Device Manager] -> [Port (COM & LPT)] -> the port you use but starts with COMx-> mouse right click -> properties - // -> [port settings] -> [details] -> change response time from 16 to the value you need +// Note: +// You can either checking or changing its value by: +// [Device Manager] -> [Port (COM & LPT)] -> the port you use but starts with COMx-> mouse right click -> properties +// -> [port settings] -> [details] -> change response time from 16 to the value you need using namespace dynamixel; -PortHandlerWindows::PortHandlerWindows(const char *port_name) - : serial_handle_(INVALID_HANDLE_VALUE), - baudrate_(DEFAULT_BAUDRATE_), - packet_start_time_(0.0), - packet_timeout_(0.0), - tx_time_per_byte_(0.0) +PortHandlerWindows::PortHandlerWindows(const char* port_name) + : serial_handle_(INVALID_HANDLE_VALUE), baudrate_(DEFAULT_BAUDRATE_), packet_start_time_(0.0), packet_timeout_(0.0), tx_time_per_byte_(0.0) { - is_using_ = false; + is_using_ = false; - char buffer[15]; - sprintf_s(buffer, sizeof(buffer), "\\\\.\\%s", port_name); - setPortName(buffer); + char buffer[15]; + sprintf_s(buffer, sizeof(buffer), "\\\\.\\%s", port_name); + setPortName(buffer); } bool PortHandlerWindows::openPort() { - return setBaudRate(baudrate_); + return setBaudRate(baudrate_); } void PortHandlerWindows::closePort() { - if (serial_handle_ != INVALID_HANDLE_VALUE) - { - CloseHandle(serial_handle_); - serial_handle_ = INVALID_HANDLE_VALUE; - } + if (serial_handle_ != INVALID_HANDLE_VALUE) { + CloseHandle(serial_handle_); + serial_handle_ = INVALID_HANDLE_VALUE; + } } void PortHandlerWindows::clearPort() { - PurgeComm(serial_handle_, PURGE_RXABORT | PURGE_RXCLEAR); + PurgeComm(serial_handle_, PURGE_RXABORT | PURGE_RXCLEAR); } -void PortHandlerWindows::setPortName(const char *port_name) +void PortHandlerWindows::setPortName(const char* port_name) { - strcpy_s(port_name_, sizeof(port_name_), port_name); + strcpy_s(port_name_, sizeof(port_name_), port_name); } -char *PortHandlerWindows::getPortName() +char* PortHandlerWindows::getPortName() { - return port_name_; + return port_name_; } bool PortHandlerWindows::setBaudRate(const int baudrate) { - closePort(); + closePort(); - baudrate_ = baudrate; - return setupPort(baudrate); + baudrate_ = baudrate; + return setupPort(baudrate); } int PortHandlerWindows::getBaudRate() { - return baudrate_; + return baudrate_; } int PortHandlerWindows::getBytesAvailable() { - DWORD retbyte = 2; - BOOL res = DeviceIoControl(serial_handle_, GENERIC_READ | GENERIC_WRITE, NULL, 0, 0, 0, &retbyte, (LPOVERLAPPED)NULL); + DWORD retbyte = 2; + BOOL res = DeviceIoControl(serial_handle_, GENERIC_READ | GENERIC_WRITE, NULL, 0, 0, 0, &retbyte, (LPOVERLAPPED)NULL); - printf("%d", (int)res); - return (int)retbyte; + printf("%d", (int)res); + return (int)retbyte; } -int PortHandlerWindows::readPort(uint8_t *packet, int length) +int PortHandlerWindows::readPort(uint8_t* packet, int length) { - DWORD dwRead = 0; + DWORD dwRead = 0; - if (ReadFile(serial_handle_, packet, (DWORD)length, &dwRead, NULL) == FALSE) - return -1; + if (ReadFile(serial_handle_, packet, (DWORD)length, &dwRead, NULL) == FALSE) + return -1; - return (int)dwRead; + return (int)dwRead; } -int PortHandlerWindows::writePort(uint8_t *packet, int length) +int PortHandlerWindows::writePort(uint8_t* packet, int length) { - DWORD dwWrite = 0; + DWORD dwWrite = 0; - if (WriteFile(serial_handle_, packet, (DWORD)length, &dwWrite, NULL) == FALSE) - return -1; + if (WriteFile(serial_handle_, packet, (DWORD)length, &dwWrite, NULL) == FALSE) + return -1; - return (int)dwWrite; + return (int)dwWrite; } void PortHandlerWindows::setPacketTimeout(uint16_t packet_length) { - packet_start_time_ = getCurrentTime(); - packet_timeout_ = (tx_time_per_byte_ * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0; + packet_start_time_ = getCurrentTime(); + packet_timeout_ = (tx_time_per_byte_ * (double)packet_length) + (LATENCY_TIMER * 2.0) + 2.0; } void PortHandlerWindows::setPacketTimeout(double msec) { - packet_start_time_ = getCurrentTime(); - packet_timeout_ = msec; + packet_start_time_ = getCurrentTime(); + packet_timeout_ = msec; } bool PortHandlerWindows::isPacketTimeout() { - if (getTimeSinceStart() > packet_timeout_) - { - packet_timeout_ = 0; - return true; - } - return false; + if (getTimeSinceStart() > packet_timeout_) { + packet_timeout_ = 0; + return true; + } + return false; } double PortHandlerWindows::getCurrentTime() { - QueryPerformanceCounter(&counter_); - QueryPerformanceFrequency(&freq_); - return (double)counter_.QuadPart / (double)freq_.QuadPart * 1000.0; + QueryPerformanceCounter(&counter_); + QueryPerformanceFrequency(&freq_); + return (double)counter_.QuadPart / (double)freq_.QuadPart * 1000.0; } double PortHandlerWindows::getTimeSinceStart() { - double time; + double time; - time = getCurrentTime() - packet_start_time_; - if (time < 0.0) packet_start_time_ = getCurrentTime(); + time = getCurrentTime() - packet_start_time_; + if (time < 0.0) + packet_start_time_ = getCurrentTime(); - return time; + return time; } bool PortHandlerWindows::setupPort(int baudrate) { - DCB dcb; - COMMTIMEOUTS timeouts; - DWORD dwError; - - closePort(); - - serial_handle_ = CreateFileA(port_name_, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, NULL); - if (serial_handle_ == INVALID_HANDLE_VALUE) - { - printf("[PortHandlerWindows::SetupPort] Error opening serial port!\n"); - return false; - } - - dcb.DCBlength = sizeof(DCB); - if (GetCommState(serial_handle_, &dcb) == FALSE) - goto DXL_HAL_OPEN_ERROR; - - // Set baudrate - dcb.BaudRate = (DWORD)baudrate; - dcb.ByteSize = 8; // Data bit = 8bit - dcb.Parity = NOPARITY; // No parity - dcb.StopBits = ONESTOPBIT; // Stop bit = 1 - dcb.fParity = NOPARITY; // No Parity check - dcb.fBinary = 1; // Binary mode - dcb.fNull = 0; // Get Null byte - dcb.fAbortOnError = 0; - dcb.fErrorChar = 0; - // Not using XOn/XOff - dcb.fOutX = 0; - dcb.fInX = 0; - // Not using H/W flow control - dcb.fDtrControl = DTR_CONTROL_DISABLE; - dcb.fRtsControl = RTS_CONTROL_DISABLE; - dcb.fDsrSensitivity = 0; - dcb.fOutxDsrFlow = 0; - dcb.fOutxCtsFlow = 0; - - if (SetCommState(serial_handle_, &dcb) == FALSE) - goto DXL_HAL_OPEN_ERROR; - - if (SetCommMask(serial_handle_, 0) == FALSE) // Not using Comm event - goto DXL_HAL_OPEN_ERROR; - if (SetupComm(serial_handle_, 4096, 4096) == FALSE) // Buffer size (Rx,Tx) - goto DXL_HAL_OPEN_ERROR; - if (PurgeComm(serial_handle_, PURGE_TXABORT | PURGE_TXCLEAR | PURGE_RXABORT | PURGE_RXCLEAR) == FALSE) // Clear buffer - goto DXL_HAL_OPEN_ERROR; - if (ClearCommError(serial_handle_, &dwError, NULL) == FALSE) - goto DXL_HAL_OPEN_ERROR; - - if (GetCommTimeouts(serial_handle_, &timeouts) == FALSE) - goto DXL_HAL_OPEN_ERROR; - // Timeout (Not using timeout) - // Immediatly return - timeouts.ReadIntervalTimeout = 0; - timeouts.ReadTotalTimeoutMultiplier = 0; - timeouts.ReadTotalTimeoutConstant = 1; // must not be zero. - timeouts.WriteTotalTimeoutMultiplier = 0; - timeouts.WriteTotalTimeoutConstant = 0; - if (SetCommTimeouts(serial_handle_, &timeouts) == FALSE) - goto DXL_HAL_OPEN_ERROR; - - tx_time_per_byte_ = (1000.0 / (double)baudrate_) * 10.0; - return true; + DCB dcb; + COMMTIMEOUTS timeouts; + DWORD dwError; + + closePort(); + + serial_handle_ = CreateFileA(port_name_, GENERIC_READ | GENERIC_WRITE, 0, NULL, OPEN_EXISTING, FILE_ATTRIBUTE_NORMAL, NULL); + if (serial_handle_ == INVALID_HANDLE_VALUE) { + printf("[PortHandlerWindows::SetupPort] Error opening serial port!\n"); + return false; + } + + dcb.DCBlength = sizeof(DCB); + if (GetCommState(serial_handle_, &dcb) == FALSE) + goto DXL_HAL_OPEN_ERROR; + + // Set baudrate + dcb.BaudRate = (DWORD)baudrate; + dcb.ByteSize = 8; // Data bit = 8bit + dcb.Parity = NOPARITY; // No parity + dcb.StopBits = ONESTOPBIT; // Stop bit = 1 + dcb.fParity = NOPARITY; // No Parity check + dcb.fBinary = 1; // Binary mode + dcb.fNull = 0; // Get Null byte + dcb.fAbortOnError = 0; + dcb.fErrorChar = 0; + // Not using XOn/XOff + dcb.fOutX = 0; + dcb.fInX = 0; + // Not using H/W flow control + dcb.fDtrControl = DTR_CONTROL_DISABLE; + dcb.fRtsControl = RTS_CONTROL_DISABLE; + dcb.fDsrSensitivity = 0; + dcb.fOutxDsrFlow = 0; + dcb.fOutxCtsFlow = 0; + + if (SetCommState(serial_handle_, &dcb) == FALSE) + goto DXL_HAL_OPEN_ERROR; + + if (SetCommMask(serial_handle_, 0) == FALSE) // Not using Comm event + goto DXL_HAL_OPEN_ERROR; + if (SetupComm(serial_handle_, 4096, 4096) == FALSE) // Buffer size (Rx,Tx) + goto DXL_HAL_OPEN_ERROR; + if (PurgeComm(serial_handle_, PURGE_TXABORT | PURGE_TXCLEAR | PURGE_RXABORT | PURGE_RXCLEAR) == FALSE) // Clear buffer + goto DXL_HAL_OPEN_ERROR; + if (ClearCommError(serial_handle_, &dwError, NULL) == FALSE) + goto DXL_HAL_OPEN_ERROR; + + if (GetCommTimeouts(serial_handle_, &timeouts) == FALSE) + goto DXL_HAL_OPEN_ERROR; + // Timeout (Not using timeout) + // Immediatly return + timeouts.ReadIntervalTimeout = 0; + timeouts.ReadTotalTimeoutMultiplier = 0; + timeouts.ReadTotalTimeoutConstant = 1; // must not be zero. + timeouts.WriteTotalTimeoutMultiplier = 0; + timeouts.WriteTotalTimeoutConstant = 0; + if (SetCommTimeouts(serial_handle_, &timeouts) == FALSE) + goto DXL_HAL_OPEN_ERROR; + + tx_time_per_byte_ = (1000.0 / (double)baudrate_) * 10.0; + return true; DXL_HAL_OPEN_ERROR: - closePort(); - return false; + closePort(); + return false; } #endif diff --git a/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp b/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp index 668adfa..807e250 100644 --- a/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp +++ b/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/protocol1_packet_handler.cpp @@ -1,18 +1,18 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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: zerom, Ryu Woon Jung (Leon) */ @@ -27,729 +27,688 @@ #include "../../include/dynamixel_sdk/protocol1_packet_handler.h" #endif -#include #include +#include -#define TXPACKET_MAX_LEN (250) -#define RXPACKET_MAX_LEN (250) +#define TXPACKET_MAX_LEN (250) +#define RXPACKET_MAX_LEN (250) ///////////////// for Protocol 1.0 Packet ///////////////// -#define PKT_HEADER0 0 -#define PKT_HEADER1 1 -#define PKT_ID 2 -#define PKT_LENGTH 3 -#define PKT_INSTRUCTION 4 -#define PKT_ERROR 4 -#define PKT_PARAMETER0 5 +#define PKT_HEADER0 0 +#define PKT_HEADER1 1 +#define PKT_ID 2 +#define PKT_LENGTH 3 +#define PKT_INSTRUCTION 4 +#define PKT_ERROR 4 +#define PKT_PARAMETER0 5 ///////////////// Protocol 1.0 Error bit ///////////////// -#define ERRBIT_VOLTAGE 1 // Supplied voltage is out of the range (operating volatage set in the control table) -#define ERRBIT_ANGLE 2 // Goal position is written out of the range (from CW angle limit to CCW angle limit) -#define ERRBIT_OVERHEAT 4 // Temperature is out of the range (operating temperature set in the control table) -#define ERRBIT_RANGE 8 // Command(setting value) is out of the range for use. -#define ERRBIT_CHECKSUM 16 // Instruction packet checksum is incorrect. -#define ERRBIT_OVERLOAD 32 // The current load cannot be controlled by the set torque. -#define ERRBIT_INSTRUCTION 64 // Undefined instruction or delivering the action command without the reg_write command. +#define ERRBIT_VOLTAGE 1 // Supplied voltage is out of the range (operating volatage set in the control table) +#define ERRBIT_ANGLE 2 // Goal position is written out of the range (from CW angle limit to CCW angle limit) +#define ERRBIT_OVERHEAT 4 // Temperature is out of the range (operating temperature set in the control table) +#define ERRBIT_RANGE 8 // Command(setting value) is out of the range for use. +#define ERRBIT_CHECKSUM 16 // Instruction packet checksum is incorrect. +#define ERRBIT_OVERLOAD 32 // The current load cannot be controlled by the set torque. +#define ERRBIT_INSTRUCTION 64 // Undefined instruction or delivering the action command without the reg_write command. using namespace dynamixel; -Protocol1PacketHandler *Protocol1PacketHandler::unique_instance_ = new Protocol1PacketHandler(); +Protocol1PacketHandler* Protocol1PacketHandler::unique_instance_ = new Protocol1PacketHandler(); -Protocol1PacketHandler::Protocol1PacketHandler() { } +Protocol1PacketHandler::Protocol1PacketHandler() {} -const char *Protocol1PacketHandler::getTxRxResult(int result) +const char* Protocol1PacketHandler::getTxRxResult(int result) { - switch(result) - { - case COMM_SUCCESS: - return "[TxRxResult] Communication success."; + switch (result) { + case COMM_SUCCESS: return "[TxRxResult] Communication success."; - case COMM_PORT_BUSY: - return "[TxRxResult] Port is in use!"; + case COMM_PORT_BUSY: return "[TxRxResult] Port is in use!"; - case COMM_TX_FAIL: - return "[TxRxResult] Failed transmit instruction packet!"; + case COMM_TX_FAIL: return "[TxRxResult] Failed transmit instruction packet!"; - case COMM_RX_FAIL: - return "[TxRxResult] Failed get status packet from device!"; + case COMM_RX_FAIL: return "[TxRxResult] Failed get status packet from device!"; - case COMM_TX_ERROR: - return "[TxRxResult] Incorrect instruction packet!"; + case COMM_TX_ERROR: return "[TxRxResult] Incorrect instruction packet!"; - case COMM_RX_WAITING: - return "[TxRxResult] Now recieving status packet!"; + case COMM_RX_WAITING: return "[TxRxResult] Now recieving status packet!"; - case COMM_RX_TIMEOUT: - return "[TxRxResult] There is no status packet!"; + case COMM_RX_TIMEOUT: return "[TxRxResult] There is no status packet!"; - case COMM_RX_CORRUPT: - return "[TxRxResult] Incorrect status packet!"; + case COMM_RX_CORRUPT: return "[TxRxResult] Incorrect status packet!"; - case COMM_NOT_AVAILABLE: - return "[TxRxResult] Protocol does not support This function!"; + case COMM_NOT_AVAILABLE: return "[TxRxResult] Protocol does not support This function!"; - default: - return ""; - } + default: return ""; + } } -const char *Protocol1PacketHandler::getRxPacketError(uint8_t error) +const char* Protocol1PacketHandler::getRxPacketError(uint8_t error) { - if (error & ERRBIT_VOLTAGE) - return "[RxPacketError] Input voltage error!"; + if (error & ERRBIT_VOLTAGE) + return "[RxPacketError] Input voltage error!"; - if (error & ERRBIT_ANGLE) - return "[RxPacketError] Angle limit error!"; + if (error & ERRBIT_ANGLE) + return "[RxPacketError] Angle limit error!"; - if (error & ERRBIT_OVERHEAT) - return "[RxPacketError] Overheat error!"; + if (error & ERRBIT_OVERHEAT) + return "[RxPacketError] Overheat error!"; - if (error & ERRBIT_RANGE) - return "[RxPacketError] Out of range error!"; + if (error & ERRBIT_RANGE) + return "[RxPacketError] Out of range error!"; - if (error & ERRBIT_CHECKSUM) - return "[RxPacketError] Checksum error!"; + if (error & ERRBIT_CHECKSUM) + return "[RxPacketError] Checksum error!"; - if (error & ERRBIT_OVERLOAD) - return "[RxPacketError] Overload error!"; + if (error & ERRBIT_OVERLOAD) + return "[RxPacketError] Overload error!"; - if (error & ERRBIT_INSTRUCTION) - return "[RxPacketError] Instruction code error!"; + if (error & ERRBIT_INSTRUCTION) + return "[RxPacketError] Instruction code error!"; - return ""; + return ""; } -int Protocol1PacketHandler::txPacket(PortHandler *port, uint8_t *txpacket) +int Protocol1PacketHandler::txPacket(PortHandler* port, uint8_t* txpacket) { - uint8_t checksum = 0; - uint8_t total_packet_length = txpacket[PKT_LENGTH] + 4; // 4: HEADER0 HEADER1 ID LENGTH - uint8_t written_packet_length = 0; + uint8_t checksum = 0; + uint8_t total_packet_length = txpacket[PKT_LENGTH] + 4; // 4: HEADER0 HEADER1 ID LENGTH + uint8_t written_packet_length = 0; - if (port->is_using_) - return COMM_PORT_BUSY; - port->is_using_ = true; + if (port->is_using_) + return COMM_PORT_BUSY; + port->is_using_ = true; - // check max packet length - if (total_packet_length > TXPACKET_MAX_LEN) - { - port->is_using_ = false; - return COMM_TX_ERROR; - } - - // make packet header - txpacket[PKT_HEADER0] = 0xFF; - txpacket[PKT_HEADER1] = 0xFF; - - // add a checksum to the packet - for (uint16_t idx = 2; idx < total_packet_length - 1; idx++) // except header, checksum - checksum += txpacket[idx]; - txpacket[total_packet_length - 1] = ~checksum; - - // tx packet - port->clearPort(); - written_packet_length = port->writePort(txpacket, total_packet_length); - if (total_packet_length != written_packet_length) - { - port->is_using_ = false; - return COMM_TX_FAIL; - } - - return COMM_SUCCESS; -} - -int Protocol1PacketHandler::rxPacket(PortHandler *port, uint8_t *rxpacket) -{ - int result = COMM_TX_FAIL; - - uint8_t checksum = 0; - uint8_t rx_length = 0; - uint8_t wait_length = 6; // minimum length (HEADER0 HEADER1 ID LENGTH ERROR CHKSUM) - - while(true) - { - rx_length += port->readPort(&rxpacket[rx_length], wait_length - rx_length); - if (rx_length >= wait_length) - { - uint8_t idx = 0; - - // find packet header - for (idx = 0; idx < (rx_length - 1); idx++) - { - if (rxpacket[idx] == 0xFF && rxpacket[idx+1] == 0xFF) - break; - } - - if (idx == 0) // found at the beginning of the packet - { - if (rxpacket[PKT_ID] > 0xFD || // unavailable ID - rxpacket[PKT_LENGTH] > RXPACKET_MAX_LEN || // unavailable Length - rxpacket[PKT_ERROR] > 0x7F) // unavailable Error - { - // remove the first byte in the packet - for (uint16_t s = 0; s < rx_length - 1; s++) - rxpacket[s] = rxpacket[1 + s]; - //memcpy(&rxpacket[0], &rxpacket[idx], rx_length - idx); - rx_length -= 1; - continue; - } + // check max packet length + if (total_packet_length > TXPACKET_MAX_LEN) { + port->is_using_ = false; + return COMM_TX_ERROR; + } - // re-calculate the exact length of the rx packet - if (wait_length != rxpacket[PKT_LENGTH] + PKT_LENGTH + 1) - { - wait_length = rxpacket[PKT_LENGTH] + PKT_LENGTH + 1; - continue; - } + // make packet header + txpacket[PKT_HEADER0] = 0xFF; + txpacket[PKT_HEADER1] = 0xFF; + + // add a checksum to the packet + for (uint16_t idx = 2; idx < total_packet_length - 1; idx++) // except header, checksum + checksum += txpacket[idx]; + txpacket[total_packet_length - 1] = ~checksum; + + // tx packet + port->clearPort(); + written_packet_length = port->writePort(txpacket, total_packet_length); + if (total_packet_length != written_packet_length) { + port->is_using_ = false; + return COMM_TX_FAIL; + } - if (rx_length < wait_length) - { - // check timeout - if (port->isPacketTimeout() == true) - { - if (rx_length == 0) - { - result = COMM_RX_TIMEOUT; + return COMM_SUCCESS; +} + +int Protocol1PacketHandler::rxPacket(PortHandler* port, uint8_t* rxpacket) +{ + int result = COMM_TX_FAIL; + + uint8_t checksum = 0; + uint8_t rx_length = 0; + uint8_t wait_length = 6; // minimum length (HEADER0 HEADER1 ID LENGTH ERROR CHKSUM) + + while (true) { + rx_length += port->readPort(&rxpacket[rx_length], wait_length - rx_length); + if (rx_length >= wait_length) { + uint8_t idx = 0; + + // find packet header + for (idx = 0; idx < (rx_length - 1); idx++) { + if (rxpacket[idx] == 0xFF && rxpacket[idx + 1] == 0xFF) + break; } - else + + if (idx == 0) // found at the beginning of the packet { - result = COMM_RX_CORRUPT; + if (rxpacket[PKT_ID] > 0xFD || // unavailable ID + rxpacket[PKT_LENGTH] > RXPACKET_MAX_LEN || // unavailable Length + rxpacket[PKT_ERROR] > 0x7F) // unavailable Error + { + // remove the first byte in the packet + for (uint16_t s = 0; s < rx_length - 1; s++) + rxpacket[s] = rxpacket[1 + s]; + // memcpy(&rxpacket[0], &rxpacket[idx], rx_length - idx); + rx_length -= 1; + continue; + } + + // re-calculate the exact length of the rx packet + if (wait_length != rxpacket[PKT_LENGTH] + PKT_LENGTH + 1) { + wait_length = rxpacket[PKT_LENGTH] + PKT_LENGTH + 1; + continue; + } + + if (rx_length < wait_length) { + // check timeout + if (port->isPacketTimeout() == true) { + if (rx_length == 0) { + result = COMM_RX_TIMEOUT; + } + else { + result = COMM_RX_CORRUPT; + } + break; + } + else { + continue; + } + } + + // calculate checksum + for (uint16_t i = 2; i < wait_length - 1; i++) // except header, checksum + checksum += rxpacket[i]; + checksum = ~checksum; + + // verify checksum + if (rxpacket[wait_length - 1] == checksum) { + result = COMM_SUCCESS; + } + else { + result = COMM_RX_CORRUPT; + } + break; + } + else { + // remove unnecessary packets + for (uint16_t s = 0; s < rx_length - idx; s++) + rxpacket[s] = rxpacket[idx + s]; + // memcpy(&rxpacket[0], &rxpacket[idx], rx_length - idx); + rx_length -= idx; } - break; - } - else - { - continue; - } - } - - // calculate checksum - for (uint16_t i = 2; i < wait_length - 1; i++) // except header, checksum - checksum += rxpacket[i]; - checksum = ~checksum; - - // verify checksum - if (rxpacket[wait_length - 1] == checksum) - { - result = COMM_SUCCESS; - } - else - { - result = COMM_RX_CORRUPT; - } - break; - } - else - { - // remove unnecessary packets - for (uint16_t s = 0; s < rx_length - idx; s++) - rxpacket[s] = rxpacket[idx + s]; - //memcpy(&rxpacket[0], &rxpacket[idx], rx_length - idx); - rx_length -= idx; - } - } - else - { - // check timeout - if (port->isPacketTimeout() == true) - { - if (rx_length == 0) - { - result = COMM_RX_TIMEOUT; } - else - { - result = COMM_RX_CORRUPT; + else { + // check timeout + if (port->isPacketTimeout() == true) { + if (rx_length == 0) { + result = COMM_RX_TIMEOUT; + } + else { + result = COMM_RX_CORRUPT; + } + break; + } } - break; - } } - } - port->is_using_ = false; + port->is_using_ = false; - return result; + return result; } // NOT for BulkRead instruction -int Protocol1PacketHandler::txRxPacket(PortHandler *port, uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error) +int Protocol1PacketHandler::txRxPacket(PortHandler* port, uint8_t* txpacket, uint8_t* rxpacket, uint8_t* error) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - // tx packet - result = txPacket(port, txpacket); - if (result != COMM_SUCCESS) - return result; + // tx packet + result = txPacket(port, txpacket); + if (result != COMM_SUCCESS) + return result; - // (Instruction == BulkRead) == this function is not available. - if(txpacket[PKT_INSTRUCTION] == INST_BULK_READ) - result = COMM_NOT_AVAILABLE; + // (Instruction == BulkRead) == this function is not available. + if (txpacket[PKT_INSTRUCTION] == INST_BULK_READ) + result = COMM_NOT_AVAILABLE; - // (ID == Broadcast ID) == no need to wait for status packet or not available - // (Instruction == action) == no need to wait for status packet - if (txpacket[PKT_ID] == BROADCAST_ID || txpacket[PKT_INSTRUCTION] == INST_ACTION) - { - port->is_using_ = false; - return result; - } + // (ID == Broadcast ID) == no need to wait for status packet or not available + // (Instruction == action) == no need to wait for status packet + if (txpacket[PKT_ID] == BROADCAST_ID || txpacket[PKT_INSTRUCTION] == INST_ACTION) { + port->is_using_ = false; + return result; + } - // set packet timeout - if (txpacket[PKT_INSTRUCTION] == INST_READ) - { - port->setPacketTimeout((uint16_t)(txpacket[PKT_PARAMETER0+1] + 6)); - } - else - { - port->setPacketTimeout((uint16_t)6); // HEADER0 HEADER1 ID LENGTH ERROR CHECKSUM - } + // set packet timeout + if (txpacket[PKT_INSTRUCTION] == INST_READ) { + port->setPacketTimeout((uint16_t)(txpacket[PKT_PARAMETER0 + 1] + 6)); + } + else { + port->setPacketTimeout((uint16_t)6); // HEADER0 HEADER1 ID LENGTH ERROR CHECKSUM + } - // rx packet - do { - result = rxPacket(port, rxpacket); - } while (result == COMM_SUCCESS && txpacket[PKT_ID] != rxpacket[PKT_ID]); + // rx packet + do { + result = rxPacket(port, rxpacket); + } while (result == COMM_SUCCESS && txpacket[PKT_ID] != rxpacket[PKT_ID]); - if (result == COMM_SUCCESS && txpacket[PKT_ID] == rxpacket[PKT_ID]) - { - if (error != 0) - *error = (uint8_t)rxpacket[PKT_ERROR]; - } + if (result == COMM_SUCCESS && txpacket[PKT_ID] == rxpacket[PKT_ID]) { + if (error != 0) + *error = (uint8_t)rxpacket[PKT_ERROR]; + } - return result; + return result; } -int Protocol1PacketHandler::ping(PortHandler *port, uint8_t id, uint8_t *error) +int Protocol1PacketHandler::ping(PortHandler* port, uint8_t id, uint8_t* error) { - return ping(port, id, 0, error); + return ping(port, id, 0, error); } -int Protocol1PacketHandler::ping(PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error) +int Protocol1PacketHandler::ping(PortHandler* port, uint8_t id, uint16_t* model_number, uint8_t* error) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t txpacket[6] = {0}; - uint8_t rxpacket[6] = {0}; + uint8_t txpacket[6] = { 0 }; + uint8_t rxpacket[6] = { 0 }; - if (id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; + if (id >= BROADCAST_ID) + return COMM_NOT_AVAILABLE; - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = 2; - txpacket[PKT_INSTRUCTION] = INST_PING; + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = 2; + txpacket[PKT_INSTRUCTION] = INST_PING; - result = txRxPacket(port, txpacket, rxpacket, error); - if (result == COMM_SUCCESS && model_number != 0) - { - uint8_t data_read[2] = {0}; - result = readTxRx(port, id, 0, 2, data_read); // Address 0 : Model Number - if (result == COMM_SUCCESS) *model_number = DXL_MAKEWORD(data_read[0], data_read[1]); - } + result = txRxPacket(port, txpacket, rxpacket, error); + if (result == COMM_SUCCESS && model_number != 0) { + uint8_t data_read[2] = { 0 }; + result = readTxRx(port, id, 0, 2, data_read); // Address 0 : Model Number + if (result == COMM_SUCCESS) + *model_number = DXL_MAKEWORD(data_read[0], data_read[1]); + } - return result; + return result; } -int Protocol1PacketHandler::broadcastPing(PortHandler *port, std::vector &id_list) +int Protocol1PacketHandler::broadcastPing(PortHandler* port, std::vector& id_list) { - return COMM_NOT_AVAILABLE; + return COMM_NOT_AVAILABLE; } -int Protocol1PacketHandler::action(PortHandler *port, uint8_t id) +int Protocol1PacketHandler::action(PortHandler* port, uint8_t id) { - uint8_t txpacket[6] = {0}; + uint8_t txpacket[6] = { 0 }; - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = 2; - txpacket[PKT_INSTRUCTION] = INST_ACTION; + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = 2; + txpacket[PKT_INSTRUCTION] = INST_ACTION; - return txRxPacket(port, txpacket, 0); + return txRxPacket(port, txpacket, 0); } -int Protocol1PacketHandler::reboot(PortHandler *port, uint8_t id, uint8_t *error) +int Protocol1PacketHandler::reboot(PortHandler* port, uint8_t id, uint8_t* error) { - return COMM_NOT_AVAILABLE; + return COMM_NOT_AVAILABLE; } -int Protocol1PacketHandler::clearMultiTurn(PortHandler *port, uint8_t id, uint8_t *error) +int Protocol1PacketHandler::clearMultiTurn(PortHandler* port, uint8_t id, uint8_t* error) { - return COMM_NOT_AVAILABLE; + return COMM_NOT_AVAILABLE; } -int Protocol1PacketHandler::factoryReset(PortHandler *port, uint8_t id, uint8_t option, uint8_t *error) +int Protocol1PacketHandler::factoryReset(PortHandler* port, uint8_t id, uint8_t option, uint8_t* error) { - uint8_t txpacket[6] = {0}; - uint8_t rxpacket[6] = {0}; + uint8_t txpacket[6] = { 0 }; + uint8_t rxpacket[6] = { 0 }; - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = 2; - txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET; + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = 2; + txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET; - return txRxPacket(port, txpacket, rxpacket, error); + return txRxPacket(port, txpacket, rxpacket, error); } -int Protocol1PacketHandler::readTx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length) +int Protocol1PacketHandler::readTx(PortHandler* port, uint8_t id, uint16_t address, uint16_t length) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t txpacket[8] = {0}; + uint8_t txpacket[8] = { 0 }; - if (id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; + if (id >= BROADCAST_ID) + return COMM_NOT_AVAILABLE; - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = 4; - txpacket[PKT_INSTRUCTION] = INST_READ; - txpacket[PKT_PARAMETER0+0] = (uint8_t)address; - txpacket[PKT_PARAMETER0+1] = (uint8_t)length; + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = 4; + txpacket[PKT_INSTRUCTION] = INST_READ; + txpacket[PKT_PARAMETER0 + 0] = (uint8_t)address; + txpacket[PKT_PARAMETER0 + 1] = (uint8_t)length; - result = txPacket(port, txpacket); + result = txPacket(port, txpacket); - // set packet timeout - if (result == COMM_SUCCESS) - port->setPacketTimeout((uint16_t)(length+6)); + // set packet timeout + if (result == COMM_SUCCESS) + port->setPacketTimeout((uint16_t)(length + 6)); - return result; + return result; } -int Protocol1PacketHandler::readRx(PortHandler *port, uint8_t id, uint16_t length, uint8_t *data, uint8_t *error) +int Protocol1PacketHandler::readRx(PortHandler* port, uint8_t id, uint16_t length, uint8_t* data, uint8_t* error) { - int result = COMM_TX_FAIL; - uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); //(length+6); - //uint8_t *rxpacket = new uint8_t[length+6]; + int result = COMM_TX_FAIL; + uint8_t* rxpacket = (uint8_t*)malloc(RXPACKET_MAX_LEN); //(length+6); + // uint8_t *rxpacket = new uint8_t[length+6]; - if (rxpacket == NULL) - return result; + if (rxpacket == NULL) + return result; - do { - result = rxPacket(port, rxpacket); - } while (result == COMM_SUCCESS && rxpacket[PKT_ID] != id); + do { + result = rxPacket(port, rxpacket); + } while (result == COMM_SUCCESS && rxpacket[PKT_ID] != id); - if (result == COMM_SUCCESS && rxpacket[PKT_ID] == id) - { - if (error != 0) - { - *error = (uint8_t)rxpacket[PKT_ERROR]; - } - for (uint16_t s = 0; s < length; s++) - { - data[s] = rxpacket[PKT_PARAMETER0 + s]; + if (result == COMM_SUCCESS && rxpacket[PKT_ID] == id) { + if (error != 0) { + *error = (uint8_t)rxpacket[PKT_ERROR]; + } + for (uint16_t s = 0; s < length; s++) { + data[s] = rxpacket[PKT_PARAMETER0 + s]; + } + // memcpy(data, &rxpacket[PKT_PARAMETER0], length); } - //memcpy(data, &rxpacket[PKT_PARAMETER0], length); - } - free(rxpacket); - //delete[] rxpacket; - return result; + free(rxpacket); + // delete[] rxpacket; + return result; } -int Protocol1PacketHandler::readTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) +int Protocol1PacketHandler::readTxRx(PortHandler* port, uint8_t id, uint16_t address, uint16_t length, uint8_t* data, uint8_t* error) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t txpacket[8] = {0}; - uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN);//(length+6); + uint8_t txpacket[8] = { 0 }; + uint8_t* rxpacket = (uint8_t*)malloc(RXPACKET_MAX_LEN); //(length+6); - if (rxpacket == NULL) - return result; + if (rxpacket == NULL) + return result; - if (id >= BROADCAST_ID) - { - free(rxpacket); - return COMM_NOT_AVAILABLE; - } - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = 4; - txpacket[PKT_INSTRUCTION] = INST_READ; - txpacket[PKT_PARAMETER0+0] = (uint8_t)address; - txpacket[PKT_PARAMETER0+1] = (uint8_t)length; - - result = txRxPacket(port, txpacket, rxpacket, error); - if (result == COMM_SUCCESS) - { - if (error != 0) - { - *error = (uint8_t)rxpacket[PKT_ERROR]; + if (id >= BROADCAST_ID) { + free(rxpacket); + return COMM_NOT_AVAILABLE; } - for (uint16_t s = 0; s < length; s++) - { - data[s] = rxpacket[PKT_PARAMETER0 + s]; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = 4; + txpacket[PKT_INSTRUCTION] = INST_READ; + txpacket[PKT_PARAMETER0 + 0] = (uint8_t)address; + txpacket[PKT_PARAMETER0 + 1] = (uint8_t)length; + + result = txRxPacket(port, txpacket, rxpacket, error); + if (result == COMM_SUCCESS) { + if (error != 0) { + *error = (uint8_t)rxpacket[PKT_ERROR]; + } + for (uint16_t s = 0; s < length; s++) { + data[s] = rxpacket[PKT_PARAMETER0 + s]; + } + // memcpy(data, &rxpacket[PKT_PARAMETER0], length); } - //memcpy(data, &rxpacket[PKT_PARAMETER0], length); - } - free(rxpacket); - //delete[] rxpacket; - return result; + free(rxpacket); + // delete[] rxpacket; + return result; } -int Protocol1PacketHandler::read1ByteTx(PortHandler *port, uint8_t id, uint16_t address) +int Protocol1PacketHandler::read1ByteTx(PortHandler* port, uint8_t id, uint16_t address) { - return readTx(port, id, address, 1); + return readTx(port, id, address, 1); } -int Protocol1PacketHandler::read1ByteRx(PortHandler *port, uint8_t id, uint8_t *data, uint8_t *error) +int Protocol1PacketHandler::read1ByteRx(PortHandler* port, uint8_t id, uint8_t* data, uint8_t* error) { - uint8_t data_read[1] = {0}; - int result = readRx(port, id, 1, data_read, error); - if (result == COMM_SUCCESS) - *data = data_read[0]; - return result; + uint8_t data_read[1] = { 0 }; + int result = readRx(port, id, 1, data_read, error); + if (result == COMM_SUCCESS) + *data = data_read[0]; + return result; } -int Protocol1PacketHandler::read1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error) +int Protocol1PacketHandler::read1ByteTxRx(PortHandler* port, uint8_t id, uint16_t address, uint8_t* data, uint8_t* error) { - uint8_t data_read[1] = {0}; - int result = readTxRx(port, id, address, 1, data_read, error); - if (result == COMM_SUCCESS) - *data = data_read[0]; - return result; + uint8_t data_read[1] = { 0 }; + int result = readTxRx(port, id, address, 1, data_read, error); + if (result == COMM_SUCCESS) + *data = data_read[0]; + return result; } -int Protocol1PacketHandler::read2ByteTx(PortHandler *port, uint8_t id, uint16_t address) +int Protocol1PacketHandler::read2ByteTx(PortHandler* port, uint8_t id, uint16_t address) { - return readTx(port, id, address, 2); + return readTx(port, id, address, 2); } -int Protocol1PacketHandler::read2ByteRx(PortHandler *port, uint8_t id, uint16_t *data, uint8_t *error) +int Protocol1PacketHandler::read2ByteRx(PortHandler* port, uint8_t id, uint16_t* data, uint8_t* error) { - uint8_t data_read[2] = {0}; - int result = readRx(port, id, 2, data_read, error); - if (result == COMM_SUCCESS) - *data = DXL_MAKEWORD(data_read[0], data_read[1]); - return result; + uint8_t data_read[2] = { 0 }; + int result = readRx(port, id, 2, data_read, error); + if (result == COMM_SUCCESS) + *data = DXL_MAKEWORD(data_read[0], data_read[1]); + return result; } -int Protocol1PacketHandler::read2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error) +int Protocol1PacketHandler::read2ByteTxRx(PortHandler* port, uint8_t id, uint16_t address, uint16_t* data, uint8_t* error) { - uint8_t data_read[2] = {0}; - int result = readTxRx(port, id, address, 2, data_read, error); - if (result == COMM_SUCCESS) - *data = DXL_MAKEWORD(data_read[0], data_read[1]); - return result; + uint8_t data_read[2] = { 0 }; + int result = readTxRx(port, id, address, 2, data_read, error); + if (result == COMM_SUCCESS) + *data = DXL_MAKEWORD(data_read[0], data_read[1]); + return result; } -int Protocol1PacketHandler::read4ByteTx(PortHandler *port, uint8_t id, uint16_t address) +int Protocol1PacketHandler::read4ByteTx(PortHandler* port, uint8_t id, uint16_t address) { - return readTx(port, id, address, 4); + return readTx(port, id, address, 4); } -int Protocol1PacketHandler::read4ByteRx(PortHandler *port, uint8_t id, uint32_t *data, uint8_t *error) +int Protocol1PacketHandler::read4ByteRx(PortHandler* port, uint8_t id, uint32_t* data, uint8_t* error) { - uint8_t data_read[4] = {0}; - int result = readRx(port, id, 4, data_read, error); - if (result == COMM_SUCCESS) - *data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3])); - return result; + uint8_t data_read[4] = { 0 }; + int result = readRx(port, id, 4, data_read, error); + if (result == COMM_SUCCESS) + *data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3])); + return result; } -int Protocol1PacketHandler::read4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error) +int Protocol1PacketHandler::read4ByteTxRx(PortHandler* port, uint8_t id, uint16_t address, uint32_t* data, uint8_t* error) { - uint8_t data_read[4] = {0}; - int result = readTxRx(port, id, address, 4, data_read, error); - if (result == COMM_SUCCESS) - *data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3])); - return result; + uint8_t data_read[4] = { 0 }; + int result = readTxRx(port, id, address, 4, data_read, error); + if (result == COMM_SUCCESS) + *data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3])); + return result; } -int Protocol1PacketHandler::writeTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) +int Protocol1PacketHandler::writeTxOnly(PortHandler* port, uint8_t id, uint16_t address, uint16_t length, uint8_t* data) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length+7); - //uint8_t *txpacket = new uint8_t[length+7]; + uint8_t* txpacket = (uint8_t*)malloc(length + 7); + // uint8_t *txpacket = new uint8_t[length+7]; - if (txpacket == NULL) - return result; + if (txpacket == NULL) + return result; - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = length+3; - txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0] = (uint8_t)address; + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = length + 3; + txpacket[PKT_INSTRUCTION] = INST_WRITE; + txpacket[PKT_PARAMETER0] = (uint8_t)address; - for (uint16_t s = 0; s < length; s++) - txpacket[PKT_PARAMETER0+1+s] = data[s]; - //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); + for (uint16_t s = 0; s < length; s++) + txpacket[PKT_PARAMETER0 + 1 + s] = data[s]; + // memcpy(&txpacket[PKT_PARAMETER0+1], data, length); - result = txPacket(port, txpacket); - port->is_using_ = false; + result = txPacket(port, txpacket); + port->is_using_ = false; - free(txpacket); - //delete[] txpacket; - return result; + free(txpacket); + // delete[] txpacket; + return result; } -int Protocol1PacketHandler::writeTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) +int Protocol1PacketHandler::writeTxRx(PortHandler* port, uint8_t id, uint16_t address, uint16_t length, uint8_t* data, uint8_t* error) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length+7); //#6->7 - //uint8_t *txpacket = new uint8_t[length+7]; - uint8_t rxpacket[6] = {0}; + uint8_t* txpacket = (uint8_t*)malloc(length + 7); //#6->7 + // uint8_t *txpacket = new uint8_t[length+7]; + uint8_t rxpacket[6] = { 0 }; - if (txpacket == NULL) - return result; + if (txpacket == NULL) + return result; - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = length+3; - txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0] = (uint8_t)address; + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = length + 3; + txpacket[PKT_INSTRUCTION] = INST_WRITE; + txpacket[PKT_PARAMETER0] = (uint8_t)address; - for (uint16_t s = 0; s < length; s++) - txpacket[PKT_PARAMETER0+1+s] = data[s]; - //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); + for (uint16_t s = 0; s < length; s++) + txpacket[PKT_PARAMETER0 + 1 + s] = data[s]; + // memcpy(&txpacket[PKT_PARAMETER0+1], data, length); - result = txRxPacket(port, txpacket, rxpacket, error); + result = txRxPacket(port, txpacket, rxpacket, error); - free(txpacket); - //delete[] txpacket; - return result; + free(txpacket); + // delete[] txpacket; + return result; } -int Protocol1PacketHandler::write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data) +int Protocol1PacketHandler::write1ByteTxOnly(PortHandler* port, uint8_t id, uint16_t address, uint8_t data) { - uint8_t data_write[1] = { data }; - return writeTxOnly(port, id, address, 1, data_write); + uint8_t data_write[1] = { data }; + return writeTxOnly(port, id, address, 1, data_write); } -int Protocol1PacketHandler::write1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error) +int Protocol1PacketHandler::write1ByteTxRx(PortHandler* port, uint8_t id, uint16_t address, uint8_t data, uint8_t* error) { - uint8_t data_write[1] = { data }; - return writeTxRx(port, id, address, 1, data_write, error); + uint8_t data_write[1] = { data }; + return writeTxRx(port, id, address, 1, data_write, error); } -int Protocol1PacketHandler::write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data) +int Protocol1PacketHandler::write2ByteTxOnly(PortHandler* port, uint8_t id, uint16_t address, uint16_t data) { - uint8_t data_write[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; - return writeTxOnly(port, id, address, 2, data_write); + uint8_t data_write[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; + return writeTxOnly(port, id, address, 2, data_write); } -int Protocol1PacketHandler::write2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error) +int Protocol1PacketHandler::write2ByteTxRx(PortHandler* port, uint8_t id, uint16_t address, uint16_t data, uint8_t* error) { - uint8_t data_write[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; - return writeTxRx(port, id, address, 2, data_write, error); + uint8_t data_write[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; + return writeTxRx(port, id, address, 2, data_write, error); } -int Protocol1PacketHandler::write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data) +int Protocol1PacketHandler::write4ByteTxOnly(PortHandler* port, uint8_t id, uint16_t address, uint32_t data) { - uint8_t data_write[4] = { DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data)) }; - return writeTxOnly(port, id, address, 4, data_write); + uint8_t data_write[4] = { DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data)) }; + return writeTxOnly(port, id, address, 4, data_write); } -int Protocol1PacketHandler::write4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error) +int Protocol1PacketHandler::write4ByteTxRx(PortHandler* port, uint8_t id, uint16_t address, uint32_t data, uint8_t* error) { - uint8_t data_write[4] = { DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data)) }; - return writeTxRx(port, id, address, 4, data_write, error); + uint8_t data_write[4] = { DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data)) }; + return writeTxRx(port, id, address, 4, data_write, error); } -int Protocol1PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) +int Protocol1PacketHandler::regWriteTxOnly(PortHandler* port, uint8_t id, uint16_t address, uint16_t length, uint8_t* data) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length+6); - //uint8_t *txpacket = new uint8_t[length+6]; + uint8_t* txpacket = (uint8_t*)malloc(length + 6); + // uint8_t *txpacket = new uint8_t[length+6]; - if (txpacket == NULL) - return result; + if (txpacket == NULL) + return result; - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = length+3; - txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0] = (uint8_t)address; + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = length + 3; + txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; + txpacket[PKT_PARAMETER0] = (uint8_t)address; - for (uint16_t s = 0; s < length; s++) - txpacket[PKT_PARAMETER0+1+s] = data[s]; - //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); + for (uint16_t s = 0; s < length; s++) + txpacket[PKT_PARAMETER0 + 1 + s] = data[s]; + // memcpy(&txpacket[PKT_PARAMETER0+1], data, length); - result = txPacket(port, txpacket); - port->is_using_ = false; + result = txPacket(port, txpacket); + port->is_using_ = false; - free(txpacket); - //delete[] txpacket; - return result; + free(txpacket); + // delete[] txpacket; + return result; } -int Protocol1PacketHandler::regWriteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) +int Protocol1PacketHandler::regWriteTxRx(PortHandler* port, uint8_t id, uint16_t address, uint16_t length, uint8_t* data, uint8_t* error) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length+6); - //uint8_t *txpacket = new uint8_t[length+6]; - uint8_t rxpacket[6] = {0}; + uint8_t* txpacket = (uint8_t*)malloc(length + 6); + // uint8_t *txpacket = new uint8_t[length+6]; + uint8_t rxpacket[6] = { 0 }; - if (txpacket == NULL) - return result; + if (txpacket == NULL) + return result; - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH] = length+3; - txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0] = (uint8_t)address; + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH] = length + 3; + txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; + txpacket[PKT_PARAMETER0] = (uint8_t)address; - for (uint16_t s = 0; s < length; s++) - txpacket[PKT_PARAMETER0+1+s] = data[s]; - //memcpy(&txpacket[PKT_PARAMETER0+1], data, length); + for (uint16_t s = 0; s < length; s++) + txpacket[PKT_PARAMETER0 + 1 + s] = data[s]; + // memcpy(&txpacket[PKT_PARAMETER0+1], data, length); - result = txRxPacket(port, txpacket, rxpacket, error); + result = txRxPacket(port, txpacket, rxpacket, error); - free(txpacket); - //delete[] txpacket; - return result; + free(txpacket); + // delete[] txpacket; + return result; } -int Protocol1PacketHandler::syncReadTx(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) +int Protocol1PacketHandler::syncReadTx(PortHandler* port, uint16_t start_address, uint16_t data_length, uint8_t* param, uint16_t param_length) { - return COMM_NOT_AVAILABLE; + return COMM_NOT_AVAILABLE; } -int Protocol1PacketHandler::syncWriteTxOnly(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) +int Protocol1PacketHandler::syncWriteTxOnly(PortHandler* port, uint16_t start_address, uint16_t data_length, uint8_t* param, uint16_t param_length) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(param_length+8); - // 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM - //uint8_t *txpacket = new uint8_t[param_length + 8]; + uint8_t* txpacket = (uint8_t*)malloc(param_length + 8); + // 8: HEADER0 HEADER1 ID LEN INST START_ADDR DATA_LEN ... CHKSUM + // uint8_t *txpacket = new uint8_t[param_length + 8]; - if (txpacket == NULL) - return result; + if (txpacket == NULL) + return result; - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH] = param_length + 4; // 4: INST START_ADDR DATA_LEN ... CHKSUM - txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE; - txpacket[PKT_PARAMETER0+0] = start_address; - txpacket[PKT_PARAMETER0+1] = data_length; + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH] = param_length + 4; // 4: INST START_ADDR DATA_LEN ... CHKSUM + txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE; + txpacket[PKT_PARAMETER0 + 0] = start_address; + txpacket[PKT_PARAMETER0 + 1] = data_length; - for (uint16_t s = 0; s < param_length; s++) - txpacket[PKT_PARAMETER0+2+s] = param[s]; - //memcpy(&txpacket[PKT_PARAMETER0+2], param, param_length); + for (uint16_t s = 0; s < param_length; s++) + txpacket[PKT_PARAMETER0 + 2 + s] = param[s]; + // memcpy(&txpacket[PKT_PARAMETER0+2], param, param_length); - result = txRxPacket(port, txpacket, 0, 0); + result = txRxPacket(port, txpacket, 0, 0); - free(txpacket); - //delete[] txpacket; - return result; + free(txpacket); + // delete[] txpacket; + return result; } -int Protocol1PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, uint16_t param_length) +int Protocol1PacketHandler::bulkReadTx(PortHandler* port, uint8_t* param, uint16_t param_length) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(param_length+7); - // 7: HEADER0 HEADER1 ID LEN INST 0x00 ... CHKSUM - //uint8_t *txpacket = new uint8_t[param_length + 7]; + uint8_t* txpacket = (uint8_t*)malloc(param_length + 7); + // 7: HEADER0 HEADER1 ID LEN INST 0x00 ... CHKSUM + // uint8_t *txpacket = new uint8_t[param_length + 7]; - if (txpacket == NULL) - return result; + if (txpacket == NULL) + return result; - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH] = param_length + 3; // 3: INST 0x00 ... CHKSUM - txpacket[PKT_INSTRUCTION] = INST_BULK_READ; - txpacket[PKT_PARAMETER0+0] = 0x00; + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH] = param_length + 3; // 3: INST 0x00 ... CHKSUM + txpacket[PKT_INSTRUCTION] = INST_BULK_READ; + txpacket[PKT_PARAMETER0 + 0] = 0x00; - for (uint16_t s = 0; s < param_length; s++) - txpacket[PKT_PARAMETER0+1+s] = param[s]; - //memcpy(&txpacket[PKT_PARAMETER0+1], param, param_length); + for (uint16_t s = 0; s < param_length; s++) + txpacket[PKT_PARAMETER0 + 1 + s] = param[s]; + // memcpy(&txpacket[PKT_PARAMETER0+1], param, param_length); - result = txPacket(port, txpacket); - if (result == COMM_SUCCESS) - { - int wait_length = 0; - for (uint16_t i = 0; i < param_length; i += 3) - wait_length += param[i] + 7; - port->setPacketTimeout((uint16_t)wait_length); - } + result = txPacket(port, txpacket); + if (result == COMM_SUCCESS) { + int wait_length = 0; + for (uint16_t i = 0; i < param_length; i += 3) + wait_length += param[i] + 7; + port->setPacketTimeout((uint16_t)wait_length); + } - free(txpacket); - //delete[] txpacket; - return result; + free(txpacket); + // delete[] txpacket; + return result; } -int Protocol1PacketHandler::bulkWriteTxOnly(PortHandler *port, uint8_t *param, uint16_t param_length) +int Protocol1PacketHandler::bulkWriteTxOnly(PortHandler* port, uint8_t* param, uint16_t param_length) { - return COMM_NOT_AVAILABLE; + return COMM_NOT_AVAILABLE; } diff --git a/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp b/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp old mode 100755 new mode 100644 index 06dae1d..bc7ffe4 --- a/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp +++ b/DynamixelSDK/dynamixel_sdk/src/dynamixel_sdk/protocol2_packet_handler.cpp @@ -1,1078 +1,985 @@ /******************************************************************************* -* Copyright 2017 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. -*******************************************************************************/ + * Copyright 2017 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: zerom, Ryu Woon Jung (Leon) */ #if defined(__linux__) -#include #include "protocol2_packet_handler.h" -#elif defined(__APPLE__) #include +#elif defined(__APPLE__) #include "protocol2_packet_handler.h" +#include #elif defined(_WIN32) || defined(_WIN64) #define WINDLLEXPORT -#include #include "protocol2_packet_handler.h" +#include #elif defined(ARDUINO) || defined(__OPENCR__) || defined(__OPENCM904__) #include "../../include/dynamixel_sdk/protocol2_packet_handler.h" #endif #include -#include #include +#include -#define TXPACKET_MAX_LEN (1*1024) -#define RXPACKET_MAX_LEN (1*1024) +#define TXPACKET_MAX_LEN (1 * 1024) +#define RXPACKET_MAX_LEN (1 * 1024) ///////////////// for Protocol 2.0 Packet ///////////////// -#define PKT_HEADER0 0 -#define PKT_HEADER1 1 -#define PKT_HEADER2 2 -#define PKT_RESERVED 3 -#define PKT_ID 4 -#define PKT_LENGTH_L 5 -#define PKT_LENGTH_H 6 -#define PKT_INSTRUCTION 7 -#define PKT_ERROR 8 -#define PKT_PARAMETER0 8 +#define PKT_HEADER0 0 +#define PKT_HEADER1 1 +#define PKT_HEADER2 2 +#define PKT_RESERVED 3 +#define PKT_ID 4 +#define PKT_LENGTH_L 5 +#define PKT_LENGTH_H 6 +#define PKT_INSTRUCTION 7 +#define PKT_ERROR 8 +#define PKT_PARAMETER0 8 ///////////////// Protocol 2.0 Error bit ///////////////// -#define ERRNUM_RESULT_FAIL 1 // Failed to process the instruction packet. -#define ERRNUM_INSTRUCTION 2 // Instruction error -#define ERRNUM_CRC 3 // CRC check error -#define ERRNUM_DATA_RANGE 4 // Data range error -#define ERRNUM_DATA_LENGTH 5 // Data length error -#define ERRNUM_DATA_LIMIT 6 // Data limit error -#define ERRNUM_ACCESS 7 // Access error +#define ERRNUM_RESULT_FAIL 1 // Failed to process the instruction packet. +#define ERRNUM_INSTRUCTION 2 // Instruction error +#define ERRNUM_CRC 3 // CRC check error +#define ERRNUM_DATA_RANGE 4 // Data range error +#define ERRNUM_DATA_LENGTH 5 // Data length error +#define ERRNUM_DATA_LIMIT 6 // Data limit error +#define ERRNUM_ACCESS 7 // Access error -#define ERRBIT_ALERT 128 //When the device has a problem, this bit is set to 1. Check "Device Status Check" value. +#define ERRBIT_ALERT 128 // When the device has a problem, this bit is set to 1. Check "Device Status Check" value. using namespace dynamixel; -Protocol2PacketHandler *Protocol2PacketHandler::unique_instance_ = new Protocol2PacketHandler(); +Protocol2PacketHandler* Protocol2PacketHandler::unique_instance_ = new Protocol2PacketHandler(); -Protocol2PacketHandler::Protocol2PacketHandler() { } +Protocol2PacketHandler::Protocol2PacketHandler() {} -const char *Protocol2PacketHandler::getTxRxResult(int result) +const char* Protocol2PacketHandler::getTxRxResult(int result) { - switch(result) - { - case COMM_SUCCESS: - return "[TxRxResult] Communication success."; + switch (result) { + case COMM_SUCCESS: return "[TxRxResult] Communication success."; - case COMM_PORT_BUSY: - return "[TxRxResult] Port is in use!"; + case COMM_PORT_BUSY: return "[TxRxResult] Port is in use!"; - case COMM_TX_FAIL: - return "[TxRxResult] Failed transmit instruction packet!"; + case COMM_TX_FAIL: return "[TxRxResult] Failed transmit instruction packet!"; - case COMM_RX_FAIL: - return "[TxRxResult] Failed get status packet from device!"; + case COMM_RX_FAIL: return "[TxRxResult] Failed get status packet from device!"; - case COMM_TX_ERROR: - return "[TxRxResult] Incorrect instruction packet!"; + case COMM_TX_ERROR: return "[TxRxResult] Incorrect instruction packet!"; - case COMM_RX_WAITING: - return "[TxRxResult] Now recieving status packet!"; + case COMM_RX_WAITING: return "[TxRxResult] Now recieving status packet!"; - case COMM_RX_TIMEOUT: - return "[TxRxResult] There is no status packet!"; + case COMM_RX_TIMEOUT: return "[TxRxResult] There is no status packet!"; - case COMM_RX_CORRUPT: - return "[TxRxResult] Incorrect status packet!"; + case COMM_RX_CORRUPT: return "[TxRxResult] Incorrect status packet!"; - case COMM_NOT_AVAILABLE: - return "[TxRxResult] Protocol does not support This function!"; + case COMM_NOT_AVAILABLE: return "[TxRxResult] Protocol does not support This function!"; - default: - return ""; - } + default: return ""; + } } -const char *Protocol2PacketHandler::getRxPacketError(uint8_t error) +const char* Protocol2PacketHandler::getRxPacketError(uint8_t error) { - if (error & ERRBIT_ALERT) - return "[RxPacketError] Hardware error occurred. Check the error at Control Table (Hardware Error Status)!"; + if (error & ERRBIT_ALERT) + return "[RxPacketError] Hardware error occurred. Check the error at Control Table (Hardware Error Status)!"; - int not_alert_error = error & ~ERRBIT_ALERT; + int not_alert_error = error & ~ERRBIT_ALERT; - switch(not_alert_error) - { - case 0: - return ""; + switch (not_alert_error) { + case 0: return ""; - case ERRNUM_RESULT_FAIL: - return "[RxPacketError] Failed to process the instruction packet!"; + case ERRNUM_RESULT_FAIL: return "[RxPacketError] Failed to process the instruction packet!"; - case ERRNUM_INSTRUCTION: - return "[RxPacketError] Undefined instruction or incorrect instruction!"; + case ERRNUM_INSTRUCTION: return "[RxPacketError] Undefined instruction or incorrect instruction!"; - case ERRNUM_CRC: - return "[RxPacketError] CRC doesn't match!"; + case ERRNUM_CRC: return "[RxPacketError] CRC doesn't match!"; - case ERRNUM_DATA_RANGE: - return "[RxPacketError] The data value is out of range!"; + case ERRNUM_DATA_RANGE: return "[RxPacketError] The data value is out of range!"; - case ERRNUM_DATA_LENGTH: - return "[RxPacketError] The data length does not match as expected!"; + case ERRNUM_DATA_LENGTH: return "[RxPacketError] The data length does not match as expected!"; - case ERRNUM_DATA_LIMIT: - return "[RxPacketError] The data value exceeds the limit value!"; + case ERRNUM_DATA_LIMIT: return "[RxPacketError] The data value exceeds the limit value!"; - case ERRNUM_ACCESS: - return "[RxPacketError] Writing or Reading is not available to target address!"; + case ERRNUM_ACCESS: return "[RxPacketError] Writing or Reading is not available to target address!"; - default: - return "[RxPacketError] Unknown error code!"; - } + default: return "[RxPacketError] Unknown error code!"; + } } -unsigned short Protocol2PacketHandler::updateCRC(uint16_t crc_accum, uint8_t *data_blk_ptr, uint16_t data_blk_size) -{ - uint16_t i; - static const uint16_t crc_table[256] = {0x0000, - 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011, - 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, - 0x0022, 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, - 0x8077, 0x0072, 0x0050, 0x8055, 0x805F, 0x005A, 0x804B, - 0x004E, 0x0044, 0x8041, 0x80C3, 0x00C6, 0x00CC, 0x80C9, - 0x00D8, 0x80DD, 0x80D7, 0x00D2, 0x00F0, 0x80F5, 0x80FF, - 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1, 0x00A0, 0x80A5, - 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1, 0x8093, - 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082, - 0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, - 0x0192, 0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, - 0x01A4, 0x81A1, 0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, - 0x01FE, 0x01F4, 0x81F1, 0x81D3, 0x01D6, 0x01DC, 0x81D9, - 0x01C8, 0x81CD, 0x81C7, 0x01C2, 0x0140, 0x8145, 0x814F, - 0x014A, 0x815B, 0x015E, 0x0154, 0x8151, 0x8173, 0x0176, - 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162, 0x8123, - 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132, - 0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, - 0x8101, 0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, - 0x8317, 0x0312, 0x0330, 0x8335, 0x833F, 0x033A, 0x832B, - 0x032E, 0x0324, 0x8321, 0x0360, 0x8365, 0x836F, 0x036A, - 0x837B, 0x037E, 0x0374, 0x8371, 0x8353, 0x0356, 0x035C, - 0x8359, 0x0348, 0x834D, 0x8347, 0x0342, 0x03C0, 0x83C5, - 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1, 0x83F3, - 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2, - 0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, - 0x03B2, 0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, - 0x0384, 0x8381, 0x0280, 0x8285, 0x828F, 0x028A, 0x829B, - 0x029E, 0x0294, 0x8291, 0x82B3, 0x02B6, 0x02BC, 0x82B9, - 0x02A8, 0x82AD, 0x82A7, 0x02A2, 0x82E3, 0x02E6, 0x02EC, - 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2, 0x02D0, 0x82D5, - 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1, 0x8243, - 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252, - 0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, - 0x8261, 0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, - 0x0234, 0x8231, 0x8213, 0x0216, 0x021C, 0x8219, 0x0208, - 0x820D, 0x8207, 0x0202 }; - - for (uint16_t j = 0; j < data_blk_size; j++) - { - i = ((uint16_t)(crc_accum >> 8) ^ *data_blk_ptr++) & 0xFF; - crc_accum = (crc_accum << 8) ^ crc_table[i]; - } - - return crc_accum; +unsigned short Protocol2PacketHandler::updateCRC(uint16_t crc_accum, uint8_t* data_blk_ptr, uint16_t data_blk_size) +{ + uint16_t i; + static const uint16_t crc_table[256] = { + 0x0000, 0x8005, 0x800F, 0x000A, 0x801B, 0x001E, 0x0014, 0x8011, 0x8033, 0x0036, 0x003C, 0x8039, 0x0028, 0x802D, 0x8027, 0x0022, 0x8063, 0x0066, 0x006C, 0x8069, 0x0078, 0x807D, 0x8077, 0x0072, + 0x0050, 0x8055, 0x805F, 0x005A, 0x804B, 0x004E, 0x0044, 0x8041, 0x80C3, 0x00C6, 0x00CC, 0x80C9, 0x00D8, 0x80DD, 0x80D7, 0x00D2, 0x00F0, 0x80F5, 0x80FF, 0x00FA, 0x80EB, 0x00EE, 0x00E4, 0x80E1, + 0x00A0, 0x80A5, 0x80AF, 0x00AA, 0x80BB, 0x00BE, 0x00B4, 0x80B1, 0x8093, 0x0096, 0x009C, 0x8099, 0x0088, 0x808D, 0x8087, 0x0082, 0x8183, 0x0186, 0x018C, 0x8189, 0x0198, 0x819D, 0x8197, 0x0192, + 0x01B0, 0x81B5, 0x81BF, 0x01BA, 0x81AB, 0x01AE, 0x01A4, 0x81A1, 0x01E0, 0x81E5, 0x81EF, 0x01EA, 0x81FB, 0x01FE, 0x01F4, 0x81F1, 0x81D3, 0x01D6, 0x01DC, 0x81D9, 0x01C8, 0x81CD, 0x81C7, 0x01C2, + 0x0140, 0x8145, 0x814F, 0x014A, 0x815B, 0x015E, 0x0154, 0x8151, 0x8173, 0x0176, 0x017C, 0x8179, 0x0168, 0x816D, 0x8167, 0x0162, 0x8123, 0x0126, 0x012C, 0x8129, 0x0138, 0x813D, 0x8137, 0x0132, + 0x0110, 0x8115, 0x811F, 0x011A, 0x810B, 0x010E, 0x0104, 0x8101, 0x8303, 0x0306, 0x030C, 0x8309, 0x0318, 0x831D, 0x8317, 0x0312, 0x0330, 0x8335, 0x833F, 0x033A, 0x832B, 0x032E, 0x0324, 0x8321, + 0x0360, 0x8365, 0x836F, 0x036A, 0x837B, 0x037E, 0x0374, 0x8371, 0x8353, 0x0356, 0x035C, 0x8359, 0x0348, 0x834D, 0x8347, 0x0342, 0x03C0, 0x83C5, 0x83CF, 0x03CA, 0x83DB, 0x03DE, 0x03D4, 0x83D1, + 0x83F3, 0x03F6, 0x03FC, 0x83F9, 0x03E8, 0x83ED, 0x83E7, 0x03E2, 0x83A3, 0x03A6, 0x03AC, 0x83A9, 0x03B8, 0x83BD, 0x83B7, 0x03B2, 0x0390, 0x8395, 0x839F, 0x039A, 0x838B, 0x038E, 0x0384, 0x8381, + 0x0280, 0x8285, 0x828F, 0x028A, 0x829B, 0x029E, 0x0294, 0x8291, 0x82B3, 0x02B6, 0x02BC, 0x82B9, 0x02A8, 0x82AD, 0x82A7, 0x02A2, 0x82E3, 0x02E6, 0x02EC, 0x82E9, 0x02F8, 0x82FD, 0x82F7, 0x02F2, + 0x02D0, 0x82D5, 0x82DF, 0x02DA, 0x82CB, 0x02CE, 0x02C4, 0x82C1, 0x8243, 0x0246, 0x024C, 0x8249, 0x0258, 0x825D, 0x8257, 0x0252, 0x0270, 0x8275, 0x827F, 0x027A, 0x826B, 0x026E, 0x0264, 0x8261, + 0x0220, 0x8225, 0x822F, 0x022A, 0x823B, 0x023E, 0x0234, 0x8231, 0x8213, 0x0216, 0x021C, 0x8219, 0x0208, 0x820D, 0x8207, 0x0202 + }; + + for (uint16_t j = 0; j < data_blk_size; j++) { + i = ((uint16_t)(crc_accum >> 8) ^ *data_blk_ptr++) & 0xFF; + crc_accum = (crc_accum << 8) ^ crc_table[i]; + } + + return crc_accum; } -void Protocol2PacketHandler::addStuffing(uint8_t *packet) +void Protocol2PacketHandler::addStuffing(uint8_t* packet) { - int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]); - int packet_length_out = packet_length_in; - - if (packet_length_in < 8) // INSTRUCTION, ADDR_L, ADDR_H, CRC16_L, CRC16_H + FF FF FD - return; + int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]); + int packet_length_out = packet_length_in; - uint8_t *packet_ptr; - uint16_t packet_length_before_crc = packet_length_in - 2; - for (uint16_t i = 3; i < packet_length_before_crc; i++) - { - packet_ptr = &packet[i+PKT_INSTRUCTION-2]; - if (packet_ptr[0] == 0xFF && packet_ptr[1] == 0xFF && packet_ptr[2] == 0xFD) - packet_length_out++; - } - - if (packet_length_in == packet_length_out) // no stuffing required - return; - - uint16_t out_index = packet_length_out + 6 - 2; // last index before crc - uint16_t in_index = packet_length_in + 6 - 2; // last index before crc - while (out_index != in_index) - { - if (packet[in_index] == 0xFD && packet[in_index-1] == 0xFF && packet[in_index-2] == 0xFF) - { - packet[out_index--] = 0xFD; // byte stuffing - if (out_index != in_index) - { - packet[out_index--] = packet[in_index--]; // FD - packet[out_index--] = packet[in_index--]; // FF - packet[out_index--] = packet[in_index--]; // FF - } + if (packet_length_in < 8) // INSTRUCTION, ADDR_L, ADDR_H, CRC16_L, CRC16_H + FF FF FD + return; + + uint8_t* packet_ptr; + uint16_t packet_length_before_crc = packet_length_in - 2; + for (uint16_t i = 3; i < packet_length_before_crc; i++) { + packet_ptr = &packet[i + PKT_INSTRUCTION - 2]; + if (packet_ptr[0] == 0xFF && packet_ptr[1] == 0xFF && packet_ptr[2] == 0xFD) + packet_length_out++; } - else - { - packet[out_index--] = packet[in_index--]; + + if (packet_length_in == packet_length_out) // no stuffing required + return; + + uint16_t out_index = packet_length_out + 6 - 2; // last index before crc + uint16_t in_index = packet_length_in + 6 - 2; // last index before crc + while (out_index != in_index) { + if (packet[in_index] == 0xFD && packet[in_index - 1] == 0xFF && packet[in_index - 2] == 0xFF) { + packet[out_index--] = 0xFD; // byte stuffing + if (out_index != in_index) { + packet[out_index--] = packet[in_index--]; // FD + packet[out_index--] = packet[in_index--]; // FF + packet[out_index--] = packet[in_index--]; // FF + } + } + else { + packet[out_index--] = packet[in_index--]; + } } - } - packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out); - packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out); + packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out); + packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out); - return; + return; } -void Protocol2PacketHandler::removeStuffing(uint8_t *packet) +void Protocol2PacketHandler::removeStuffing(uint8_t* packet) { - int i = 0, index = 0; - int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]); - int packet_length_out = packet_length_in; - - index = PKT_INSTRUCTION; - for (i = 0; i < packet_length_in - 2; i++) // except CRC - { - if (packet[i+PKT_INSTRUCTION] == 0xFD && packet[i+PKT_INSTRUCTION+1] == 0xFD && packet[i+PKT_INSTRUCTION-1] == 0xFF && packet[i+PKT_INSTRUCTION-2] == 0xFF) - { // FF FF FD FD - packet_length_out--; - i++; + int i = 0, index = 0; + int packet_length_in = DXL_MAKEWORD(packet[PKT_LENGTH_L], packet[PKT_LENGTH_H]); + int packet_length_out = packet_length_in; + + index = PKT_INSTRUCTION; + for (i = 0; i < packet_length_in - 2; i++) // except CRC + { + if (packet[i + PKT_INSTRUCTION] == 0xFD && packet[i + PKT_INSTRUCTION + 1] == 0xFD && packet[i + PKT_INSTRUCTION - 1] == 0xFF && packet[i + PKT_INSTRUCTION - 2] == 0xFF) { // FF FF FD FD + packet_length_out--; + i++; + } + packet[index++] = packet[i + PKT_INSTRUCTION]; } - packet[index++] = packet[i+PKT_INSTRUCTION]; - } - packet[index++] = packet[PKT_INSTRUCTION+packet_length_in-2]; - packet[index++] = packet[PKT_INSTRUCTION+packet_length_in-1]; + packet[index++] = packet[PKT_INSTRUCTION + packet_length_in - 2]; + packet[index++] = packet[PKT_INSTRUCTION + packet_length_in - 1]; - packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out); - packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out); + packet[PKT_LENGTH_L] = DXL_LOBYTE(packet_length_out); + packet[PKT_LENGTH_H] = DXL_HIBYTE(packet_length_out); } -int Protocol2PacketHandler::txPacket(PortHandler *port, uint8_t *txpacket) +int Protocol2PacketHandler::txPacket(PortHandler* port, uint8_t* txpacket) { - uint16_t total_packet_length = 0; - uint16_t written_packet_length = 0; + uint16_t total_packet_length = 0; + uint16_t written_packet_length = 0; - if (port->is_using_) - return COMM_PORT_BUSY; - port->is_using_ = true; + if (port->is_using_) + return COMM_PORT_BUSY; + port->is_using_ = true; - // byte stuffing for header - addStuffing(txpacket); + // byte stuffing for header + addStuffing(txpacket); - // check max packet length - total_packet_length = DXL_MAKEWORD(txpacket[PKT_LENGTH_L], txpacket[PKT_LENGTH_H]) + 7; - // 7: HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H - if (total_packet_length > TXPACKET_MAX_LEN) - { - port->is_using_ = false; - return COMM_TX_ERROR; - } - - // make packet header - txpacket[PKT_HEADER0] = 0xFF; - txpacket[PKT_HEADER1] = 0xFF; - txpacket[PKT_HEADER2] = 0xFD; - txpacket[PKT_RESERVED] = 0x00; - - // add CRC16 - uint16_t crc = updateCRC(0, txpacket, total_packet_length - 2); // 2: CRC16 - txpacket[total_packet_length - 2] = DXL_LOBYTE(crc); - txpacket[total_packet_length - 1] = DXL_HIBYTE(crc); - - // tx packet - port->clearPort(); - written_packet_length = port->writePort(txpacket, total_packet_length); - if (total_packet_length != written_packet_length) - { - port->is_using_ = false; - return COMM_TX_FAIL; - } + // check max packet length + total_packet_length = DXL_MAKEWORD(txpacket[PKT_LENGTH_L], txpacket[PKT_LENGTH_H]) + 7; + // 7: HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H + if (total_packet_length > TXPACKET_MAX_LEN) { + port->is_using_ = false; + return COMM_TX_ERROR; + } - return COMM_SUCCESS; + // make packet header + txpacket[PKT_HEADER0] = 0xFF; + txpacket[PKT_HEADER1] = 0xFF; + txpacket[PKT_HEADER2] = 0xFD; + txpacket[PKT_RESERVED] = 0x00; + + // add CRC16 + uint16_t crc = updateCRC(0, txpacket, total_packet_length - 2); // 2: CRC16 + txpacket[total_packet_length - 2] = DXL_LOBYTE(crc); + txpacket[total_packet_length - 1] = DXL_HIBYTE(crc); + + // tx packet + port->clearPort(); + written_packet_length = port->writePort(txpacket, total_packet_length); + if (total_packet_length != written_packet_length) { + port->is_using_ = false; + return COMM_TX_FAIL; + } + + return COMM_SUCCESS; } -int Protocol2PacketHandler::rxPacket(PortHandler *port, uint8_t *rxpacket) +int Protocol2PacketHandler::rxPacket(PortHandler* port, uint8_t* rxpacket) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint16_t rx_length = 0; - uint16_t wait_length = 11; // minimum length (HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H INST ERROR CRC16_L CRC16_H) + uint16_t rx_length = 0; + uint16_t wait_length = 11; // minimum length (HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H INST ERROR CRC16_L CRC16_H) - while(true) - { - rx_length += port->readPort(&rxpacket[rx_length], wait_length - rx_length); - if (rx_length >= wait_length) - { - uint16_t idx = 0; - - // find packet header - for (idx = 0; idx < (rx_length - 3); idx++) - { - if ((rxpacket[idx] == 0xFF) && (rxpacket[idx+1] == 0xFF) && (rxpacket[idx+2] == 0xFD) && (rxpacket[idx+3] != 0xFD)) - break; - } - - if (idx == 0) // found at the beginning of the packet - { - if (rxpacket[PKT_RESERVED] != 0x00 || - rxpacket[PKT_ID] > 0xFC || - DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) > RXPACKET_MAX_LEN || - rxpacket[PKT_INSTRUCTION] != 0x55) - { - // remove the first byte in the packet - for (uint16_t s = 0; s < rx_length - 1; s++) - rxpacket[s] = rxpacket[1 + s]; - //memcpy(&rxpacket[0], &rxpacket[idx], rx_length - idx); - rx_length -= 1; - continue; - } + while (true) { + rx_length += port->readPort(&rxpacket[rx_length], wait_length - rx_length); + if (rx_length >= wait_length) { + uint16_t idx = 0; - // re-calculate the exact length of the rx packet - if (wait_length != DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1) - { - wait_length = DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1; - continue; - } + // find packet header + for (idx = 0; idx < (rx_length - 3); idx++) { + if ((rxpacket[idx] == 0xFF) && (rxpacket[idx + 1] == 0xFF) && (rxpacket[idx + 2] == 0xFD) && (rxpacket[idx + 3] != 0xFD)) + break; + } - if (rx_length < wait_length) - { - // check timeout - if (port->isPacketTimeout() == true) - { - if (rx_length == 0) + if (idx == 0) // found at the beginning of the packet { - result = COMM_RX_TIMEOUT; + if (rxpacket[PKT_RESERVED] != 0x00 || rxpacket[PKT_ID] > 0xFC || DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) > RXPACKET_MAX_LEN || rxpacket[PKT_INSTRUCTION] != 0x55) { + // remove the first byte in the packet + for (uint16_t s = 0; s < rx_length - 1; s++) + rxpacket[s] = rxpacket[1 + s]; + // memcpy(&rxpacket[0], &rxpacket[idx], rx_length - idx); + rx_length -= 1; + continue; + } + + // re-calculate the exact length of the rx packet + if (wait_length != DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1) { + wait_length = DXL_MAKEWORD(rxpacket[PKT_LENGTH_L], rxpacket[PKT_LENGTH_H]) + PKT_LENGTH_H + 1; + continue; + } + + if (rx_length < wait_length) { + // check timeout + if (port->isPacketTimeout() == true) { + if (rx_length == 0) { + result = COMM_RX_TIMEOUT; + } + else { + result = COMM_RX_CORRUPT; + } + break; + } + else { + continue; + } + } + + // verify CRC16 + uint16_t crc = DXL_MAKEWORD(rxpacket[wait_length - 2], rxpacket[wait_length - 1]); + if (updateCRC(0, rxpacket, wait_length - 2) == crc) { + result = COMM_SUCCESS; + } + else { + result = COMM_RX_CORRUPT; + } + break; } - else - { - result = COMM_RX_CORRUPT; + else { + // remove unnecessary packets + for (uint16_t s = 0; s < rx_length - idx; s++) + rxpacket[s] = rxpacket[idx + s]; + // memcpy(&rxpacket[0], &rxpacket[idx], rx_length - idx); + rx_length -= idx; } - break; - } - else - { - continue; - } - } - - // verify CRC16 - uint16_t crc = DXL_MAKEWORD(rxpacket[wait_length-2], rxpacket[wait_length-1]); - if (updateCRC(0, rxpacket, wait_length - 2) == crc) - { - result = COMM_SUCCESS; } - else - { - result = COMM_RX_CORRUPT; - } - break; - } - else - { - // remove unnecessary packets - for (uint16_t s = 0; s < rx_length - idx; s++) - rxpacket[s] = rxpacket[idx + s]; - //memcpy(&rxpacket[0], &rxpacket[idx], rx_length - idx); - rx_length -= idx; - } - } - else - { - // check timeout - if (port->isPacketTimeout() == true) - { - if (rx_length == 0) - { - result = COMM_RX_TIMEOUT; - } - else - { - result = COMM_RX_CORRUPT; + else { + // check timeout + if (port->isPacketTimeout() == true) { + if (rx_length == 0) { + result = COMM_RX_TIMEOUT; + } + else { + result = COMM_RX_CORRUPT; + } + break; + } } - break; - } - } #if defined(__linux__) || defined(__APPLE__) - usleep(0); + usleep(0); #elif defined(_WIN32) || defined(_WIN64) - Sleep(0); + Sleep(0); #endif - } - port->is_using_ = false; + } + port->is_using_ = false; - if (result == COMM_SUCCESS) - removeStuffing(rxpacket); + if (result == COMM_SUCCESS) + removeStuffing(rxpacket); - return result; + return result; } // NOT for BulkRead / SyncRead instruction -int Protocol2PacketHandler::txRxPacket(PortHandler *port, uint8_t *txpacket, uint8_t *rxpacket, uint8_t *error) +int Protocol2PacketHandler::txRxPacket(PortHandler* port, uint8_t* txpacket, uint8_t* rxpacket, uint8_t* error) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - // tx packet - result = txPacket(port, txpacket); - if (result != COMM_SUCCESS) - return result; + // tx packet + result = txPacket(port, txpacket); + if (result != COMM_SUCCESS) + return result; - // (Instruction == BulkRead or SyncRead) == this function is not available. - if (txpacket[PKT_INSTRUCTION] == INST_BULK_READ || txpacket[PKT_INSTRUCTION] == INST_SYNC_READ) - result = COMM_NOT_AVAILABLE; + // (Instruction == BulkRead or SyncRead) == this function is not available. + if (txpacket[PKT_INSTRUCTION] == INST_BULK_READ || txpacket[PKT_INSTRUCTION] == INST_SYNC_READ) + result = COMM_NOT_AVAILABLE; + + // (ID == Broadcast ID) == no need to wait for status packet or not available. + // (Instruction == action) == no need to wait for status packet + if (txpacket[PKT_ID] == BROADCAST_ID || txpacket[PKT_INSTRUCTION] == INST_ACTION) { + port->is_using_ = false; + return result; + } + + // set packet timeout + if (txpacket[PKT_INSTRUCTION] == INST_READ) { + port->setPacketTimeout((uint16_t)(DXL_MAKEWORD(txpacket[PKT_PARAMETER0 + 2], txpacket[PKT_PARAMETER0 + 3]) + 11)); + } + else { + port->setPacketTimeout((uint16_t)11); + // HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H INST ERROR CRC16_L CRC16_H + } + + // rx packet + do { + result = rxPacket(port, rxpacket); + } while (result == COMM_SUCCESS && txpacket[PKT_ID] != rxpacket[PKT_ID]); + + if (result == COMM_SUCCESS && txpacket[PKT_ID] == rxpacket[PKT_ID]) { + if (error != 0) + *error = (uint8_t)rxpacket[PKT_ERROR]; + } - // (ID == Broadcast ID) == no need to wait for status packet or not available. - // (Instruction == action) == no need to wait for status packet - if (txpacket[PKT_ID] == BROADCAST_ID || txpacket[PKT_INSTRUCTION] == INST_ACTION) - { - port->is_using_ = false; return result; - } - - // set packet timeout - if (txpacket[PKT_INSTRUCTION] == INST_READ) - { - port->setPacketTimeout((uint16_t)(DXL_MAKEWORD(txpacket[PKT_PARAMETER0+2], txpacket[PKT_PARAMETER0+3]) + 11)); - } - else - { - port->setPacketTimeout((uint16_t)11); - // HEADER0 HEADER1 HEADER2 RESERVED ID LENGTH_L LENGTH_H INST ERROR CRC16_L CRC16_H - } - - // rx packet - do { - result = rxPacket(port, rxpacket); - } while (result == COMM_SUCCESS && txpacket[PKT_ID] != rxpacket[PKT_ID]); - - if (result == COMM_SUCCESS && txpacket[PKT_ID] == rxpacket[PKT_ID]) - { - if (error != 0) - *error = (uint8_t)rxpacket[PKT_ERROR]; - } - - return result; } -int Protocol2PacketHandler::ping(PortHandler *port, uint8_t id, uint8_t *error) +int Protocol2PacketHandler::ping(PortHandler* port, uint8_t id, uint8_t* error) { - return ping(port, id, 0, error); + return ping(port, id, 0, error); } -int Protocol2PacketHandler::ping(PortHandler *port, uint8_t id, uint16_t *model_number, uint8_t *error) +int Protocol2PacketHandler::ping(PortHandler* port, uint8_t id, uint16_t* model_number, uint8_t* error) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t txpacket[10] = {0}; - uint8_t rxpacket[14] = {0}; + uint8_t txpacket[10] = { 0 }; + uint8_t rxpacket[14] = { 0 }; - if (id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; + if (id >= BROADCAST_ID) + return COMM_NOT_AVAILABLE; - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 3; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_PING; + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = 3; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_PING; - result = txRxPacket(port, txpacket, rxpacket, error); - if (result == COMM_SUCCESS && model_number != 0) - *model_number = DXL_MAKEWORD(rxpacket[PKT_PARAMETER0+1], rxpacket[PKT_PARAMETER0+2]); + result = txRxPacket(port, txpacket, rxpacket, error); + if (result == COMM_SUCCESS && model_number != 0) + *model_number = DXL_MAKEWORD(rxpacket[PKT_PARAMETER0 + 1], rxpacket[PKT_PARAMETER0 + 2]); - return result; + return result; } -int Protocol2PacketHandler::broadcastPing(PortHandler *port, std::vector &id_list) +int Protocol2PacketHandler::broadcastPing(PortHandler* port, std::vector& id_list) { - const int STATUS_LENGTH = 14; - int result = COMM_TX_FAIL; + const int STATUS_LENGTH = 14; + int result = COMM_TX_FAIL; - id_list.clear(); + id_list.clear(); - uint16_t rx_length = 0; - uint16_t wait_length = STATUS_LENGTH * MAX_ID; + uint16_t rx_length = 0; + uint16_t wait_length = STATUS_LENGTH * MAX_ID; - uint8_t txpacket[10] = {0}; - uint8_t rxpacket[STATUS_LENGTH * MAX_ID] = {0}; + uint8_t txpacket[10] = { 0 }; + uint8_t rxpacket[STATUS_LENGTH * MAX_ID] = { 0 }; - double tx_time_per_byte = (1000.0 / (double)port->getBaudRate()) * 10.0; + double tx_time_per_byte = (1000.0 / (double)port->getBaudRate()) * 10.0; - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = 3; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_PING; + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH_L] = 3; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_PING; + + result = txPacket(port, txpacket); + if (result != COMM_SUCCESS) { + port->is_using_ = false; + return result; + } + + // set rx timeout + // port->setPacketTimeout((uint16_t)(wait_length * 30)); + port->setPacketTimeout(((double)wait_length * tx_time_per_byte) + (3.0 * (double)MAX_ID) + 16.0); + + while (1) { + rx_length += port->readPort(&rxpacket[rx_length], wait_length - rx_length); + if (port->isPacketTimeout() == true) // || rx_length >= wait_length) + break; + } - result = txPacket(port, txpacket); - if (result != COMM_SUCCESS) - { port->is_using_ = false; - return result; - } - // set rx timeout - //port->setPacketTimeout((uint16_t)(wait_length * 30)); - port->setPacketTimeout(((double)wait_length * tx_time_per_byte) + (3.0 * (double)MAX_ID) + 16.0); + if (rx_length == 0) + return COMM_RX_TIMEOUT; - while(1) - { - rx_length += port->readPort(&rxpacket[rx_length], wait_length - rx_length); - if (port->isPacketTimeout() == true)// || rx_length >= wait_length) - break; - } + while (1) { + if (rx_length < STATUS_LENGTH) + return COMM_RX_CORRUPT; - port->is_using_ = false; + uint16_t idx = 0; - if (rx_length == 0) - return COMM_RX_TIMEOUT; + // find packet header + for (idx = 0; idx < (rx_length - 2); idx++) { + if (rxpacket[idx] == 0xFF && rxpacket[idx + 1] == 0xFF && rxpacket[idx + 2] == 0xFD) + break; + } - while(1) - { - if (rx_length < STATUS_LENGTH) - return COMM_RX_CORRUPT; + if (idx == 0) // found at the beginning of the packet + { + // verify CRC16 + uint16_t crc = DXL_MAKEWORD(rxpacket[STATUS_LENGTH - 2], rxpacket[STATUS_LENGTH - 1]); - uint16_t idx = 0; + if (updateCRC(0, rxpacket, STATUS_LENGTH - 2) == crc) { + result = COMM_SUCCESS; - // find packet header - for (idx = 0; idx < (rx_length - 2); idx++) - { - if (rxpacket[idx] == 0xFF && rxpacket[idx+1] == 0xFF && rxpacket[idx+2] == 0xFD) - break; - } + id_list.push_back(rxpacket[PKT_ID]); - if (idx == 0) // found at the beginning of the packet - { - // verify CRC16 - uint16_t crc = DXL_MAKEWORD(rxpacket[STATUS_LENGTH-2], rxpacket[STATUS_LENGTH-1]); - - if (updateCRC(0, rxpacket, STATUS_LENGTH - 2) == crc) - { - result = COMM_SUCCESS; - - id_list.push_back(rxpacket[PKT_ID]); - - for (uint16_t s = 0; s < rx_length - STATUS_LENGTH; s++) - rxpacket[s] = rxpacket[STATUS_LENGTH + s]; - rx_length -= STATUS_LENGTH; - - if (rx_length == 0) - return result; - } - else - { - result = COMM_RX_CORRUPT; - - // remove header (0xFF 0xFF 0xFD) - for (uint16_t s = 0; s < rx_length - 3; s++) - rxpacket[s] = rxpacket[3 + s]; - rx_length -= 3; - } - } - else - { - // remove unnecessary packets - for (uint16_t s = 0; s < rx_length - idx; s++) - rxpacket[s] = rxpacket[idx + s]; - rx_length -= idx; + for (uint16_t s = 0; s < rx_length - STATUS_LENGTH; s++) + rxpacket[s] = rxpacket[STATUS_LENGTH + s]; + rx_length -= STATUS_LENGTH; + + if (rx_length == 0) + return result; + } + else { + result = COMM_RX_CORRUPT; + + // remove header (0xFF 0xFF 0xFD) + for (uint16_t s = 0; s < rx_length - 3; s++) + rxpacket[s] = rxpacket[3 + s]; + rx_length -= 3; + } + } + else { + // remove unnecessary packets + for (uint16_t s = 0; s < rx_length - idx; s++) + rxpacket[s] = rxpacket[idx + s]; + rx_length -= idx; + } } - } - return result; + return result; } -int Protocol2PacketHandler::action(PortHandler *port, uint8_t id) +int Protocol2PacketHandler::action(PortHandler* port, uint8_t id) { - uint8_t txpacket[10] = {0}; + uint8_t txpacket[10] = { 0 }; - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 3; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_ACTION; + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = 3; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_ACTION; - return txRxPacket(port, txpacket, 0); + return txRxPacket(port, txpacket, 0); } -int Protocol2PacketHandler::reboot(PortHandler *port, uint8_t id, uint8_t *error) +int Protocol2PacketHandler::reboot(PortHandler* port, uint8_t id, uint8_t* error) { - uint8_t txpacket[10] = {0}; - uint8_t rxpacket[11] = {0}; + uint8_t txpacket[10] = { 0 }; + uint8_t rxpacket[11] = { 0 }; - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 3; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_REBOOT; + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = 3; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_REBOOT; - return txRxPacket(port, txpacket, rxpacket, error); + return txRxPacket(port, txpacket, rxpacket, error); } -int Protocol2PacketHandler::clearMultiTurn(PortHandler *port, uint8_t id, uint8_t *error) +int Protocol2PacketHandler::clearMultiTurn(PortHandler* port, uint8_t id, uint8_t* error) { - uint8_t txpacket[15] = {0}; - uint8_t rxpacket[11] = {0}; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 8; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_CLEAR; - txpacket[PKT_PARAMETER0] = 0x01; - txpacket[PKT_PARAMETER0+1] = 0x44; - txpacket[PKT_PARAMETER0+2] = 0x58; - txpacket[PKT_PARAMETER0+3] = 0x4C; - txpacket[PKT_PARAMETER0+4] = 0x22; - - return txRxPacket(port, txpacket, rxpacket, error); + uint8_t txpacket[15] = { 0 }; + uint8_t rxpacket[11] = { 0 }; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = 8; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_CLEAR; + txpacket[PKT_PARAMETER0] = 0x01; + txpacket[PKT_PARAMETER0 + 1] = 0x44; + txpacket[PKT_PARAMETER0 + 2] = 0x58; + txpacket[PKT_PARAMETER0 + 3] = 0x4C; + txpacket[PKT_PARAMETER0 + 4] = 0x22; + + return txRxPacket(port, txpacket, rxpacket, error); } -int Protocol2PacketHandler::factoryReset(PortHandler *port, uint8_t id, uint8_t option, uint8_t *error) +int Protocol2PacketHandler::factoryReset(PortHandler* port, uint8_t id, uint8_t option, uint8_t* error) { - uint8_t txpacket[11] = {0}; - uint8_t rxpacket[11] = {0}; + uint8_t txpacket[11] = { 0 }; + uint8_t rxpacket[11] = { 0 }; - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 4; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET; - txpacket[PKT_PARAMETER0] = option; + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = 4; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_FACTORY_RESET; + txpacket[PKT_PARAMETER0] = option; - return txRxPacket(port, txpacket, rxpacket, error); + return txRxPacket(port, txpacket, rxpacket, error); } -int Protocol2PacketHandler::readTx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length) +int Protocol2PacketHandler::readTx(PortHandler* port, uint8_t id, uint16_t address, uint16_t length) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t txpacket[14] = {0}; + uint8_t txpacket[14] = { 0 }; - if (id >= BROADCAST_ID) - return COMM_NOT_AVAILABLE; + if (id >= BROADCAST_ID) + return COMM_NOT_AVAILABLE; - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 7; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_READ; - txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); - txpacket[PKT_PARAMETER0+2] = (uint8_t)DXL_LOBYTE(length); - txpacket[PKT_PARAMETER0+3] = (uint8_t)DXL_HIBYTE(length); + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = 7; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_READ; + txpacket[PKT_PARAMETER0 + 0] = (uint8_t)DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0 + 1] = (uint8_t)DXL_HIBYTE(address); + txpacket[PKT_PARAMETER0 + 2] = (uint8_t)DXL_LOBYTE(length); + txpacket[PKT_PARAMETER0 + 3] = (uint8_t)DXL_HIBYTE(length); - result = txPacket(port, txpacket); + result = txPacket(port, txpacket); - // set packet timeout - if (result == COMM_SUCCESS) - port->setPacketTimeout((uint16_t)(length + 11)); + // set packet timeout + if (result == COMM_SUCCESS) + port->setPacketTimeout((uint16_t)(length + 11)); - return result; + return result; } -int Protocol2PacketHandler::readRx(PortHandler *port, uint8_t id, uint16_t length, uint8_t *data, uint8_t *error) +int Protocol2PacketHandler::readRx(PortHandler* port, uint8_t id, uint16_t length, uint8_t* data, uint8_t* error) { - int result = COMM_TX_FAIL; - uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); - //(length + 11 + (length/3)); // (length/3): consider stuffing - - if (rxpacket == NULL) - return result; - - do { - result = rxPacket(port, rxpacket); - } while (result == COMM_SUCCESS && rxpacket[PKT_ID] != id); + int result = COMM_TX_FAIL; + uint8_t* rxpacket = (uint8_t*)malloc(RXPACKET_MAX_LEN); + //(length + 11 + (length/3)); // (length/3): consider stuffing - if (result == COMM_SUCCESS && rxpacket[PKT_ID] == id) - { - if (error != 0) - *error = (uint8_t)rxpacket[PKT_ERROR]; + if (rxpacket == NULL) + return result; - for (uint16_t s = 0; s < length; s++) - { - data[s] = rxpacket[PKT_PARAMETER0 + 1 + s]; + do { + result = rxPacket(port, rxpacket); + } while (result == COMM_SUCCESS && rxpacket[PKT_ID] != id); + + if (result == COMM_SUCCESS && rxpacket[PKT_ID] == id) { + if (error != 0) + *error = (uint8_t)rxpacket[PKT_ERROR]; + + for (uint16_t s = 0; s < length; s++) { + data[s] = rxpacket[PKT_PARAMETER0 + 1 + s]; + } + // memcpy(data, &rxpacket[PKT_PARAMETER0+1], length); } - //memcpy(data, &rxpacket[PKT_PARAMETER0+1], length); - } - free(rxpacket); - //delete[] rxpacket; - return result; + free(rxpacket); + // delete[] rxpacket; + return result; } -int Protocol2PacketHandler::readTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) +int Protocol2PacketHandler::readTxRx(PortHandler* port, uint8_t id, uint16_t address, uint16_t length, uint8_t* data, uint8_t* error) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t txpacket[14] = {0}; - uint8_t *rxpacket = (uint8_t *)malloc(RXPACKET_MAX_LEN); - //(length + 11 + (length/3)); // (length/3): consider stuffing + uint8_t txpacket[14] = { 0 }; + uint8_t* rxpacket = (uint8_t*)malloc(RXPACKET_MAX_LEN); + //(length + 11 + (length/3)); // (length/3): consider stuffing - if (rxpacket == NULL) - return result; - - if (id >= BROADCAST_ID) - { - free(rxpacket); - return COMM_NOT_AVAILABLE; - } - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = 7; - txpacket[PKT_LENGTH_H] = 0; - txpacket[PKT_INSTRUCTION] = INST_READ; - txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); - txpacket[PKT_PARAMETER0+2] = (uint8_t)DXL_LOBYTE(length); - txpacket[PKT_PARAMETER0+3] = (uint8_t)DXL_HIBYTE(length); - - result = txRxPacket(port, txpacket, rxpacket, error); - if (result == COMM_SUCCESS) - { - if (error != 0) - *error = (uint8_t)rxpacket[PKT_ERROR]; + if (rxpacket == NULL) + return result; - for (uint16_t s = 0; s < length; s++) - { - data[s] = rxpacket[PKT_PARAMETER0 + 1 + s]; + if (id >= BROADCAST_ID) { + free(rxpacket); + return COMM_NOT_AVAILABLE; + } + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = 7; + txpacket[PKT_LENGTH_H] = 0; + txpacket[PKT_INSTRUCTION] = INST_READ; + txpacket[PKT_PARAMETER0 + 0] = (uint8_t)DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0 + 1] = (uint8_t)DXL_HIBYTE(address); + txpacket[PKT_PARAMETER0 + 2] = (uint8_t)DXL_LOBYTE(length); + txpacket[PKT_PARAMETER0 + 3] = (uint8_t)DXL_HIBYTE(length); + + result = txRxPacket(port, txpacket, rxpacket, error); + if (result == COMM_SUCCESS) { + if (error != 0) + *error = (uint8_t)rxpacket[PKT_ERROR]; + + for (uint16_t s = 0; s < length; s++) { + data[s] = rxpacket[PKT_PARAMETER0 + 1 + s]; + } + // memcpy(data, &rxpacket[PKT_PARAMETER0+1], length); } - //memcpy(data, &rxpacket[PKT_PARAMETER0+1], length); - } - free(rxpacket); - //delete[] rxpacket; - return result; + free(rxpacket); + // delete[] rxpacket; + return result; } -int Protocol2PacketHandler::read1ByteTx(PortHandler *port, uint8_t id, uint16_t address) +int Protocol2PacketHandler::read1ByteTx(PortHandler* port, uint8_t id, uint16_t address) { - return readTx(port, id, address, 1); + return readTx(port, id, address, 1); } -int Protocol2PacketHandler::read1ByteRx(PortHandler *port, uint8_t id, uint8_t *data, uint8_t *error) +int Protocol2PacketHandler::read1ByteRx(PortHandler* port, uint8_t id, uint8_t* data, uint8_t* error) { - uint8_t data_read[1] = {0}; - int result = readRx(port, id, 1, data_read, error); - if (result == COMM_SUCCESS) - *data = data_read[0]; - return result; + uint8_t data_read[1] = { 0 }; + int result = readRx(port, id, 1, data_read, error); + if (result == COMM_SUCCESS) + *data = data_read[0]; + return result; } -int Protocol2PacketHandler::read1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t *data, uint8_t *error) +int Protocol2PacketHandler::read1ByteTxRx(PortHandler* port, uint8_t id, uint16_t address, uint8_t* data, uint8_t* error) { - uint8_t data_read[1] = {0}; - int result = readTxRx(port, id, address, 1, data_read, error); - if (result == COMM_SUCCESS) - *data = data_read[0]; - return result; + uint8_t data_read[1] = { 0 }; + int result = readTxRx(port, id, address, 1, data_read, error); + if (result == COMM_SUCCESS) + *data = data_read[0]; + return result; } -int Protocol2PacketHandler::read2ByteTx(PortHandler *port, uint8_t id, uint16_t address) +int Protocol2PacketHandler::read2ByteTx(PortHandler* port, uint8_t id, uint16_t address) { - return readTx(port, id, address, 2); + return readTx(port, id, address, 2); } -int Protocol2PacketHandler::read2ByteRx(PortHandler *port, uint8_t id, uint16_t *data, uint8_t *error) +int Protocol2PacketHandler::read2ByteRx(PortHandler* port, uint8_t id, uint16_t* data, uint8_t* error) { - uint8_t data_read[2] = {0}; - int result = readRx(port, id, 2, data_read, error); - if (result == COMM_SUCCESS) - *data = DXL_MAKEWORD(data_read[0], data_read[1]); - return result; + uint8_t data_read[2] = { 0 }; + int result = readRx(port, id, 2, data_read, error); + if (result == COMM_SUCCESS) + *data = DXL_MAKEWORD(data_read[0], data_read[1]); + return result; } -int Protocol2PacketHandler::read2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t *data, uint8_t *error) +int Protocol2PacketHandler::read2ByteTxRx(PortHandler* port, uint8_t id, uint16_t address, uint16_t* data, uint8_t* error) { - uint8_t data_read[2] = {0}; - int result = readTxRx(port, id, address, 2, data_read, error); - if (result == COMM_SUCCESS) - *data = DXL_MAKEWORD(data_read[0], data_read[1]); - return result; + uint8_t data_read[2] = { 0 }; + int result = readTxRx(port, id, address, 2, data_read, error); + if (result == COMM_SUCCESS) + *data = DXL_MAKEWORD(data_read[0], data_read[1]); + return result; } -int Protocol2PacketHandler::read4ByteTx(PortHandler *port, uint8_t id, uint16_t address) +int Protocol2PacketHandler::read4ByteTx(PortHandler* port, uint8_t id, uint16_t address) { - return readTx(port, id, address, 4); + return readTx(port, id, address, 4); } -int Protocol2PacketHandler::read4ByteRx(PortHandler *port, uint8_t id, uint32_t *data, uint8_t *error) +int Protocol2PacketHandler::read4ByteRx(PortHandler* port, uint8_t id, uint32_t* data, uint8_t* error) { - uint8_t data_read[4] = {0}; - int result = readRx(port, id, 4, data_read, error); - if (result == COMM_SUCCESS) - *data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3])); - return result; + uint8_t data_read[4] = { 0 }; + int result = readRx(port, id, 4, data_read, error); + if (result == COMM_SUCCESS) + *data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3])); + return result; } -int Protocol2PacketHandler::read4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t *data, uint8_t *error) +int Protocol2PacketHandler::read4ByteTxRx(PortHandler* port, uint8_t id, uint16_t address, uint32_t* data, uint8_t* error) { - uint8_t data_read[4] = {0}; - int result = readTxRx(port, id, address, 4, data_read, error); - if (result == COMM_SUCCESS) - *data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3])); - return result; + uint8_t data_read[4] = { 0 }; + int result = readTxRx(port, id, address, 4, data_read, error); + if (result == COMM_SUCCESS) + *data = DXL_MAKEDWORD(DXL_MAKEWORD(data_read[0], data_read[1]), DXL_MAKEWORD(data_read[2], data_read[3])); + return result; } - -int Protocol2PacketHandler::writeTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) +int Protocol2PacketHandler::writeTxOnly(PortHandler* port, uint8_t id, uint16_t address, uint16_t length, uint8_t* data) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3)); - - if (txpacket == NULL) - return result; + uint8_t* txpacket = (uint8_t*)malloc(length + 12 + (length / 3)); - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); - txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); + if (txpacket == NULL) + return result; - for (uint16_t s = 0; s < length; s++) - txpacket[PKT_PARAMETER0+2+s] = data[s]; - //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length + 5); + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length + 5); + txpacket[PKT_INSTRUCTION] = INST_WRITE; + txpacket[PKT_PARAMETER0 + 0] = (uint8_t)DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0 + 1] = (uint8_t)DXL_HIBYTE(address); - result = txPacket(port, txpacket); - port->is_using_ = false; + for (uint16_t s = 0; s < length; s++) + txpacket[PKT_PARAMETER0 + 2 + s] = data[s]; + // memcpy(&txpacket[PKT_PARAMETER0+2], data, length); - free(txpacket); - //delete[] txpacket; - return result; + result = txPacket(port, txpacket); + port->is_using_ = false; + + free(txpacket); + // delete[] txpacket; + return result; } -int Protocol2PacketHandler::writeTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) +int Protocol2PacketHandler::writeTxRx(PortHandler* port, uint8_t id, uint16_t address, uint16_t length, uint8_t* data, uint8_t* error) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; + + uint8_t* txpacket = (uint8_t*)malloc(length + 12 + (length / 3)); + uint8_t rxpacket[11] = { 0 }; + + if (txpacket == NULL) + return result; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length + 5); + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length + 5); + txpacket[PKT_INSTRUCTION] = INST_WRITE; + txpacket[PKT_PARAMETER0 + 0] = (uint8_t)DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0 + 1] = (uint8_t)DXL_HIBYTE(address); + + for (uint16_t s = 0; s < length; s++) + txpacket[PKT_PARAMETER0 + 2 + s] = data[s]; + // memcpy(&txpacket[PKT_PARAMETER0+2], data, length); - uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3)); - uint8_t rxpacket[11] = {0}; + result = txRxPacket(port, txpacket, rxpacket, error); - if (txpacket == NULL) + free(txpacket); + // delete[] txpacket; return result; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); - txpacket[PKT_INSTRUCTION] = INST_WRITE; - txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); - - for (uint16_t s = 0; s < length; s++) - txpacket[PKT_PARAMETER0+2+s] = data[s]; - //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); - - result = txRxPacket(port, txpacket, rxpacket, error); - - free(txpacket); - //delete[] txpacket; - return result; } -int Protocol2PacketHandler::write1ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint8_t data) +int Protocol2PacketHandler::write1ByteTxOnly(PortHandler* port, uint8_t id, uint16_t address, uint8_t data) { - uint8_t data_write[1] = { data }; - return writeTxOnly(port, id, address, 1, data_write); + uint8_t data_write[1] = { data }; + return writeTxOnly(port, id, address, 1, data_write); } -int Protocol2PacketHandler::write1ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint8_t data, uint8_t *error) +int Protocol2PacketHandler::write1ByteTxRx(PortHandler* port, uint8_t id, uint16_t address, uint8_t data, uint8_t* error) { - uint8_t data_write[1] = { data }; - return writeTxRx(port, id, address, 1, data_write, error); + uint8_t data_write[1] = { data }; + return writeTxRx(port, id, address, 1, data_write, error); } -int Protocol2PacketHandler::write2ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t data) +int Protocol2PacketHandler::write2ByteTxOnly(PortHandler* port, uint8_t id, uint16_t address, uint16_t data) { - uint8_t data_write[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; - return writeTxOnly(port, id, address, 2, data_write); + uint8_t data_write[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; + return writeTxOnly(port, id, address, 2, data_write); } -int Protocol2PacketHandler::write2ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t data, uint8_t *error) +int Protocol2PacketHandler::write2ByteTxRx(PortHandler* port, uint8_t id, uint16_t address, uint16_t data, uint8_t* error) { - uint8_t data_write[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; - return writeTxRx(port, id, address, 2, data_write, error); + uint8_t data_write[2] = { DXL_LOBYTE(data), DXL_HIBYTE(data) }; + return writeTxRx(port, id, address, 2, data_write, error); } -int Protocol2PacketHandler::write4ByteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint32_t data) +int Protocol2PacketHandler::write4ByteTxOnly(PortHandler* port, uint8_t id, uint16_t address, uint32_t data) { - uint8_t data_write[4] = { DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data)) }; - return writeTxOnly(port, id, address, 4, data_write); + uint8_t data_write[4] = { DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data)) }; + return writeTxOnly(port, id, address, 4, data_write); } -int Protocol2PacketHandler::write4ByteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint32_t data, uint8_t *error) +int Protocol2PacketHandler::write4ByteTxRx(PortHandler* port, uint8_t id, uint16_t address, uint32_t data, uint8_t* error) { - uint8_t data_write[4] = { DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data)) }; - return writeTxRx(port, id, address, 4, data_write, error); + uint8_t data_write[4] = { DXL_LOBYTE(DXL_LOWORD(data)), DXL_HIBYTE(DXL_LOWORD(data)), DXL_LOBYTE(DXL_HIWORD(data)), DXL_HIBYTE(DXL_HIWORD(data)) }; + return writeTxRx(port, id, address, 4, data_write, error); } -int Protocol2PacketHandler::regWriteTxOnly(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data) +int Protocol2PacketHandler::regWriteTxOnly(PortHandler* port, uint8_t id, uint16_t address, uint16_t length, uint8_t* data) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3)); + uint8_t* txpacket = (uint8_t*)malloc(length + 12 + (length / 3)); - if (txpacket == NULL) + if (txpacket == NULL) + return result; + + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length + 5); + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length + 5); + txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; + txpacket[PKT_PARAMETER0 + 0] = (uint8_t)DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0 + 1] = (uint8_t)DXL_HIBYTE(address); + + for (uint16_t s = 0; s < length; s++) + txpacket[PKT_PARAMETER0 + 2 + s] = data[s]; + // memcpy(&txpacket[PKT_PARAMETER0+2], data, length); + + result = txPacket(port, txpacket); + port->is_using_ = false; + + free(txpacket); + // delete[] txpacket; return result; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); - txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); - - for (uint16_t s = 0; s < length; s++) - txpacket[PKT_PARAMETER0+2+s] = data[s]; - //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); - - result = txPacket(port, txpacket); - port->is_using_ = false; - - free(txpacket); - //delete[] txpacket; - return result; } -int Protocol2PacketHandler::regWriteTxRx(PortHandler *port, uint8_t id, uint16_t address, uint16_t length, uint8_t *data, uint8_t *error) +int Protocol2PacketHandler::regWriteTxRx(PortHandler* port, uint8_t id, uint16_t address, uint16_t length, uint8_t* data, uint8_t* error) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; + + uint8_t* txpacket = (uint8_t*)malloc(length + 12 + (length / 3)); + uint8_t rxpacket[11] = { 0 }; - uint8_t *txpacket = (uint8_t *)malloc(length + 12 + (length / 3)); - uint8_t rxpacket[11] = {0}; + if (txpacket == NULL) + return result; - if (txpacket == NULL) + txpacket[PKT_ID] = id; + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length + 5); + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length + 5); + txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; + txpacket[PKT_PARAMETER0 + 0] = (uint8_t)DXL_LOBYTE(address); + txpacket[PKT_PARAMETER0 + 1] = (uint8_t)DXL_HIBYTE(address); + + for (uint16_t s = 0; s < length; s++) + txpacket[PKT_PARAMETER0 + 2 + s] = data[s]; + // memcpy(&txpacket[PKT_PARAMETER0+2], data, length); + + result = txRxPacket(port, txpacket, rxpacket, error); + + free(txpacket); + // delete[] txpacket; return result; - - txpacket[PKT_ID] = id; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(length+5); - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(length+5); - txpacket[PKT_INSTRUCTION] = INST_REG_WRITE; - txpacket[PKT_PARAMETER0+0] = (uint8_t)DXL_LOBYTE(address); - txpacket[PKT_PARAMETER0+1] = (uint8_t)DXL_HIBYTE(address); - - for (uint16_t s = 0; s < length; s++) - txpacket[PKT_PARAMETER0+2+s] = data[s]; - //memcpy(&txpacket[PKT_PARAMETER0+2], data, length); - - result = txRxPacket(port, txpacket, rxpacket, error); - - free(txpacket); - //delete[] txpacket; - return result; } -int Protocol2PacketHandler::syncReadTx(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) +int Protocol2PacketHandler::syncReadTx(PortHandler* port, uint16_t start_address, uint16_t data_length, uint8_t* param, uint16_t param_length) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; + + uint8_t* txpacket = (uint8_t*)malloc(param_length + 14 + (param_length / 3)); + // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + + if (txpacket == NULL) + return result; + + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + txpacket[PKT_INSTRUCTION] = INST_SYNC_READ; + txpacket[PKT_PARAMETER0 + 0] = DXL_LOBYTE(start_address); + txpacket[PKT_PARAMETER0 + 1] = DXL_HIBYTE(start_address); + txpacket[PKT_PARAMETER0 + 2] = DXL_LOBYTE(data_length); + txpacket[PKT_PARAMETER0 + 3] = DXL_HIBYTE(data_length); - uint8_t *txpacket = (uint8_t *)malloc(param_length + 14 + (param_length / 3)); - // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + for (uint16_t s = 0; s < param_length; s++) + txpacket[PKT_PARAMETER0 + 4 + s] = param[s]; + // memcpy(&txpacket[PKT_PARAMETER0+4], param, param_length); - if (txpacket == NULL) + result = txPacket(port, txpacket); + if (result == COMM_SUCCESS) + port->setPacketTimeout((uint16_t)((11 + data_length) * param_length)); + + free(txpacket); return result; - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - txpacket[PKT_INSTRUCTION] = INST_SYNC_READ; - txpacket[PKT_PARAMETER0+0] = DXL_LOBYTE(start_address); - txpacket[PKT_PARAMETER0+1] = DXL_HIBYTE(start_address); - txpacket[PKT_PARAMETER0+2] = DXL_LOBYTE(data_length); - txpacket[PKT_PARAMETER0+3] = DXL_HIBYTE(data_length); - - for (uint16_t s = 0; s < param_length; s++) - txpacket[PKT_PARAMETER0+4+s] = param[s]; - //memcpy(&txpacket[PKT_PARAMETER0+4], param, param_length); - - result = txPacket(port, txpacket); - if (result == COMM_SUCCESS) - port->setPacketTimeout((uint16_t)((11 + data_length) * param_length)); - - free(txpacket); - return result; } -int Protocol2PacketHandler::syncWriteTxOnly(PortHandler *port, uint16_t start_address, uint16_t data_length, uint8_t *param, uint16_t param_length) +int Protocol2PacketHandler::syncWriteTxOnly(PortHandler* port, uint16_t start_address, uint16_t data_length, uint8_t* param, uint16_t param_length) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; + + uint8_t* txpacket = (uint8_t*)malloc(param_length + 14 + (param_length / 3)); + // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + + if (txpacket == NULL) + return result; + + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE; + txpacket[PKT_PARAMETER0 + 0] = DXL_LOBYTE(start_address); + txpacket[PKT_PARAMETER0 + 1] = DXL_HIBYTE(start_address); + txpacket[PKT_PARAMETER0 + 2] = DXL_LOBYTE(data_length); + txpacket[PKT_PARAMETER0 + 3] = DXL_HIBYTE(data_length); - uint8_t *txpacket = (uint8_t *)malloc(param_length + 14 + (param_length / 3)); - // 14: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H + for (uint16_t s = 0; s < param_length; s++) + txpacket[PKT_PARAMETER0 + 4 + s] = param[s]; + // memcpy(&txpacket[PKT_PARAMETER0+4], param, param_length); - if (txpacket == NULL) + result = txRxPacket(port, txpacket, 0, 0); + + free(txpacket); + // delete[] txpacket; return result; - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 7); // 7: INST START_ADDR_L START_ADDR_H DATA_LEN_L DATA_LEN_H CRC16_L CRC16_H - txpacket[PKT_INSTRUCTION] = INST_SYNC_WRITE; - txpacket[PKT_PARAMETER0+0] = DXL_LOBYTE(start_address); - txpacket[PKT_PARAMETER0+1] = DXL_HIBYTE(start_address); - txpacket[PKT_PARAMETER0+2] = DXL_LOBYTE(data_length); - txpacket[PKT_PARAMETER0+3] = DXL_HIBYTE(data_length); - - for (uint16_t s = 0; s < param_length; s++) - txpacket[PKT_PARAMETER0+4+s] = param[s]; - //memcpy(&txpacket[PKT_PARAMETER0+4], param, param_length); - - result = txRxPacket(port, txpacket, 0, 0); - - free(txpacket); - //delete[] txpacket; - return result; } -int Protocol2PacketHandler::bulkReadTx(PortHandler *port, uint8_t *param, uint16_t param_length) +int Protocol2PacketHandler::bulkReadTx(PortHandler* port, uint8_t* param, uint16_t param_length) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; + + uint8_t* txpacket = (uint8_t*)malloc(param_length + 10 + (param_length / 3)); + // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H + + if (txpacket == NULL) + return result; - uint8_t *txpacket = (uint8_t *)malloc(param_length + 10 + (param_length / 3)); - // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H + txpacket[PKT_INSTRUCTION] = INST_BULK_READ; - if (txpacket == NULL) + for (uint16_t s = 0; s < param_length; s++) + txpacket[PKT_PARAMETER0 + s] = param[s]; + // memcpy(&txpacket[PKT_PARAMETER0], param, param_length); + + result = txPacket(port, txpacket); + if (result == COMM_SUCCESS) { + int wait_length = 0; + for (uint16_t i = 0; i < param_length; i += 5) + wait_length += DXL_MAKEWORD(param[i + 3], param[i + 4]) + 10; + port->setPacketTimeout((uint16_t)wait_length); + } + + free(txpacket); + // delete[] txpacket; return result; - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H - txpacket[PKT_INSTRUCTION] = INST_BULK_READ; - - for (uint16_t s = 0; s < param_length; s++) - txpacket[PKT_PARAMETER0+s] = param[s]; - //memcpy(&txpacket[PKT_PARAMETER0], param, param_length); - - result = txPacket(port, txpacket); - if (result == COMM_SUCCESS) - { - int wait_length = 0; - for (uint16_t i = 0; i < param_length; i += 5) - wait_length += DXL_MAKEWORD(param[i+3], param[i+4]) + 10; - port->setPacketTimeout((uint16_t)wait_length); - } - - free(txpacket); - //delete[] txpacket; - return result; } -int Protocol2PacketHandler::bulkWriteTxOnly(PortHandler *port, uint8_t *param, uint16_t param_length) +int Protocol2PacketHandler::bulkWriteTxOnly(PortHandler* port, uint8_t* param, uint16_t param_length) { - int result = COMM_TX_FAIL; + int result = COMM_TX_FAIL; - uint8_t *txpacket = (uint8_t *)malloc(param_length + 10 + (param_length / 3)); - // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H + uint8_t* txpacket = (uint8_t*)malloc(param_length + 10 + (param_length / 3)); + // 10: HEADER0 HEADER1 HEADER2 RESERVED ID LEN_L LEN_H INST CRC16_L CRC16_H - if (txpacket == NULL) - return result; - - txpacket[PKT_ID] = BROADCAST_ID; - txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H - txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H - txpacket[PKT_INSTRUCTION] = INST_BULK_WRITE; + if (txpacket == NULL) + return result; - for (uint16_t s = 0; s < param_length; s++) - txpacket[PKT_PARAMETER0+s] = param[s]; - //memcpy(&txpacket[PKT_PARAMETER0], param, param_length); + txpacket[PKT_ID] = BROADCAST_ID; + txpacket[PKT_LENGTH_L] = DXL_LOBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H + txpacket[PKT_LENGTH_H] = DXL_HIBYTE(param_length + 3); // 3: INST CRC16_L CRC16_H + txpacket[PKT_INSTRUCTION] = INST_BULK_WRITE; - result = txRxPacket(port, txpacket, 0, 0); + for (uint16_t s = 0; s < param_length; s++) + txpacket[PKT_PARAMETER0 + s] = param[s]; + // memcpy(&txpacket[PKT_PARAMETER0], param, param_length); - free(txpacket); - //delete[] txpacket; - return result; + result = txRxPacket(port, txpacket, 0, 0); + + free(txpacket); + // delete[] txpacket; + return result; } diff --git a/serial-ros2/include/serial/serial.h b/serial-ros2/include/serial/serial.h index 4970d62..8875141 100644 --- a/serial-ros2/include/serial/serial.h +++ b/serial-ros2/include/serial/serial.h @@ -79,7 +79,10 @@ struct Timeout { #ifdef max #undef max #endif - static uint32_t max() { return std::numeric_limits::max(); } + static uint32_t max() + { + return std::numeric_limits::max(); + } /*! * Convenience function to generate Timeout structs using a * single absolute timeout. @@ -89,7 +92,10 @@ struct Timeout { * * \return Timeout struct that represents this simple timeout provided. */ - static Timeout simpleTimeout(uint32_t timeout) { return Timeout(max(), timeout, 0, timeout, 0); } + static Timeout simpleTimeout(uint32_t timeout) + { + return Timeout(max(), timeout, 0, timeout, 0); + } /*! Number of milliseconds between bytes received to timeout on. */ uint32_t inter_byte_timeout; @@ -636,9 +642,15 @@ class IOException : public std::exception { virtual ~IOException() throw() {} IOException(const IOException& other) : line_(other.line_), e_what_(other.e_what_), errno_(other.errno_) {} - int getErrorNumber() const { return errno_; } + int getErrorNumber() const + { + return errno_; + } - virtual const char* what() const throw() { return e_what_.c_str(); } + virtual const char* what() const throw() + { + return e_what_.c_str(); + } }; class PortNotOpenedException : public std::exception {