├── .travis.yml ├── CMakeLists.txt ├── cfg └── Servo.py ├── include ├── ft_scservo_driver │ ├── ft_scservo_driver.hpp │ └── ft_scservo_handle.hpp └── scservo │ └── scservo.h ├── package.xml └── src ├── scservo.c ├── scservo_driver.cpp ├── scservo_handle.cpp ├── scservo_node.cpp └── scservo_scan.c /.travis.yml: -------------------------------------------------------------------------------- 1 | language: cpp 2 | compiler: 3 | - gcc 4 | before_install: 5 | - sudo add-apt-repository "deb mirror://mirrors.ubuntu.com/mirrors.txt trusty main restricted universe multiverse" -y 6 | - echo "deb http://packages.ros.org/ros/ubuntu trusty main" | sudo tee -a /etc/apt/sources.list.d/ros-latest.list 7 | - wget https://raw.githubusercontent.com/ros/rosdistro/master/ros.key -O - | sudo apt-key add - 8 | - sudo apt-get update -qq 9 | - sudo apt-get install --only-upgrade -qq base-files 10 | install: 11 | - sudo apt-get install -qq python-rosdep 12 | - sudo rosdep init 13 | - rosdep update 14 | - rosdep install --from-path ./ --ignore-src --rosdistro $ROSDISTRO 15 | before_script: 16 | - source /opt/ros/$ROSDISTRO/setup.bash 17 | script: 18 | - mkdir build 19 | - cd build 20 | - > 21 | cmake .. 22 | -DCATKIN_BUILD_BINARY_PACKAGE="1" 23 | -DCMAKE_INSTALL_PREFIX="/opt/ros/$ROSDISTRO" 24 | -DCMAKE_PREFIX_PATH="/opt/ros/$ROSDISTRO" 25 | - make -j2 26 | - sudo make install -j2 27 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ft_scservo_driver) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | dynamic_reconfigure 6 | diagnostic_updater 7 | hardware_interface 8 | roscpp 9 | std_srvs 10 | ) 11 | 12 | find_package(Boost REQUIRED COMPONENTS thread) 13 | 14 | ################################################ 15 | ## Declare ROS messages, services and actions ## 16 | ################################################ 17 | 18 | generate_dynamic_reconfigure_options( 19 | cfg/Servo.py 20 | ) 21 | 22 | ################################### 23 | ## catkin specific configuration ## 24 | ################################### 25 | catkin_package( 26 | INCLUDE_DIRS include 27 | LIBRARIES scservo_ros 28 | CATKIN_DEPENDS diagnostic_updater dynamic_reconfigure hardware_interface roscpp std_srvs 29 | ) 30 | 31 | ########### 32 | ## Build ## 33 | ########### 34 | 35 | include_directories( 36 | include 37 | ${catkin_INCLUDE_DIRS} 38 | ) 39 | 40 | add_library(scservo src/scservo.c) 41 | add_library(scservo_ros src/scservo_driver.cpp src/scservo_handle.cpp) 42 | 43 | add_executable(scservo_node src/scservo_node.cpp) 44 | add_executable(scservo_scan src/scservo_scan.c) 45 | 46 | add_dependencies(scservo_ros ${PROJECT_NAME}_gencfg) 47 | 48 | target_link_libraries(scservo_node scservo_ros ${catkin_LIBRARIES}) 49 | target_link_libraries(scservo_ros scservo ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 50 | target_link_libraries(scservo_scan scservo) 51 | 52 | set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall") 53 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall") 54 | 55 | ############# 56 | ## Install ## 57 | ############# 58 | 59 | install(TARGETS scservo scservo_node scservo_ros scservo_scan 60 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 61 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 62 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 63 | ) 64 | 65 | install(DIRECTORY include/${PROJECT_NAME} include/scservo 66 | DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION} 67 | FILES_MATCHING PATTERN "*.h" PATTERN "*.hpp" 68 | PATTERN ".svn" EXCLUDE 69 | ) 70 | -------------------------------------------------------------------------------- /cfg/Servo.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | from dynamic_reconfigure.parameter_generator_catkin import * 3 | 4 | gen = ParameterGenerator() 5 | 6 | # Servo params 7 | gen.add("min_angle_limit", int_t, 0, "Minimum Angle Limit (ticks)", 100, 0, 1023); 8 | gen.add("max_angle_limit", int_t, 0, "Maximum Angle Limit (ticks)", 900, 0, 1023); 9 | gen.add("limit_temperature", int_t, 0, "Temperature Limit", 80, 0, 255); 10 | gen.add("max_limit_voltage", double_t, 0, "Maximum Voltage Limit", 25.0, 0.0, 25.5); 11 | gen.add("min_limit_voltage", double_t, 0, "Minimum Voltage Limit", 5.0, 0.0, 25.5); 12 | gen.add("max_torque", int_t, 0, "Maximum Torque", 1023, 0, 1023); 13 | gen.add("compliance_p", int_t, 0, "Proportional Gain", 30, 0, 255); 14 | gen.add("compliance_d", int_t, 0, "Derivative Gain", 30, 0, 255); 15 | gen.add("compliance_i", int_t, 0, "Integral Gain", 0, 0, 255); 16 | gen.add("imax", int_t, 0, "Maximum Integral Value", 0, 0, 255); 17 | 18 | # Driver params 19 | gen.add("rad_offset", double_t, 0, "Radians center offset", -1.890499397, -3.780, 0.0); 20 | gen.add("rad_per_tick", double_t, 0, "Radians per servo tick", -0.003695991, -0.005, -0.001); 21 | 22 | exit(gen.generate("ft_scservo_driver", "scservo_driver", "Servo")) 23 | -------------------------------------------------------------------------------- /include/ft_scservo_driver/ft_scservo_driver.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015, Scott K Logan 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, this 9 | * list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * 14 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 15 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 16 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 17 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 18 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 19 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 20 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 21 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 22 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 23 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 24 | */ 25 | 26 | #ifndef _scservo_driver_hpp 27 | #define _scservo_driver_hpp 28 | 29 | #include 30 | 31 | #include 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | 38 | #include "ft_scservo_driver/ServoConfig.h" 39 | 40 | namespace ft_scservo_driver 41 | { 42 | class Exception : public ros::Exception 43 | { 44 | public: 45 | Exception(const enum SC_ERROR sc_error); 46 | 47 | const enum SC_ERROR sc_error; 48 | }; 49 | 50 | class SCServoBus 51 | { 52 | private: 53 | friend class Servo; 54 | class Servo 55 | { 56 | public: 57 | Servo(class SCServoBus *parent, const ros::NodeHandle &nh_priv, const uint8_t id); 58 | ~Servo(); 59 | 60 | void diagUpdate(); 61 | void getStatus(double &velocity, double &position, double &effort); 62 | bool ping(); 63 | void setEnableTorque(bool enable); 64 | void setPosition(double position); 65 | void setVelocity(double velocity); 66 | 67 | const uint8_t id; 68 | 69 | private: 70 | void dynReCallback(ft_scservo_driver::ServoConfig &config, uint32_t level); 71 | ft_scservo_driver::ServoConfig getConfig(); 72 | std::string getMessageName() const; 73 | std::string getNodeHandleName() const; 74 | void mergeSettings(); 75 | void queryDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat); 76 | bool relaxCallback(std_srvs::Empty::Request &reques, std_srvs::Empty::Response &response); 77 | bool tenseCallback(std_srvs::Empty::Request &reques, std_srvs::Empty::Response &response); 78 | 79 | ros::NodeHandle nh_priv; 80 | diagnostic_updater::Updater diag; 81 | const dynamic_reconfigure::Server::CallbackType dyn_re_cb; 82 | boost::recursive_mutex dyn_re_mutex; 83 | dynamic_reconfigure::Server *dyn_re_srv; 84 | struct sc_settings last_settings; 85 | class SCServoBus *parent; 86 | double rad_offset; 87 | double rad_per_tick; 88 | ros::ServiceServer relax_service; 89 | ros::ServiceServer tense_service; 90 | const std::string this_name; 91 | }; 92 | 93 | public: 94 | SCServoBus(const ros::NodeHandle &nh, const ros::NodeHandle &nh_priv); 95 | ~SCServoBus(); 96 | 97 | void close(); 98 | void getStatus(int id, double &velocity, double &position, double &effort); 99 | void open(); 100 | void setEnableTorque(int id, bool enable); 101 | void setPosition(int id, double position); 102 | void setVelocity(int id, double velocity); 103 | void start(); 104 | bool stat(); 105 | void stop(); 106 | 107 | static const int default_baud; 108 | static const std::string default_port; 109 | static const int default_timeout; 110 | 111 | private: 112 | void diagTimerCallback(const ros::TimerEvent &event); 113 | 114 | bool active; 115 | int baud; 116 | ros::Timer diag_timer; 117 | boost::recursive_mutex io_mutex; 118 | const ros::NodeHandle nh; 119 | const ros::NodeHandle nh_priv; 120 | std::string port; 121 | int scd; 122 | std::vector servo_list; 123 | std::map servos; 124 | const std::string this_name; 125 | int timeout; 126 | }; 127 | } 128 | 129 | #endif /* _scservo_driver_hpp */ 130 | -------------------------------------------------------------------------------- /include/ft_scservo_driver/ft_scservo_handle.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015, Scott K Logan 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, this 9 | * list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * 14 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 15 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 16 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 17 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 18 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 19 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 20 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 21 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 22 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 23 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 24 | */ 25 | 26 | #ifndef _scservo_handle_hpp 27 | #define _scservo_handle_hpp 28 | 29 | #include 30 | #include 31 | #include 32 | 33 | #include "ft_scservo_driver/ft_scservo_driver.hpp" 34 | 35 | namespace ft_scservo_driver 36 | { 37 | class SCServoHandle 38 | { 39 | public: 40 | enum OPERATION_MODE 41 | { 42 | OPERATION_MODE_POSITION = 0, 43 | OPERATION_MODE_VELOCITY, 44 | }; 45 | 46 | SCServoHandle(const ros::NodeHandle &nh, const ros::NodeHandle &nh_priv); 47 | ~SCServoHandle(); 48 | 49 | void registerPositionHandles(hardware_interface::PositionActuatorInterface &api); 50 | void registerStateHandles(hardware_interface::ActuatorStateInterface &asi); 51 | void registerVelocityHandles(hardware_interface::VelocityActuatorInterface &avi); 52 | 53 | void read(); 54 | void write(); 55 | 56 | private: 57 | const ros::NodeHandle nh; 58 | const ros::NodeHandle nh_priv; 59 | 60 | ft_scservo_driver::SCServoBus scs; 61 | int servo_count; 62 | std::vector servos; 63 | std::string this_name; 64 | 65 | std::vector actuator_mode; 66 | std::vector actuator_engaged; 67 | 68 | std::vector actuator_cmd_pos; 69 | std::vector actuator_cmd_vel; 70 | std::vector actuator_eff; 71 | std::vector actuator_pos; 72 | std::vector actuator_vel; 73 | 74 | std::vector actuator_handle; 75 | std::vector actuator_handle_cmd_pos; 76 | std::vector actuator_handle_cmd_vel; 77 | }; 78 | } 79 | 80 | #endif /* _scservo_handle_hpp */ 81 | -------------------------------------------------------------------------------- /include/scservo/scservo.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015, Scott K Logan 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, this 9 | * list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * 14 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 15 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 16 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 17 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 18 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 19 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 20 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 21 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 22 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 23 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 24 | */ 25 | 26 | #ifndef _scservo_h 27 | #define _scservo_h 28 | 29 | #include 30 | 31 | #define SC_MAX_DESCRIPTORS 256 32 | #define SC_MAX_ID 0xFE 33 | #define SC_BROADCAST_ID 0xFE 34 | #define SC_TIMEOUT_DEFAULT 1 35 | 36 | #ifdef __cplusplus 37 | extern "C" { 38 | #endif 39 | 40 | enum SC_BAUD 41 | { 42 | SC_BAUD_1M = 0, 43 | SC_BAUD_500K = 1, 44 | SC_BAUD_250K = 2, 45 | SC_BAUD_128K = 3, 46 | SC_BAUD_115200 = 4, 47 | SC_BAUD_76800 = 5, 48 | SC_BAUD_57600 = 6, 49 | SC_BAUD_38400 = 7, 50 | SC_BAUD_MAX 51 | }; 52 | 53 | enum SC_FW_VER 54 | { 55 | SC_FW_SC00 = 0, 56 | SC_FW_SC10 = 1, 57 | SC_FW_MAX 58 | }; 59 | 60 | enum SC_ERROR 61 | { 62 | SC_SUCCESS = 0, 63 | SC_ERROR_IO = -1, 64 | SC_ERROR_INVALID_PARAM = -2, 65 | SC_ERROR_ACCESS = -3, 66 | SC_ERROR_NO_DEVICE = -4, 67 | SC_ERROR_NOT_FOUND = -5, 68 | SC_ERROR_BUSY = -6, 69 | SC_ERROR_TIMEOUT = -7, 70 | SC_ERROR_NO_MEM = -11, 71 | SC_ERROR_OTHER = -99, 72 | SC_ERROR_INVALID_RESPONSE = -100, 73 | SC_ERROR_CHECKSUM_FAILURE = -101, 74 | SC_ERROR_NOT_CONNECTED = -102, 75 | }; 76 | 77 | enum SC_FAULT 78 | { 79 | SC_FAULT_NONE = 0, 80 | SC_FAULT_VOLTAGE = (1 << 0), 81 | SC_FAULT_TEMPERATURE = ( 1 << 2), 82 | }; 83 | 84 | struct sc_info 85 | { 86 | uint16_t model; 87 | uint16_t version; 88 | uint8_t reserved1; 89 | uint8_t id; 90 | uint8_t baud; 91 | } __attribute__((packed)); 92 | 93 | struct sc_settings 94 | { 95 | uint16_t min_angle_limit; 96 | uint16_t max_angle_limit; 97 | uint8_t limit_temperature; 98 | uint8_t max_limit_voltage; 99 | uint8_t min_limit_voltage; 100 | uint16_t max_torque; 101 | uint8_t alarm_led; 102 | uint8_t alarm_shutdown; 103 | uint8_t reserved1; 104 | uint8_t compliance_p; 105 | uint8_t compliance_d; 106 | uint8_t compliance_i; 107 | uint16_t punch; 108 | uint8_t cw_dead; 109 | uint8_t ccw_dead; 110 | uint16_t imax; 111 | } __attribute__((packed)); 112 | 113 | struct sc_status 114 | { 115 | uint16_t present_position; 116 | int16_t present_speed; 117 | int16_t present_load; 118 | } __attribute__((packed)); 119 | 120 | struct sc_diag 121 | { 122 | uint8_t voltage; 123 | uint8_t temperature; 124 | uint8_t registered_instruction; 125 | uint8_t error; 126 | } __attribute__((packed)); 127 | 128 | #define SC_MAX_MSG sizeof(struct sc_settings) 129 | 130 | /** 131 | * Functions 132 | */ 133 | void sc_close(const int scd); 134 | int sc_open(const char *port, const enum SC_BAUD baud, const uint8_t timeout); 135 | int sc_ping(const int scd, const uint8_t id); 136 | int sc_read_diag(const int scd, const uint8_t id, struct sc_diag *diag); 137 | int sc_read_goal(const int scd, const uint8_t id, uint16_t *goal_speed, uint16_t *goal_position); 138 | int sc_read_goal_position(const int scd, const uint8_t id, uint16_t *goal_position); 139 | int sc_read_goal_speed(const int scd, const uint8_t id, int16_t *goal_speed); 140 | int sc_read_info(const int scd, const uint8_t id, struct sc_info *info); 141 | int sc_read_model(const int scd, const uint8_t id, uint16_t *model); 142 | int sc_read_position(const int scd, const uint8_t id, uint16_t *position); 143 | int sc_read_settings(const int scd, const uint8_t id, struct sc_settings *settings); 144 | int sc_read_speed(const int scd, const uint8_t id, int16_t *speed); 145 | int sc_read_status(const int scd, const uint8_t id, struct sc_status *status); 146 | int sc_read_status_and_diag(const int scd, const uint8_t id, struct sc_status *status, struct sc_diag *diag); 147 | int sc_read_temperature(const int scd, const uint8_t id, uint8_t *temperature); 148 | int sc_read_torque_enable(const int scd, const uint8_t id, uint8_t *enable); 149 | int sc_read_version(const int scd, const uint8_t id, uint8_t *major, uint8_t *minor); 150 | int sc_read_vir_position(const int scd, const uint8_t id, uint16_t *vir_position); 151 | int sc_read_voltage(const int scd, const uint8_t id, uint8_t *voltage); 152 | const char * sc_strerror(const int error); 153 | const char * sc_strfault(const enum SC_FAULT fault); 154 | int sc_write_goal(const int scd, const uint8_t id, const uint16_t speed, const uint16_t position); 155 | int sc_write_goal_position(const int scd, const uint8_t id, const uint16_t position); 156 | int sc_write_goal_speed(const int scd, const uint8_t id, const uint16_t speed); 157 | int sc_write_settings(const int scd, const uint8_t id, const struct sc_settings *settings); 158 | int sc_write_torque_enable(const int scd, const uint8_t id, const uint8_t enable); 159 | 160 | #ifdef __cplusplus 161 | } 162 | #endif 163 | 164 | #endif /* _scservo_h */ 165 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ft_scservo_driver 4 | 0.1.0 5 | ROS Driver for the SCServo line by FEETECH 6 | 7 | Scott K Logan 8 | 9 | BSD 10 | 11 | 12 | 13 | Scott K Logan 14 | 15 | catkin 16 | diagnostic_updater 17 | dynamic_reconfigure 18 | hardware_interface 19 | roscpp 20 | std_srvs 21 | diagnostic_updater 22 | dynamic_reconfigure 23 | hardware_interface 24 | roscpp 25 | std_srvs 26 | 27 | -------------------------------------------------------------------------------- /src/scservo.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015, Scott K Logan 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, this 9 | * list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * 14 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 15 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 16 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 17 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 18 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 19 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 20 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 21 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 22 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 23 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 24 | */ 25 | 26 | #include "scservo/scservo.h" 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | /** 36 | * Private Definitions 37 | */ 38 | #define SC_START_BYTE 0xFF 39 | #define SC_ENDIAN_SWAP_16(X) ((*(X) << 8) | (*(X) >> 8)) 40 | #define SC_ENDIAN_SWAP_16_ASSIGN(X) *((uint16_t *)X) = SC_ENDIAN_SWAP_16((uint16_t *)X) 41 | 42 | struct sc_priv 43 | { 44 | char *port; 45 | enum SC_BAUD baud; 46 | int fd; 47 | uint8_t timeout; 48 | }; 49 | 50 | enum SC10_CMD 51 | { 52 | SC10_CMD_PING = 0x01, 53 | SC10_CMD_READ = 0x02, 54 | SC10_CMD_WRITE = 0x03, 55 | SC10_CMD_REG_WRITE = 0x04, 56 | SC10_CMD_ACTION = 0x05, 57 | SC10_CMD_RESET = 0x06, 58 | SC10_CMD_SYNC_WRITE = 0x83 59 | }; 60 | 61 | enum SC10_REG 62 | { 63 | SC10_MODEL_NUMBER_L = 0, 64 | SC10_MODEL_NUMBER_H = 1, 65 | SC10_VERSION_L = 3, 66 | SC10_VERSION_H = 4, 67 | SC10_ID = 5, 68 | SC10_BAUD_RATE = 6, 69 | SC10_RETURN_DELAY_TIME = 7, 70 | SC10_RETURN_LEVEL = 8, 71 | SC10_MIN_ANGLE_LIMIT_L = 9, 72 | SC10_MIN_ANGLE_LIMIT_H = 10, 73 | SC10_MAX_ANGLE_LIMIT_L = 11, 74 | SC10_MAX_ANGLE_LIMIT_H = 12, 75 | SC10_LIMIT_TEMPERATURE = 13, 76 | SC10_MAX_LIMIT_VOLTAGE = 14, 77 | SC10_MIN_LIMIT_VOLTAGE = 15, 78 | SC10_MAX_TORQUE_L = 16, 79 | SC10_MAX_TORQUE_H = 17, 80 | SC10_ALARM_LED = 18, 81 | SC10_ALARM_SHUTDOWN = 19, 82 | SC10_COMPLIANCE_P = 21, 83 | SC10_COMPLIANCE_D = 22, 84 | SC10_COMPLIANCE_I = 23, 85 | SC10_PUNCH_H = 24, 86 | SC10_PUNCH_L = 25, 87 | SC10_CW_DEAD = 26, 88 | SC10_CCW_DEAD = 27, 89 | SC10_IMAX_L = 28, 90 | SC10_IMAX_H = 29, 91 | SC10_OFFSET_L = 30, 92 | SC10_OFFSET_H = 31, 93 | SC10_TORQUE_ENABLE = 40, 94 | SC10_LED = 41, 95 | SC10_GOAL_POSITION_L = 42, 96 | SC10_GOAL_POSITION_H = 43, 97 | SC10_GOAL_SPEED_L = 44, 98 | SC10_GOAL_SPEED_H = 45, 99 | SC10_LOCK = 48, 100 | SC10_PRESENT_POSITION_L = 56, 101 | SC10_PRESENT_POSITION_H = 57, 102 | SC10_PRESENT_SPEED_L = 58, 103 | SC10_PRESENT_SPEED_H = 59, 104 | SC10_PRESENT_LOAD_L = 60, 105 | SC10_PRESENT_LOAD_H = 61, 106 | SC10_PRESENT_VOLTAGE = 62, 107 | SC10_PRESENT_TEMPERATURE = 63, 108 | SC10_REGISTERED_INSTRUCTION = 64, 109 | SC10_ERROR = 65, 110 | SC10_MOVING = 66, 111 | SC10_VIR_POSITION_L = 67, 112 | SC10_VIR_POSITION_H = 68, 113 | SC10_CURRENT_L = 69, 114 | SC10_CURRENT_H = 70 115 | }; 116 | 117 | /** 118 | * Private Data 119 | */ 120 | static int sc_initialized = 0; 121 | static struct sc_priv *scds[SC_MAX_DESCRIPTORS] = { NULL }; 122 | static const speed_t sc_baud_termios[SC_BAUD_MAX] = 123 | { 124 | // SC_BAUD_1M 125 | B1000000, 126 | // SC_BAUD_500K 127 | B500000, 128 | // SC_BAUD_250K 129 | B0, 130 | // SC_BAUD_128K 131 | B0, 132 | // SC_BAUD_115200 133 | B115200, 134 | // SC_BAUD_76800 135 | B0, 136 | // SC_BAUD_57600 137 | B57600, 138 | // SC_BAUD_38400 139 | B38400 140 | }; 141 | 142 | /** 143 | * Private Function Prototypes 144 | */ 145 | static uint8_t sc_checksum(const uint8_t *msg); 146 | static int sc_errno(const int fallback); 147 | static void sc_exit(void); 148 | static int sc_flush(const int scd); 149 | static int sc_init(void); 150 | static int sc_next_descriptor(void); 151 | static int sc_read_msg(const int scd, void *msg, const uint8_t max_len); 152 | static int sc_read_reg(const int scd, const uint8_t id, const uint8_t reg, void *val, const uint8_t len); 153 | static inline int sc_read_reg8(const int scd, const uint8_t id, const uint8_t reg, uint8_t *val); 154 | static inline int sc_read_reg16(const int scd, const uint8_t id, const uint8_t reg, uint16_t *val); 155 | static int sc_write_msg(const int scd, const uint8_t id, const void *msg, const uint8_t len); 156 | static int sc_write_reg(const int scd, const uint8_t id, const uint8_t reg, const void *val, const uint8_t len); 157 | static inline int sc_write_reg8(const int scd, const uint8_t id, const uint8_t reg, const uint8_t val); 158 | static inline int sc_write_reg16(const int scd, const uint8_t id, const uint8_t reg, const uint16_t val); 159 | 160 | /** 161 | * Public Functions 162 | */ 163 | void sc_close(const int scd) 164 | { 165 | if (scd >= 0 && scd < SC_MAX_DESCRIPTORS && scds[scd] != NULL) 166 | { 167 | close(scds[scd]->fd); 168 | free(scds[scd]->port); 169 | free(scds[scd]); 170 | scds[scd] = NULL; 171 | } 172 | } 173 | 174 | int sc_open(const char *port, const enum SC_BAUD baud, const uint8_t timeout) 175 | { 176 | int fd; 177 | struct termios options; 178 | int ret; 179 | int scd; 180 | 181 | if (port == NULL || port[0] == '\0' || baud < 0 || baud >= SC_BAUD_MAX || timeout < 1 || sc_baud_termios[baud] == B0) 182 | { 183 | ret = SC_ERROR_INVALID_PARAM; 184 | goto sc_pre_fail; 185 | } 186 | 187 | if (sc_initialized != 1) 188 | { 189 | ret = sc_init(); 190 | if (ret != SC_SUCCESS) 191 | { 192 | sc_initialized = 0; 193 | ret = SC_ERROR_OTHER; 194 | goto sc_pre_fail; 195 | } 196 | } 197 | 198 | fd = open(port, O_NOCTTY | O_RDWR | O_SYNC); 199 | if (fd < 0) 200 | { 201 | ret = sc_errno(SC_ERROR_OTHER); 202 | goto sc_pre_fail; 203 | } 204 | 205 | scd = sc_next_descriptor(); 206 | if (scd < 0) 207 | { 208 | ret = scd; 209 | goto sc_close_fail; 210 | } 211 | 212 | scds[scd] = malloc(sizeof(struct sc_priv)); 213 | if (scds[scd] == NULL) 214 | { 215 | ret = SC_ERROR_NO_MEM; 216 | goto sc_close_fail; 217 | } 218 | 219 | scds[scd]->port = malloc(strlen(port) + 1); 220 | if (scds[scd]->port == NULL) 221 | { 222 | ret = SC_ERROR_NO_MEM; 223 | goto sc_free_fail; 224 | } 225 | 226 | strcpy(scds[scd]->port, port); 227 | scds[scd]->baud = baud; 228 | scds[scd]->fd = fd; 229 | scds[scd]->timeout = timeout; 230 | 231 | ret = tcgetattr(fd, &options); 232 | if (ret != 0) 233 | { 234 | ret = SC_ERROR_IO; 235 | goto sc_free_fail; 236 | } 237 | 238 | cfmakeraw(&options); 239 | options.c_cc[VMIN] = 0; 240 | options.c_cc[VTIME] = timeout; 241 | 242 | ret = cfsetispeed(&options, sc_baud_termios[baud]); 243 | if (ret != 0) 244 | { 245 | ret = SC_ERROR_IO; 246 | goto sc_free_fail; 247 | } 248 | 249 | 250 | ret = cfsetospeed(&options, sc_baud_termios[baud]); 251 | if (ret != 0) 252 | { 253 | ret = SC_ERROR_IO; 254 | goto sc_free_fail; 255 | } 256 | 257 | ret = tcsetattr(fd, TCSANOW, &options); 258 | if (ret != 0) 259 | { 260 | ret = SC_ERROR_IO; 261 | goto sc_free_fail; 262 | } 263 | 264 | return scd; 265 | 266 | sc_free_fail: 267 | free(scds[scd]->port); 268 | free(scds[scd]); 269 | scds[scd] = NULL; 270 | 271 | sc_close_fail: 272 | close(fd); 273 | 274 | sc_pre_fail: 275 | return ret; 276 | } 277 | 278 | int sc_ping(const int scd, const uint8_t id) 279 | { 280 | static const uint8_t msg[1] = { SC10_CMD_PING }; 281 | 282 | uint8_t recv[6]; 283 | int ret; 284 | 285 | if (scd < 0 || scd > SC_MAX_DESCRIPTORS || scds[scd] == NULL || id > SC_MAX_ID) 286 | { 287 | return SC_ERROR_INVALID_PARAM; 288 | } 289 | 290 | ret = sc_flush(scd); 291 | if (ret != SC_SUCCESS) 292 | { 293 | return ret; 294 | } 295 | 296 | ret = sc_write_msg(scd, id, msg, 1); 297 | if (ret != SC_SUCCESS) 298 | { 299 | return ret; 300 | } 301 | 302 | ret = sc_read_msg(scd, recv, 6); 303 | if (ret != SC_SUCCESS) 304 | { 305 | // If it was a timeout, that means the device isn't present 306 | if (ret == SC_ERROR_TIMEOUT) 307 | { 308 | return 0; 309 | } 310 | else 311 | { 312 | return ret; 313 | } 314 | } 315 | 316 | if (recv[0] != 0xFF || recv[1] != 0xFF || recv[3] != 2 || recv[4] != 0) 317 | { 318 | return SC_ERROR_INVALID_RESPONSE; 319 | } 320 | 321 | return 1; 322 | } 323 | 324 | int sc_read_diag(const int scd, const uint8_t id, struct sc_diag *diag) 325 | { 326 | int ret; 327 | uint8_t buf[4]; 328 | 329 | if (scd < 0 || scd > SC_MAX_DESCRIPTORS || scds[scd] == NULL || id > SC_MAX_ID) 330 | { 331 | return SC_ERROR_INVALID_PARAM; 332 | } 333 | 334 | if (diag == NULL) 335 | { 336 | return SC_ERROR_INVALID_PARAM; 337 | } 338 | 339 | ret = sc_read_reg(scd, id, SC10_PRESENT_VOLTAGE, buf, 4); 340 | if (ret == SC_SUCCESS) 341 | { 342 | diag->voltage = buf[0]; 343 | diag->temperature = buf[1]; 344 | diag->error = buf[3]; 345 | } 346 | 347 | return ret; 348 | } 349 | 350 | int sc_read_goal(const int scd, const uint8_t id, uint16_t *goal_speed, uint16_t *goal_position) 351 | { 352 | int ret; 353 | uint16_t buf[2]; 354 | 355 | if (scd < 0 || scd > SC_MAX_DESCRIPTORS || scds[scd] == NULL || id > SC_MAX_ID) 356 | { 357 | return SC_ERROR_INVALID_PARAM; 358 | } 359 | 360 | if (goal_speed == NULL || goal_position == NULL) 361 | { 362 | return SC_ERROR_INVALID_PARAM; 363 | } 364 | 365 | ret = sc_read_reg(scd, id, SC10_GOAL_POSITION_L, buf, 4); 366 | if (ret == SC_SUCCESS) 367 | { 368 | *goal_speed = SC_ENDIAN_SWAP_16(&buf[1]); 369 | *goal_position = SC_ENDIAN_SWAP_16(&buf[0]); 370 | } 371 | 372 | return ret; 373 | } 374 | 375 | int sc_read_goal_position(const int scd, const uint8_t id, uint16_t *goal_position) 376 | { 377 | if (scd < 0 || scd > SC_MAX_DESCRIPTORS || scds[scd] == NULL || id > SC_MAX_ID) 378 | { 379 | return SC_ERROR_INVALID_PARAM; 380 | } 381 | 382 | if (goal_position == NULL) 383 | { 384 | return SC_ERROR_INVALID_PARAM; 385 | } 386 | 387 | return sc_read_reg16(scd, id, SC10_GOAL_POSITION_L, goal_position); 388 | } 389 | 390 | int sc_read_goal_speed(const int scd, const uint8_t id, int16_t *goal_speed) 391 | { 392 | if (scd < 0 || scd > SC_MAX_DESCRIPTORS || scds[scd] == NULL || id > SC_MAX_ID) 393 | { 394 | return SC_ERROR_INVALID_PARAM; 395 | } 396 | 397 | if (goal_speed == NULL) 398 | { 399 | return SC_ERROR_INVALID_PARAM; 400 | } 401 | 402 | return sc_read_reg16(scd, id, SC10_GOAL_SPEED_L, (uint16_t *)goal_speed); 403 | } 404 | 405 | int sc_read_info(const int scd, const uint8_t id, struct sc_info *info) 406 | { 407 | if (scd < 0 || scd > SC_MAX_DESCRIPTORS || scds[scd] == NULL || id > SC_MAX_ID) 408 | { 409 | return SC_ERROR_INVALID_PARAM; 410 | } 411 | 412 | if (info == NULL) 413 | { 414 | return SC_ERROR_INVALID_PARAM; 415 | } 416 | 417 | return sc_read_reg(scd, id, SC10_MODEL_NUMBER_L, info, sizeof(struct sc_info)); 418 | } 419 | 420 | int sc_read_model(const int scd, const uint8_t id, uint16_t *model) 421 | { 422 | if (scd < 0 || scd > SC_MAX_DESCRIPTORS || scds[scd] == NULL || id > SC_MAX_ID) 423 | { 424 | return SC_ERROR_INVALID_PARAM; 425 | } 426 | 427 | if (model == NULL) 428 | { 429 | return SC_ERROR_INVALID_PARAM; 430 | } 431 | 432 | return sc_read_reg16(scd, id, SC10_MODEL_NUMBER_L, model); 433 | } 434 | 435 | int sc_read_position(const int scd, const uint8_t id, uint16_t *position) 436 | { 437 | if (scd < 0 || scd > SC_MAX_DESCRIPTORS || scds[scd] == NULL || id > SC_MAX_ID) 438 | { 439 | return SC_ERROR_INVALID_PARAM; 440 | } 441 | 442 | if (position == NULL) 443 | { 444 | return SC_ERROR_INVALID_PARAM; 445 | } 446 | 447 | return sc_read_reg16(scd, id, SC10_PRESENT_POSITION_L, position); 448 | } 449 | 450 | int sc_read_settings(const int scd, const uint8_t id, struct sc_settings *settings) 451 | { 452 | int ret; 453 | 454 | if (scd < 0 || scd > SC_MAX_DESCRIPTORS || scds[scd] == NULL || id > SC_MAX_ID) 455 | { 456 | return SC_ERROR_INVALID_PARAM; 457 | } 458 | 459 | if (settings == NULL) 460 | { 461 | return SC_ERROR_INVALID_PARAM; 462 | } 463 | 464 | ret = sc_read_reg(scd, id, SC10_MIN_ANGLE_LIMIT_L, settings, sizeof(struct sc_settings)); 465 | if (ret == SC_SUCCESS) 466 | { 467 | SC_ENDIAN_SWAP_16_ASSIGN(&settings->min_angle_limit); 468 | SC_ENDIAN_SWAP_16_ASSIGN(&settings->max_angle_limit); 469 | SC_ENDIAN_SWAP_16_ASSIGN(&settings->max_torque); 470 | SC_ENDIAN_SWAP_16_ASSIGN(&settings->imax); 471 | } 472 | 473 | return ret; 474 | } 475 | 476 | int sc_read_speed(const int scd, const uint8_t id, int16_t *speed) 477 | { 478 | int ret; 479 | 480 | if (scd < 0 || scd > SC_MAX_DESCRIPTORS || scds[scd] == NULL || id > SC_MAX_ID) 481 | { 482 | return SC_ERROR_INVALID_PARAM; 483 | } 484 | 485 | if (speed == NULL) 486 | { 487 | return SC_ERROR_INVALID_PARAM; 488 | } 489 | 490 | ret = sc_read_reg16(scd, id, SC10_PRESENT_SPEED_L, (uint16_t *)speed); 491 | if (ret == SC_SUCCESS && *speed & (1 << 10)) 492 | { 493 | *speed = -(*speed & 1023); 494 | } 495 | 496 | return ret; 497 | } 498 | 499 | int sc_read_status(const int scd, const uint8_t id, struct sc_status *status) 500 | { 501 | int ret; 502 | 503 | if (scd < 0 || scd > SC_MAX_DESCRIPTORS || scds[scd] == NULL || id > SC_MAX_ID) 504 | { 505 | return SC_ERROR_INVALID_PARAM; 506 | } 507 | 508 | if (status == NULL) 509 | { 510 | return SC_ERROR_INVALID_PARAM; 511 | } 512 | 513 | ret = sc_read_reg(scd, id, SC10_PRESENT_POSITION_L, status, sizeof(struct sc_status)); 514 | if (ret == SC_SUCCESS) 515 | { 516 | SC_ENDIAN_SWAP_16_ASSIGN(&status->present_position); 517 | SC_ENDIAN_SWAP_16_ASSIGN(&status->present_speed); 518 | SC_ENDIAN_SWAP_16_ASSIGN(&status->present_load); 519 | if (status->present_speed & (1 << 10)) 520 | { 521 | status->present_speed = -(status->present_speed & 1023); 522 | } 523 | if (status->present_load & (1 << 10)) 524 | { 525 | status->present_load = -(status->present_load & 1023); 526 | } 527 | } 528 | 529 | return ret; 530 | } 531 | 532 | int sc_read_status_and_diag(const int scd, const uint8_t id, struct sc_status *status, struct sc_diag *diag) 533 | { 534 | int ret; 535 | uint8_t buf[sizeof(struct sc_status) + sizeof(struct sc_diag)]; 536 | 537 | if (scd < 0 || scd > SC_MAX_DESCRIPTORS || scds[scd] == NULL || id > SC_MAX_ID) 538 | { 539 | return SC_ERROR_INVALID_PARAM; 540 | } 541 | 542 | if (status == NULL || diag == NULL) 543 | { 544 | return SC_ERROR_INVALID_PARAM; 545 | } 546 | 547 | ret = sc_read_reg(scd, id, SC10_PRESENT_POSITION_L, buf, sizeof(struct sc_status) + sizeof(struct sc_diag)); 548 | if (ret == SC_SUCCESS) 549 | { 550 | status->present_position = SC_ENDIAN_SWAP_16((uint16_t *)&buf[0]); 551 | status->present_speed = SC_ENDIAN_SWAP_16((uint16_t *)&buf[2]); 552 | status->present_load = SC_ENDIAN_SWAP_16((uint16_t *)&buf[4]); 553 | diag->voltage = buf[6]; 554 | diag->temperature = buf[7]; 555 | diag->error = buf[9]; 556 | if (status->present_speed & (1 << 10)) 557 | { 558 | status->present_speed = -(status->present_speed & 1023); 559 | } 560 | if (status->present_load & (1 << 10)) 561 | { 562 | status->present_load = -(status->present_load & 1023); 563 | } 564 | } 565 | 566 | return ret; 567 | } 568 | 569 | int sc_read_temperature(const int scd, const uint8_t id, uint8_t *temperature) 570 | { 571 | if (scd < 0 || scd > SC_MAX_DESCRIPTORS || scds[scd] == NULL || id > SC_MAX_ID) 572 | { 573 | return SC_ERROR_INVALID_PARAM; 574 | } 575 | 576 | if (temperature == NULL) 577 | { 578 | return SC_ERROR_INVALID_PARAM; 579 | } 580 | 581 | return sc_read_reg8(scd, id, SC10_PRESENT_TEMPERATURE, temperature); 582 | } 583 | 584 | int sc_read_torque_enable(const int scd, const uint8_t id, uint8_t *enable) 585 | { 586 | if (scd < 0 || scd > SC_MAX_DESCRIPTORS || scds[scd] == NULL || id > SC_MAX_ID) 587 | { 588 | return SC_ERROR_INVALID_PARAM; 589 | } 590 | 591 | if (enable == NULL) 592 | { 593 | return SC_ERROR_INVALID_PARAM; 594 | } 595 | 596 | return sc_read_reg8(scd, id, SC10_TORQUE_ENABLE, enable); 597 | } 598 | 599 | int sc_read_version(const int scd, const uint8_t id, uint8_t *major, uint8_t *minor) 600 | { 601 | int ret; 602 | uint16_t version; 603 | 604 | if (scd < 0 || scd > SC_MAX_DESCRIPTORS || scds[scd] == NULL || id > SC_MAX_ID) 605 | { 606 | return SC_ERROR_INVALID_PARAM; 607 | } 608 | 609 | if (major == NULL || minor == NULL) 610 | { 611 | return SC_ERROR_INVALID_PARAM; 612 | } 613 | 614 | ret = sc_read_reg16(scd, id, SC10_VERSION_L, &version); 615 | if (ret == SC_SUCCESS) 616 | { 617 | *major = version >> 8; 618 | *minor = version & 0xFF; 619 | } 620 | 621 | return ret; 622 | } 623 | 624 | int sc_read_vir_position(const int scd, const uint8_t id, uint16_t *vir_position) 625 | { 626 | if (scd < 0 || scd > SC_MAX_DESCRIPTORS || scds[scd] == NULL || id > SC_MAX_ID) 627 | { 628 | return SC_ERROR_INVALID_PARAM; 629 | } 630 | 631 | if (vir_position == NULL) 632 | { 633 | return SC_ERROR_INVALID_PARAM; 634 | } 635 | 636 | return sc_read_reg16(scd, id, SC10_VIR_POSITION_L, vir_position); 637 | } 638 | 639 | int sc_read_voltage(const int scd, const uint8_t id, uint8_t *voltage) 640 | { 641 | if (scd < 0 || scd > SC_MAX_DESCRIPTORS || scds[scd] == NULL || id > SC_MAX_ID) 642 | { 643 | return SC_ERROR_INVALID_PARAM; 644 | } 645 | 646 | if (voltage == NULL) 647 | { 648 | return SC_ERROR_INVALID_PARAM; 649 | } 650 | 651 | return sc_read_reg8(scd, id, SC10_PRESENT_VOLTAGE, voltage); 652 | } 653 | 654 | int sc_write_goal_position(const int scd, const uint8_t id, const uint16_t position) 655 | { 656 | if (scd < 0 || scd > SC_MAX_DESCRIPTORS || scds[scd] == NULL || id > SC_MAX_ID) 657 | { 658 | return SC_ERROR_INVALID_PARAM; 659 | } 660 | 661 | return sc_write_reg16(scd, id, SC10_GOAL_POSITION_L, position); 662 | } 663 | 664 | int sc_write_goal_speed(const int scd, const uint8_t id, const uint16_t speed) 665 | { 666 | if (scd < 0 || scd > SC_MAX_DESCRIPTORS || scds[scd] == NULL || id > SC_MAX_ID) 667 | { 668 | return SC_ERROR_INVALID_PARAM; 669 | } 670 | 671 | return sc_write_reg16(scd, id, SC10_GOAL_SPEED_L, speed); 672 | } 673 | 674 | int sc_write_goal(const int scd, const uint8_t id, const uint16_t speed, const uint16_t position) 675 | { 676 | const uint16_t buf[2] = { (position << 8) | (position >> 8), (speed << 8) | (speed >> 8) }; 677 | 678 | if (scd < 0 || scd > SC_MAX_DESCRIPTORS || scds[scd] == NULL || id > SC_MAX_ID) 679 | { 680 | return SC_ERROR_INVALID_PARAM; 681 | } 682 | 683 | return sc_write_reg(scd, id, SC10_GOAL_POSITION_L, buf, 4); 684 | } 685 | 686 | int sc_write_settings(const int scd, const uint8_t id, const struct sc_settings *settings) 687 | { 688 | struct sc_settings buf; 689 | 690 | if (scd < 0 || scd > SC_MAX_DESCRIPTORS || scds[scd] == NULL || id > SC_MAX_ID) 691 | { 692 | return SC_ERROR_INVALID_PARAM; 693 | } 694 | 695 | if (settings == NULL) 696 | { 697 | return SC_ERROR_INVALID_PARAM; 698 | } 699 | 700 | buf = *settings; 701 | 702 | SC_ENDIAN_SWAP_16_ASSIGN(&buf.min_angle_limit); 703 | SC_ENDIAN_SWAP_16_ASSIGN(&buf.max_angle_limit); 704 | SC_ENDIAN_SWAP_16_ASSIGN(&buf.max_torque); 705 | SC_ENDIAN_SWAP_16_ASSIGN(&buf.imax); 706 | 707 | return sc_write_reg(scd, id, SC10_MIN_ANGLE_LIMIT_L, &buf, sizeof(struct sc_settings)); 708 | } 709 | 710 | int sc_write_torque_enable(const int scd, const uint8_t id, const uint8_t enable) 711 | { 712 | if (scd < 0 || scd > SC_MAX_DESCRIPTORS || scds[scd] == NULL || id > SC_MAX_ID) 713 | { 714 | return SC_ERROR_INVALID_PARAM; 715 | } 716 | 717 | return sc_write_reg(scd, id, SC10_TORQUE_ENABLE, &enable, 1); 718 | } 719 | 720 | /** 721 | * Private Functions 722 | */ 723 | static uint8_t sc_checksum(const uint8_t *msg) 724 | { 725 | uint8_t ret = 0; 726 | uint8_t len = msg[3] + 3; 727 | 728 | while (len > 2) 729 | { 730 | ret += msg[--len]; 731 | } 732 | 733 | return ~ret; 734 | } 735 | 736 | static int sc_errno(const int fallback) 737 | { 738 | switch (errno) 739 | { 740 | case EPERM: 741 | case EACCES: 742 | return SC_ERROR_ACCESS; 743 | case ENOENT: 744 | case ENXIO: 745 | case ENODEV: 746 | case EISDIR: 747 | return SC_ERROR_NO_DEVICE; 748 | case ENOMEM: 749 | return SC_ERROR_NO_MEM; 750 | case EBUSY: 751 | return SC_ERROR_BUSY; 752 | } 753 | 754 | return fallback; 755 | } 756 | 757 | static void sc_exit(void) 758 | { 759 | int scd; 760 | 761 | for (scd = 0; scd < SC_MAX_DESCRIPTORS; scd++) 762 | { 763 | sc_close(scd); 764 | } 765 | } 766 | 767 | static int sc_flush(const int scd) 768 | { 769 | int ret; 770 | 771 | ret = tcflush(scds[scd]->fd, TCIOFLUSH); 772 | if (ret != 0) 773 | { 774 | return sc_errno(SC_ERROR_IO); 775 | } 776 | 777 | return SC_SUCCESS; 778 | } 779 | 780 | static int sc_init(void) 781 | { 782 | sc_initialized = 1; 783 | 784 | // Note that this only works on glibc *nix systems (not all *BSD) 785 | return (atexit(sc_exit) == 0) ? SC_SUCCESS : SC_ERROR_OTHER; 786 | } 787 | 788 | static int sc_next_descriptor(void) 789 | { 790 | int scd; 791 | 792 | for (scd = 0; scd < SC_MAX_DESCRIPTORS; scd++) 793 | { 794 | if (scds[scd] == NULL) 795 | { 796 | return scd; 797 | } 798 | } 799 | 800 | return SC_ERROR_NO_MEM; 801 | } 802 | 803 | static int sc_read_msg(const int scd, void *msg, const uint8_t max_len) 804 | { 805 | int ret; 806 | uint8_t len; 807 | uint8_t *ptr = msg; 808 | 809 | ret = read(scds[scd]->fd, ptr++, 1); 810 | if (ret == 0) 811 | { 812 | return SC_ERROR_TIMEOUT; 813 | } 814 | else if (ret != 1) 815 | { 816 | return sc_errno(SC_ERROR_IO); 817 | } 818 | 819 | ret = read(scds[scd]->fd, ptr++, 1); 820 | if (ret != 1) 821 | { 822 | return sc_errno(SC_ERROR_IO); 823 | } 824 | 825 | ret = read(scds[scd]->fd, ptr, 2); 826 | if (ret != 2) 827 | { 828 | if (ret == 1) 829 | { 830 | ret = read(scds[scd]->fd, &ptr[1], 1); 831 | if (ret != 1) 832 | { 833 | return sc_errno(SC_ERROR_IO); 834 | } 835 | } 836 | else 837 | { 838 | return sc_errno(SC_ERROR_IO); 839 | } 840 | } 841 | 842 | len = ptr[1]; 843 | 844 | if (len + 4 > max_len) 845 | { 846 | return SC_ERROR_NO_MEM; 847 | } 848 | 849 | ptr += 2; 850 | 851 | while (len > 0) 852 | { 853 | ret = read(scds[scd]->fd, ptr, len); 854 | if (ret < 1) 855 | { 856 | return sc_errno(SC_ERROR_IO); 857 | } 858 | 859 | ptr += ret; 860 | len -= ret; 861 | } 862 | 863 | if (sc_checksum(msg) != ptr[-1]) 864 | { 865 | return SC_ERROR_CHECKSUM_FAILURE; 866 | } 867 | 868 | return SC_SUCCESS; 869 | } 870 | 871 | static int sc_read_reg(const int scd, const uint8_t id, const uint8_t reg, void *val, const uint8_t len) 872 | { 873 | uint8_t buf[SC_MAX_MSG] = { SC10_CMD_READ, reg, len }; 874 | int ret; 875 | 876 | ret = sc_flush(scd); 877 | if (ret != SC_SUCCESS) 878 | { 879 | return ret; 880 | } 881 | 882 | ret = sc_write_msg(scd, id, buf, 3); 883 | if (ret != SC_SUCCESS) 884 | { 885 | return ret; 886 | } 887 | 888 | ret = sc_read_msg(scd, buf, len + 6); 889 | if (ret != SC_SUCCESS) 890 | { 891 | return ret; 892 | } 893 | 894 | if (buf[0] != 0xFF || buf[1] != 0xFF || buf[3] != len + 2) 895 | { 896 | return SC_ERROR_INVALID_RESPONSE; 897 | } 898 | 899 | memcpy(val, &buf[5], len); 900 | 901 | return SC_SUCCESS; 902 | } 903 | 904 | static inline int sc_read_reg8(const int scd, const uint8_t id, const uint8_t reg, uint8_t *val) 905 | { 906 | return sc_read_reg(scd, id, reg, val, 1); 907 | } 908 | 909 | static inline int sc_read_reg16(const int scd, const uint8_t id, const uint8_t reg, uint16_t *val) 910 | { 911 | int ret; 912 | 913 | ret = sc_read_reg(scd, id, reg, val, 2); 914 | if (ret == SC_SUCCESS) 915 | { 916 | SC_ENDIAN_SWAP_16_ASSIGN(val); 917 | } 918 | 919 | return ret; 920 | } 921 | 922 | const char * sc_strerror(const int error) 923 | { 924 | switch(error) 925 | { 926 | case SC_SUCCESS: 927 | return "Success"; 928 | case SC_ERROR_IO: 929 | return "Input/Output error"; 930 | case SC_ERROR_INVALID_PARAM: 931 | return "Invalid parameter"; 932 | case SC_ERROR_ACCESS: 933 | return "Access denied"; 934 | case SC_ERROR_NO_DEVICE: 935 | return "No such device"; 936 | case SC_ERROR_NOT_FOUND: 937 | return "Not found"; 938 | case SC_ERROR_BUSY: 939 | return "Device or resource busy"; 940 | case SC_ERROR_TIMEOUT: 941 | return "Input/Output timeout"; 942 | case SC_ERROR_NO_MEM: 943 | return "Out of memory"; 944 | case SC_ERROR_OTHER: 945 | return "General error"; 946 | case SC_ERROR_INVALID_RESPONSE: 947 | return "Invalid response from device"; 948 | case SC_ERROR_CHECKSUM_FAILURE: 949 | return "Checksum failure"; 950 | case SC_ERROR_NOT_CONNECTED: 951 | return "Not Connected"; 952 | default: 953 | return "Unknown error"; 954 | } 955 | } 956 | 957 | const char * sc_strfault(const enum SC_FAULT fault) 958 | { 959 | switch(fault) 960 | { 961 | case SC_FAULT_NONE: 962 | return "No fault"; 963 | case SC_FAULT_VOLTAGE: 964 | return "Input voltage fault"; 965 | case SC_FAULT_TEMPERATURE: 966 | return "Servo temperature fault"; 967 | default: 968 | return "Unknown fault"; 969 | } 970 | } 971 | 972 | static int sc_write_msg(const int scd, const uint8_t id, const void *msg, const uint8_t len) 973 | { 974 | uint8_t buf[SC_MAX_MSG] = { SC_START_BYTE, SC_START_BYTE, id, len + 1 }; 975 | uint8_t real_len = len + 4; 976 | int ret; 977 | 978 | memcpy(&buf[4], msg, len); 979 | 980 | buf[real_len] = sc_checksum(buf); 981 | real_len += 1; 982 | 983 | ret = write(scds[scd]->fd, buf, real_len); 984 | if (ret != real_len) 985 | { 986 | return sc_errno(SC_ERROR_IO); 987 | } 988 | 989 | return SC_SUCCESS; 990 | } 991 | 992 | static int sc_write_reg(const int scd, const uint8_t id, const uint8_t reg, const void *val, const uint8_t len) 993 | { 994 | uint8_t buf[SC_MAX_MSG] = { SC10_CMD_WRITE, reg }; 995 | int ret; 996 | 997 | memcpy(&buf[2], val, len); 998 | 999 | ret = sc_flush(scd); 1000 | if (ret != SC_SUCCESS) 1001 | { 1002 | return ret; 1003 | } 1004 | 1005 | ret = sc_write_msg(scd, id, buf, len + 2); 1006 | if (ret != SC_SUCCESS) 1007 | { 1008 | return ret; 1009 | } 1010 | 1011 | ret = sc_read_msg(scd, buf, 10); 1012 | if (ret != SC_SUCCESS) 1013 | { 1014 | return ret; 1015 | } 1016 | 1017 | if (buf[0] != 0xFF || buf[1] != 0xFF || buf[3] != 2) 1018 | { 1019 | return SC_ERROR_INVALID_RESPONSE; 1020 | } 1021 | 1022 | return buf[4]; 1023 | } 1024 | 1025 | static inline int sc_write_reg8(const int scd, const uint8_t id, const uint8_t reg, const uint8_t val) 1026 | { 1027 | return sc_write_reg(scd, id, reg, &val, 1); 1028 | } 1029 | 1030 | static inline int sc_write_reg16(const int scd, const uint8_t id, const uint8_t reg, const uint16_t val) 1031 | { 1032 | const uint16_t val_tmp = SC_ENDIAN_SWAP_16(&val); 1033 | 1034 | return sc_write_reg(scd, id, reg, &val_tmp, 2); 1035 | } 1036 | 1037 | -------------------------------------------------------------------------------- /src/scservo_driver.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015, Scott K Logan 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, this 9 | * list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * 14 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 15 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 16 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 17 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 18 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 19 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 20 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 21 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 22 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 23 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 24 | */ 25 | 26 | #include "ft_scservo_driver/ft_scservo_driver.hpp" 27 | 28 | namespace ft_scservo_driver 29 | { 30 | /** 31 | * Public Data 32 | */ 33 | const int SCServoBus::default_baud = (int)SC_BAUD_1M; 34 | const std::string SCServoBus::default_port = "/dev/ttyACM0"; 35 | const int SCServoBus::default_timeout = SC_TIMEOUT_DEFAULT; 36 | 37 | /** 38 | * Public Functions 39 | */ 40 | Exception::Exception(const enum SC_ERROR sc_error) 41 | : ros::Exception(sc_strerror(sc_error)), 42 | sc_error(sc_error) 43 | { 44 | } 45 | 46 | SCServoBus::SCServoBus(const ros::NodeHandle &nh, const ros::NodeHandle &nh_priv) 47 | : diag_timer(nh.createTimer(ros::Duration(1.0), &SCServoBus::diagTimerCallback, this)), 48 | nh(nh), 49 | nh_priv(nh_priv), 50 | scd(-1), 51 | this_name("scservo_driver") 52 | { 53 | nh_priv.param("baud", baud, SCServoBus::default_baud); 54 | nh_priv.param("port", port, SCServoBus::default_port); 55 | nh_priv.param("servos", servo_list, servo_list); 56 | nh_priv.param("timeout", timeout, SCServoBus::default_timeout); 57 | } 58 | 59 | SCServoBus::~SCServoBus() 60 | { 61 | if (scd >= 0) 62 | { 63 | sc_close(scd); 64 | } 65 | } 66 | 67 | void SCServoBus::close() 68 | { 69 | boost::recursive_mutex::scoped_lock lock(io_mutex); 70 | 71 | if (scd >= 0) 72 | { 73 | sc_close(scd); 74 | scd = -1; 75 | ROS_INFO_NAMED(this_name, "Disconnected from bus"); 76 | } 77 | } 78 | 79 | void SCServoBus::getStatus(int id, double &velocity, double &position, double &effort) 80 | { 81 | boost::recursive_mutex::scoped_lock lock(io_mutex); 82 | 83 | if (!SCServoBus::stat()) 84 | { 85 | SCServoBus::start(); 86 | 87 | if (!SCServoBus::stat()) 88 | { 89 | throw Exception(SC_ERROR_NOT_CONNECTED); 90 | } 91 | } 92 | 93 | if (servos.find(id) == servos.end()) 94 | { 95 | throw Exception(SC_ERROR_INVALID_PARAM); 96 | } 97 | 98 | servos[id]->getStatus(velocity, position, effort); 99 | } 100 | 101 | void SCServoBus::open() 102 | { 103 | boost::recursive_mutex::scoped_lock lock(io_mutex); 104 | 105 | if (SCServoBus::stat()) 106 | { 107 | ROS_DEBUG_NAMED(this_name, "Tried to open the port, but it is already open"); 108 | return; 109 | } 110 | 111 | scd = sc_open(port.c_str(), (enum SC_BAUD)baud, (uint8_t)timeout); 112 | if (scd < 0) 113 | { 114 | ROS_WARN_THROTTLE_NAMED(1, this_name, "Attempt to open device '%s' failed: %s", port.c_str(), sc_strerror(scd)); 115 | return; 116 | } 117 | 118 | ROS_INFO_NAMED(this_name, "Connected to bus at '%s'", port.c_str()); 119 | } 120 | 121 | void SCServoBus::setEnableTorque(int id, bool enable) 122 | { 123 | boost::recursive_mutex::scoped_lock lock(io_mutex); 124 | 125 | if (!SCServoBus::stat()) 126 | { 127 | SCServoBus::start(); 128 | 129 | if (!SCServoBus::stat()) 130 | { 131 | throw Exception(SC_ERROR_NOT_CONNECTED); 132 | } 133 | } 134 | 135 | if (servos.find(id) == servos.end()) 136 | { 137 | throw Exception(SC_ERROR_INVALID_PARAM); 138 | } 139 | 140 | servos[id]->setEnableTorque(enable); 141 | } 142 | 143 | void SCServoBus::setPosition(int id, double position) 144 | { 145 | boost::recursive_mutex::scoped_lock lock(io_mutex); 146 | 147 | if (!SCServoBus::stat()) 148 | { 149 | SCServoBus::start(); 150 | 151 | if (!SCServoBus::stat()) 152 | { 153 | throw Exception(SC_ERROR_NOT_CONNECTED); 154 | } 155 | } 156 | 157 | if (servos.find(id) == servos.end()) 158 | { 159 | throw Exception(SC_ERROR_INVALID_PARAM); 160 | } 161 | 162 | servos[id]->setPosition(position); 163 | } 164 | 165 | void SCServoBus::setVelocity(int id, double velocity) 166 | { 167 | boost::recursive_mutex::scoped_lock lock(io_mutex); 168 | 169 | if (!SCServoBus::stat()) 170 | { 171 | SCServoBus::start(); 172 | 173 | if (!SCServoBus::stat()) 174 | { 175 | throw Exception(SC_ERROR_NOT_CONNECTED); 176 | } 177 | } 178 | 179 | if (servos.find(id) == servos.end()) 180 | { 181 | throw Exception(SC_ERROR_INVALID_PARAM); 182 | } 183 | 184 | servos[id]->setVelocity(velocity); 185 | } 186 | 187 | bool SCServoBus::stat() 188 | { 189 | boost::recursive_mutex::scoped_lock lock(io_mutex); 190 | 191 | return (scd < 0) ? false : true; 192 | } 193 | 194 | void SCServoBus::start() 195 | { 196 | boost::recursive_mutex::scoped_lock lock(io_mutex); 197 | 198 | if (!SCServoBus::stat()) 199 | { 200 | SCServoBus::open(); 201 | } 202 | 203 | diag_timer.start(); 204 | 205 | if (SCServoBus::stat()) 206 | { 207 | ROS_DEBUG_NAMED(this_name, "Bringing up %u servos", (int)servo_list.size()); 208 | 209 | for (std::vector::iterator it = servo_list.begin(); it != servo_list.end(); it++) 210 | { 211 | if (*it < 0 || *it > SC_MAX_ID) 212 | { 213 | ROS_ERROR_NAMED(this_name, "Invalid servo ID '%d'", *it); 214 | } 215 | else if (servos.find(*it) == servos.end()) 216 | { 217 | try 218 | { 219 | servos[(uint8_t)*it] = new SCServoBus::Servo(this, nh_priv, (uint8_t)*it); 220 | } 221 | catch (Exception &e) 222 | { 223 | if (servos.find(*it) != servos.end()) 224 | { 225 | delete servos[(uint8_t)*it]; 226 | servos.erase((uint8_t)*it); 227 | } 228 | 229 | ROS_ERROR_NAMED(this_name, "Communication failure: %s", e.what()); 230 | 231 | SCServoBus::close(); 232 | 233 | break; 234 | } 235 | } 236 | } 237 | } 238 | } 239 | 240 | void SCServoBus::stop() 241 | { 242 | boost::recursive_mutex::scoped_lock lock(io_mutex); 243 | 244 | diag_timer.stop(); 245 | 246 | for (std::map::iterator it = servos.begin(); it != servos.end(); it++) 247 | { 248 | delete it->second; 249 | servos.erase(it); 250 | } 251 | } 252 | 253 | /** 254 | * Private Functions 255 | */ 256 | void SCServoBus::diagTimerCallback(const ros::TimerEvent &event) 257 | { 258 | ROS_DEBUG_NAMED(this_name, "Diagnostic callback"); 259 | 260 | if (!SCServoBus::stat()) 261 | { 262 | SCServoBus::start(); 263 | } 264 | 265 | for (std::map::iterator it = servos.begin(); it != servos.end() && SCServoBus::stat(); it++) 266 | { 267 | boost::recursive_mutex::scoped_lock lock(io_mutex); 268 | 269 | try 270 | { 271 | it->second->diagUpdate(); 272 | } 273 | catch(Exception &e) 274 | { 275 | if (e.sc_error != SC_ERROR_CHECKSUM_FAILURE) 276 | { 277 | ROS_ERROR_NAMED(this_name, "Communication failure: %s", e.what()); 278 | 279 | SCServoBus::close(); 280 | } 281 | } 282 | } 283 | 284 | diag_timer.setPeriod(ros::Duration(1.0)); 285 | } 286 | 287 | SCServoBus::Servo::Servo(class SCServoBus *parent, const ros::NodeHandle &nh_priv, const uint8_t id) 288 | : id(id), 289 | nh_priv(ros::NodeHandle(nh_priv, SCServoBus::Servo::getNodeHandleName())), 290 | diag(parent->nh, SCServoBus::Servo::nh_priv), 291 | dyn_re_cb(boost::bind(&SCServoBus::Servo::dynReCallback, this, _1, _2)), 292 | dyn_re_srv(NULL), 293 | parent(parent), 294 | rad_offset(-1.890499397), 295 | rad_per_tick(-0.003695991), 296 | relax_service(SCServoBus::Servo::nh_priv.advertiseService("relax", &SCServoBus::Servo::relaxCallback, this)), 297 | tense_service(SCServoBus::Servo::nh_priv.advertiseService("tense", &SCServoBus::Servo::tenseCallback, this)), 298 | this_name(getMessageName()) 299 | { 300 | std::stringstream str; 301 | 302 | str << "Servo " << (int)id << " Status"; 303 | 304 | diag.setHardwareIDf("Servo '%hhu' on '%s'", id, parent->port.c_str()); 305 | diag.add(str.str(), this, &SCServoBus::Servo::queryDiagnostics); 306 | diag.force_update(); 307 | 308 | SCServoBus::Servo::mergeSettings(); 309 | 310 | dyn_re_srv = new dynamic_reconfigure::Server(dyn_re_mutex, SCServoBus::Servo::nh_priv); 311 | dyn_re_srv->setCallback(dyn_re_cb); 312 | 313 | ROS_DEBUG_NAMED(this_name, "Added servo at id '%hhu'", id); 314 | } 315 | 316 | SCServoBus::Servo::~Servo() 317 | { 318 | delete dyn_re_srv; 319 | } 320 | 321 | bool SCServoBus::Servo::ping() 322 | { 323 | int ret; 324 | 325 | boost::recursive_mutex::scoped_lock lock(parent->io_mutex); 326 | 327 | ret = sc_ping(parent->scd, id); 328 | 329 | switch (ret) 330 | { 331 | case 0: 332 | return false; 333 | case 1: 334 | return true; 335 | default: 336 | throw Exception((enum SC_ERROR)ret); 337 | } 338 | } 339 | 340 | void SCServoBus::Servo::diagUpdate() 341 | { 342 | diag.update(); 343 | } 344 | 345 | void SCServoBus::Servo::dynReCallback(ft_scservo_driver::ServoConfig &config, uint32_t level) 346 | { 347 | int ret; 348 | 349 | boost::recursive_mutex::scoped_lock lock(parent->io_mutex); 350 | 351 | ROS_DEBUG_NAMED(this_name, "Dynamic Reconfigure Callback"); 352 | 353 | last_settings.min_angle_limit = config.min_angle_limit; 354 | last_settings.max_angle_limit = config.max_angle_limit; 355 | last_settings.limit_temperature = config.limit_temperature; 356 | last_settings.max_limit_voltage = (uint8_t)(config.max_limit_voltage * 10 + 0.5); 357 | last_settings.min_limit_voltage = (uint8_t)(config.min_limit_voltage * 10 + 0.5); 358 | last_settings.max_torque = config.max_torque; 359 | last_settings.compliance_p = config.compliance_p; 360 | last_settings.compliance_d = config.compliance_d; 361 | last_settings.compliance_i = config.compliance_i; 362 | last_settings.imax = config.imax; 363 | 364 | rad_offset = config.rad_offset; 365 | rad_per_tick = config.rad_per_tick; 366 | 367 | ret = sc_write_settings(parent->scd, id, &last_settings); 368 | if (ret < SC_SUCCESS) 369 | { 370 | ROS_ERROR_NAMED(this_name, "Failed to update servo settings: %s", sc_strerror(ret)); 371 | } 372 | 373 | // Propagate back to DynRe so the precision matches 374 | config.max_limit_voltage = last_settings.max_limit_voltage / 10.0; 375 | config.min_limit_voltage = last_settings.min_limit_voltage / 10.0; 376 | } 377 | 378 | ft_scservo_driver::ServoConfig SCServoBus::Servo::getConfig() 379 | { 380 | ft_scservo_driver::ServoConfig cfg; 381 | int ret; 382 | 383 | ret = sc_read_settings(parent->scd, id, &last_settings); 384 | if (ret < SC_SUCCESS) 385 | { 386 | throw Exception((enum SC_ERROR)ret); 387 | } 388 | 389 | cfg.min_angle_limit = last_settings.min_angle_limit; 390 | cfg.max_angle_limit = last_settings.max_angle_limit; 391 | cfg.limit_temperature = last_settings.limit_temperature; 392 | cfg.max_limit_voltage = last_settings.max_limit_voltage / 10.0; 393 | cfg.min_limit_voltage = last_settings.min_limit_voltage / 10.0; 394 | cfg.max_torque = last_settings.max_torque; 395 | cfg.compliance_p = last_settings.compliance_p; 396 | cfg.compliance_d = last_settings.compliance_d; 397 | cfg.compliance_i = last_settings.compliance_i; 398 | cfg.imax = last_settings.imax; 399 | 400 | cfg.rad_offset = rad_offset; 401 | cfg.rad_per_tick = rad_per_tick; 402 | 403 | return cfg; 404 | } 405 | 406 | std::string SCServoBus::Servo::getMessageName() const 407 | { 408 | std::stringstream str; 409 | 410 | str << "scservo_driver" << ".servo_" << (int)id; 411 | 412 | return str.str(); 413 | } 414 | 415 | std::string SCServoBus::Servo::getNodeHandleName() const 416 | { 417 | std::stringstream str; 418 | 419 | str << "servo_" << (int)id; 420 | 421 | return str.str(); 422 | } 423 | 424 | void SCServoBus::Servo::getStatus(double &velocity, double &position, double &effort) 425 | { 426 | int ret; 427 | struct sc_status status; 428 | 429 | ret = sc_read_status(parent->scd, id, &status); 430 | if (ret < SC_SUCCESS) 431 | { 432 | throw Exception((enum SC_ERROR)ret); 433 | } 434 | 435 | velocity = status.present_speed * rad_per_tick; 436 | position = status.present_position * rad_per_tick - rad_offset; 437 | effort = status.present_load; 438 | } 439 | 440 | void SCServoBus::Servo::mergeSettings() 441 | { 442 | ROS_DEBUG_NAMED(this_name, "Merging settings for servo at id '%hhu'", id); 443 | 444 | boost::recursive_mutex::scoped_lock lock(dyn_re_mutex); 445 | 446 | // Start with the settings already on the servo 447 | ft_scservo_driver::ServoConfig cfg = SCServoBus::Servo::getConfig(); 448 | 449 | // Update any settings in the parameter server 450 | cfg.__fromServer__(nh_priv); 451 | cfg.__clamp__(); 452 | 453 | // Update the parameter server 454 | if (dyn_re_srv != NULL) 455 | { 456 | dyn_re_srv->updateConfig(cfg); 457 | } 458 | else 459 | { 460 | cfg.__toServer__(nh_priv); 461 | } 462 | } 463 | 464 | void SCServoBus::Servo::queryDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &stat) 465 | { 466 | int ret; 467 | struct sc_diag diag; 468 | struct sc_status status; 469 | uint8_t torque_enable; 470 | 471 | ROS_DEBUG_NAMED(this_name, "Getting diagnostics for servo at id '%hhu'", id); 472 | 473 | ret = sc_read_status_and_diag(parent->scd, id, &status, &diag); 474 | if (ret < SC_SUCCESS) 475 | { 476 | throw Exception((enum SC_ERROR)ret); 477 | } 478 | 479 | ret = sc_read_torque_enable(parent->scd, id, &torque_enable); 480 | if (ret < SC_SUCCESS) 481 | { 482 | throw Exception((enum SC_ERROR)ret); 483 | } 484 | 485 | stat.addf("Fault Code", "%s (%d)", sc_strfault((enum SC_FAULT)diag.error), (int)diag.error); 486 | stat.add("Load", status.present_load); 487 | stat.add("Position", status.present_position); 488 | stat.add("Speed", status.present_speed); 489 | stat.add("Temperature", (int)diag.temperature); 490 | stat.add("Torque Enabled", (bool)torque_enable); 491 | stat.add("Voltage", diag.voltage / 10.0f); 492 | 493 | if (diag.error == 0) 494 | { 495 | stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Servo Communication OK"); 496 | } 497 | else 498 | { 499 | stat.summary(diagnostic_msgs::DiagnosticStatus::ERROR, "Servo is reporting error(s)"); 500 | } 501 | } 502 | 503 | bool SCServoBus::Servo::relaxCallback(std_srvs::Empty::Request &reques, std_srvs::Empty::Response &response) 504 | { 505 | int ret; 506 | 507 | boost::recursive_mutex::scoped_lock lock(parent->io_mutex); 508 | 509 | ROS_DEBUG_NAMED(this_name, "Relaxing servo at id '%hhu'", id); 510 | 511 | ret = sc_write_torque_enable(parent->scd, id, 0); 512 | if (ret < SC_SUCCESS) 513 | { 514 | ROS_ERROR_NAMED(this_name, "Servo Communication Failure: %s", sc_strerror(ret)); 515 | return false; 516 | } 517 | 518 | return true; 519 | } 520 | 521 | bool SCServoBus::Servo::tenseCallback(std_srvs::Empty::Request &reques, std_srvs::Empty::Response &response) 522 | { 523 | int ret; 524 | 525 | boost::recursive_mutex::scoped_lock lock(parent->io_mutex); 526 | 527 | ROS_DEBUG_NAMED(this_name, "Tensing servo at id '%hhu'", id); 528 | 529 | ret = sc_write_torque_enable(parent->scd, id, 1); 530 | if (ret < SC_SUCCESS) 531 | { 532 | ROS_ERROR_NAMED(this_name, "Servo Communication Failure: %s", sc_strerror(ret)); 533 | return false; 534 | } 535 | 536 | return true; 537 | } 538 | 539 | void SCServoBus::Servo::setEnableTorque(bool enable) 540 | { 541 | int ret; 542 | 543 | ret = sc_write_torque_enable(parent->scd, id, enable ? 1 : 0); 544 | if (ret < SC_SUCCESS) 545 | { 546 | throw Exception((enum SC_ERROR)ret); 547 | } 548 | } 549 | 550 | void SCServoBus::Servo::setPosition(double position) 551 | { 552 | uint16_t pos; 553 | int ret; 554 | 555 | position = (position + rad_offset) / rad_per_tick; 556 | 557 | if (position < 0 || position > 1023) 558 | { 559 | throw Exception(SC_ERROR_INVALID_PARAM); 560 | } 561 | 562 | pos = (position + 0.5); 563 | 564 | ret = sc_write_goal(parent->scd, id, 25, pos); 565 | if (ret < SC_SUCCESS) 566 | { 567 | throw Exception((enum SC_ERROR)ret); 568 | } 569 | } 570 | 571 | void SCServoBus::Servo::setVelocity(double velocity) 572 | { 573 | uint16_t vel; 574 | int ret; 575 | 576 | velocity = velocity / rad_per_tick; 577 | 578 | if (velocity < -1024 || velocity > 1023) 579 | { 580 | throw Exception(SC_ERROR_INVALID_PARAM); 581 | } 582 | 583 | vel = (velocity + 0.5); 584 | 585 | ret = sc_write_goal_speed(parent->scd, id, vel); 586 | if (ret < SC_SUCCESS) 587 | { 588 | throw Exception((enum SC_ERROR)ret); 589 | } 590 | } 591 | } 592 | -------------------------------------------------------------------------------- /src/scservo_handle.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015, Scott K Logan 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, this 9 | * list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * 14 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 15 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 16 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 17 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 18 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 19 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 20 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 21 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 22 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 23 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 24 | */ 25 | 26 | #include "ft_scservo_driver/ft_scservo_handle.hpp" 27 | 28 | namespace ft_scservo_driver 29 | { 30 | SCServoHandle::SCServoHandle(const ros::NodeHandle &nh, const ros::NodeHandle &nh_priv) 31 | : nh(nh), 32 | nh_priv(nh_priv), 33 | scs(nh, nh_priv), 34 | servo_count(0), 35 | this_name("scservo_driver") 36 | { 37 | std::stringstream strstr; 38 | std::string actuator_name; 39 | std::string op_mode; 40 | 41 | scs.start(); 42 | 43 | nh_priv.param("servos", servos, servos); 44 | 45 | servo_count = servos.size(); 46 | 47 | if (servo_count > 0) 48 | { 49 | for (std::vector::iterator it = servos.begin(); it != servos.end(); it++) 50 | { 51 | strstr << "servo_" << *it; 52 | if (!nh_priv.getParam(strstr.str() + "/actuator_name", actuator_name)) 53 | { 54 | actuator_name = strstr.str(); 55 | ROS_WARN_NAMED(this_name, "Actuator name for servo at id '%d' is not set - defaulting to '%s'", *it, actuator_name.c_str()); 56 | } 57 | if (!nh_priv.getParam(strstr.str() + "/operation_mode", op_mode)) 58 | { 59 | actuator_mode.push_back(OPERATION_MODE_POSITION); 60 | } 61 | else 62 | { 63 | if (op_mode == "profile_velocity" || op_mode == "vel" || op_mode == "velocity") 64 | { 65 | actuator_mode.push_back(OPERATION_MODE_VELOCITY); 66 | } 67 | else 68 | { 69 | if (op_mode != "profile_position" && op_mode != "pos" && op_mode != "position") 70 | { 71 | ROS_WARN_NAMED(this_name, "Invalid operation_mode value '%s' - defaulting to position control", op_mode.c_str()); 72 | } 73 | 74 | actuator_mode.push_back(OPERATION_MODE_POSITION); 75 | } 76 | } 77 | 78 | actuator_engaged.push_back(false); 79 | 80 | actuator_cmd_pos.push_back(std::numeric_limits::quiet_NaN()); 81 | actuator_cmd_vel.push_back(std::numeric_limits::quiet_NaN()); 82 | actuator_eff.push_back(std::numeric_limits::quiet_NaN()); 83 | actuator_pos.push_back(std::numeric_limits::quiet_NaN()); 84 | actuator_vel.push_back(std::numeric_limits::quiet_NaN()); 85 | 86 | actuator_handle.push_back(hardware_interface::ActuatorStateHandle(actuator_name, &actuator_pos.back(), &actuator_vel.back(), &actuator_eff.back())); 87 | actuator_handle_cmd_pos.push_back(hardware_interface::ActuatorHandle(actuator_handle.back(), &actuator_cmd_pos.back())); 88 | actuator_handle_cmd_vel.push_back(hardware_interface::ActuatorHandle(actuator_handle.back(), &actuator_cmd_vel.back())); 89 | } 90 | } 91 | } 92 | 93 | SCServoHandle::~SCServoHandle() 94 | { 95 | scs.stop(); 96 | } 97 | 98 | void SCServoHandle::registerPositionHandles(hardware_interface::PositionActuatorInterface &api) 99 | { 100 | int i; 101 | 102 | for (i = 0; i < servo_count; i++) 103 | { 104 | if (actuator_mode[i] == OPERATION_MODE_POSITION) 105 | { 106 | api.registerHandle(actuator_handle_cmd_pos[i]); 107 | } 108 | } 109 | } 110 | 111 | void SCServoHandle::registerStateHandles(hardware_interface::ActuatorStateInterface &asi) 112 | { 113 | int i; 114 | 115 | for (i = 0; i < servo_count; i++) 116 | { 117 | asi.registerHandle(actuator_handle[i]); 118 | } 119 | } 120 | 121 | void SCServoHandle::registerVelocityHandles(hardware_interface::VelocityActuatorInterface &avi) 122 | { 123 | int i; 124 | 125 | for (i = 0; i < servo_count; i++) 126 | { 127 | if (actuator_mode[i] == OPERATION_MODE_VELOCITY) 128 | { 129 | avi.registerHandle(actuator_handle_cmd_vel[i]); 130 | } 131 | } 132 | } 133 | 134 | void SCServoHandle::read() 135 | { 136 | int i; 137 | 138 | for (i = 0; i < servo_count; i++) 139 | { 140 | try 141 | { 142 | scs.getStatus(servos[i], actuator_vel[i], actuator_pos[i], actuator_eff[i]); 143 | } 144 | catch(ft_scservo_driver::Exception &e) 145 | { 146 | ROS_WARN_NAMED(this_name, "Failed to read state of servo at id '%d': %s", servos[i], e.what()); 147 | } 148 | } 149 | } 150 | 151 | void SCServoHandle::write() 152 | { 153 | int i; 154 | 155 | for (i = 0; i < servo_count; i++) 156 | { 157 | try 158 | { 159 | switch(actuator_mode[i]) 160 | { 161 | default: 162 | case OPERATION_MODE_POSITION: 163 | if (isnan(actuator_cmd_pos[i])) 164 | { 165 | if (actuator_engaged[i]) 166 | { 167 | scs.setEnableTorque(servos[i], false); 168 | actuator_engaged[i] = false; 169 | } 170 | } 171 | else 172 | { 173 | if (!actuator_engaged[i]) 174 | { 175 | scs.setEnableTorque(servos[i], true); 176 | actuator_engaged[i] = true; 177 | } 178 | 179 | scs.setPosition(servos[i], actuator_cmd_pos[i]); 180 | } 181 | break; 182 | case OPERATION_MODE_VELOCITY: 183 | scs.setVelocity(servos[i], actuator_cmd_vel[i]); 184 | break; 185 | } 186 | } 187 | catch(ft_scservo_driver::Exception &e) 188 | { 189 | ROS_WARN_NAMED(this_name, "Failed to command servo at id '%d': %s", servos[i], e.what()); 190 | } 191 | } 192 | } 193 | } 194 | -------------------------------------------------------------------------------- /src/scservo_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015, Scott K Logan 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, this 9 | * list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * 14 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 15 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 16 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 17 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 18 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 19 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 20 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 21 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 22 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 23 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 24 | */ 25 | 26 | #include 27 | 28 | #include 29 | 30 | int main(int argc, char *argv[]) 31 | { 32 | ft_scservo_driver::SCServoBus *sc = NULL; 33 | 34 | ros::init(argc, argv, "scservo_driver"); 35 | 36 | sc = new ft_scservo_driver::SCServoBus(ros::NodeHandle(), ros::NodeHandle("~")); 37 | if (sc == NULL) 38 | { 39 | ROS_FATAL("Failed to create bus controller"); 40 | } 41 | 42 | try 43 | { 44 | sc->start(); 45 | } 46 | catch (ft_scservo_driver::Exception &e) 47 | { 48 | ROS_FATAL("Failed to open device: %s", e.what()); 49 | } 50 | 51 | ros::spin(); 52 | 53 | sc->close(); 54 | 55 | return 0; 56 | } 57 | -------------------------------------------------------------------------------- /src/scservo_scan.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2015, Scott K Logan 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * 1. Redistributions of source code must retain the above copyright notice, this 9 | * list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * 14 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 15 | * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 16 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 17 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR 18 | * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 19 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 20 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 21 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 22 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 23 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 24 | */ 25 | 26 | #include 27 | 28 | #include 29 | #include 30 | #include 31 | 32 | static enum SC_BAUD strtobaud(const char *baud) 33 | { 34 | if (strcmp(baud, "1000000") == 0) 35 | { 36 | return SC_BAUD_1M; 37 | } 38 | else if(strcmp(baud, "500000") == 0) 39 | { 40 | return SC_BAUD_500K; 41 | } 42 | else if(strcmp(baud, "115200") == 0) 43 | { 44 | return SC_BAUD_115200; 45 | } 46 | else if(strcmp(baud, "57600") == 0) 47 | { 48 | return SC_BAUD_57600; 49 | } 50 | else if(strcmp(baud, "38400") == 0) 51 | { 52 | return SC_BAUD_38400; 53 | } 54 | else 55 | { 56 | fprintf(stderr, "Invalid baud rate %s\n", baud); 57 | exit(EXIT_FAILURE); 58 | return SC_BAUD_MAX; 59 | } 60 | } 61 | 62 | int main(const int argc, const char *argv[]) 63 | { 64 | uint8_t count = 0; 65 | uint8_t i; 66 | int ret = 0; 67 | int scd = -1; 68 | 69 | if (argc != 3) 70 | { 71 | fprintf(stderr, "Usage: %s \n\n", argv[0]); 72 | return EXIT_FAILURE; 73 | } 74 | 75 | scd = sc_open(argv[1], strtobaud(argv[2]), 1); 76 | if (scd < 0) 77 | { 78 | fprintf(stderr, "Failed to open port: %s\n", sc_strerror(scd)); 79 | return scd; 80 | } 81 | 82 | for (i = 0; i <= SC_MAX_ID && i != SC_BROADCAST_ID; i++) 83 | { 84 | printf("\rPinging %d...", i); 85 | fflush(stdout); 86 | ret = sc_ping(scd, i); 87 | if (ret < 0) 88 | { 89 | fprintf(stderr, "\rFailed to ping device %d: %s\n", i, sc_strerror(ret)); 90 | } 91 | else if (ret == 1) 92 | { 93 | count += 1; 94 | printf("\rFound servo at ID %d\n", i); 95 | } 96 | } 97 | printf("\rTotal servos: %d\n", count); 98 | 99 | sc_close(scd); 100 | 101 | return EXIT_SUCCESS; 102 | } 103 | --------------------------------------------------------------------------------