├── srv └── Service.srv ├── .github └── FUNDING.yml ├── .gitignore ├── msg ├── ControlStatus.msg ├── Peripheral.msg └── MotorStatus.msg ├── Dockerfile ├── .travis.yml ├── LICENSE ├── config ├── control.yaml └── roboteq.yaml ├── cfg ├── RoboteqPIDtype.cfg ├── RoboteqPID.cfg ├── RoboteqController.cfg ├── RoboteqEncoder.cfg ├── RoboteqParameter.cfg ├── RoboteqAnalogInput.cfg └── RoboteqPulseInput.cfg ├── include ├── configurator │ ├── gpio_sensor.h │ ├── motor_pid.h │ ├── gpio_pulse.h │ ├── gpio_encoder.h │ ├── gpio_analog.h │ └── motor_param.h └── roboteq │ ├── serial_controller.h │ ├── motor.h │ └── roboteq.h ├── launch ├── roboteq.launch └── differential_drive.launch ├── package.xml ├── README.md ├── urdf └── differential-robot.urdf.xacro ├── CMakeLists.txt ├── rviz └── urdf.rviz └── src ├── roboteq_control.cpp ├── configurator ├── gpio_analog.cpp ├── gpio_pulse.cpp ├── gpio_encoder.cpp ├── motor_pid.cpp └── motor_param.cpp └── roboteq ├── serial_controller.cpp ├── motor.cpp └── roboteq.cpp /srv/Service.srv: -------------------------------------------------------------------------------- 1 | string service 2 | --- 3 | string information 4 | -------------------------------------------------------------------------------- /.github/FUNDING.yml: -------------------------------------------------------------------------------- 1 | github: rbonghi 2 | patreon: # Replace with a single Patreon username 3 | custom: https://rnext.it 4 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | 2 | *.txt.user 3 | *.autosave 4 | 5 | # Build folders 6 | build/ 7 | build-*-Desktop-Default/ 8 | *-build/ 9 | # Visual studio code 10 | .vscode/ -------------------------------------------------------------------------------- /msg/ControlStatus.msg: -------------------------------------------------------------------------------- 1 | # 50Hz feedback message for controls purposes 2 | Header header 3 | 4 | # PWM 5 | float64 pwm 6 | 7 | # reference 8 | float64 reference 9 | 10 | # feedback 11 | float64 feedback 12 | 13 | # Loop error 14 | float64 loop_error 15 | -------------------------------------------------------------------------------- /msg/Peripheral.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | #GPIO 4 | 5 | #Pulse in [pag. 256] 6 | uint16[] pulse_in 7 | 8 | #Analog in [pag. 231] 9 | float64[] analog 10 | 11 | #Digital input [pag. 242] 12 | uint8[] digital_in 13 | 14 | #Digital outut [pag. 242] 15 | uint8[] digital_out 16 | -------------------------------------------------------------------------------- /msg/MotorStatus.msg: -------------------------------------------------------------------------------- 1 | # 50Hz feedback message for controls purposes 2 | Header header 3 | 4 | # Electrical power supply to the driver (V) 5 | float64 volts 6 | 7 | # Current flowing in the motors (A) 8 | float64 amps_motor 9 | float64 amps_batt 10 | 11 | # Tracks 12 | float64 track 13 | -------------------------------------------------------------------------------- /Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ros:melodic 2 | 3 | # Make workspace dir 4 | RUN mkdir -p /home/catkin_ws/src 5 | # Change WORKDIR 6 | WORKDIR /home/catkin_ws/ 7 | # Copy roboteq_control folder 8 | ADD . /home/catkin_ws/src/roboteq_control 9 | # Install all dependencies 10 | RUN apt-get update && apt-get install -y \ 11 | && rosdep install --from-paths src --ignore-src -r -y \ 12 | && rm -rf /var/lib/apt/lists/ 13 | # Compile package 14 | RUN /bin/bash -c '. /opt/ros/melodic/setup.bash; catkin_make' -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | # This config uses industrial_ci (https://github.com/ros-industrial/industrial_ci.git). 2 | # For troubleshooting, see readme (https://github.com/ros-industrial/industrial_ci/blob/master/README.rst) 3 | 4 | language: generic # optional, just removes the language badge 5 | 6 | services: 7 | - docker 8 | 9 | notifications: 10 | email: 11 | recipients: 12 | - raffaello@rnext.it 13 | on_success: change #[always|never|change] # default: change 14 | on_failure: always #[always|never|change] # default: always 15 | 16 | env: 17 | matrix: 18 | - ROS_DISTRO="melodic" 19 | 20 | # clone and run industrial_ci 21 | install: 22 | - git clone --quiet --depth 1 https://github.com/ros-industrial/industrial_ci.git .industrial_ci 23 | script: 24 | - .industrial_ci/travis.sh 25 | 26 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | The MIT License (MIT) 2 | 3 | Copyright (c) 2020 Raffaello Bonghi 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. -------------------------------------------------------------------------------- /config/control.yaml: -------------------------------------------------------------------------------- 1 | # Publish all joint states ----------------------------------- 2 | joint_publisher: 3 | type: "joint_state_controller/JointStateController" 4 | publish_rate: 50 5 | 6 | velocity_controller: 7 | type: "diff_drive_controller/DiffDriveController" 8 | left_wheel: 'left_joint' 9 | right_wheel: 'right_joint' 10 | publish_rate: 50 11 | pose_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03] 12 | twist_covariance_diagonal: [0.001, 0.001, 0.001, 0.001, 0.001, 0.03] 13 | cmd_vel_timeout: 0.25 14 | 15 | # Base frame_id 16 | base_frame_id: base_link 17 | 18 | # Odometry fused with IMU is published by robot_localization, so 19 | # no need to publish a TF based on encoders alone. 20 | enable_odom_tf: true 21 | 22 | # Panther hardware provides wheel velocities 23 | estimate_velocity_from_position: false 24 | 25 | # Wheel separation and radius multipliers 26 | # wheel_separation_multiplier: 1 # default: 1.0 27 | # wheel_radius_multiplier: 1 # default: 1.0 28 | 29 | # Velocity and acceleration limits 30 | # Whenever a min_* is unspecified, default to -max_* 31 | linear: 32 | x: 33 | has_velocity_limits: true 34 | max_velocity: 0.6 # m/s 35 | has_acceleration_limits: true 36 | max_acceleration: 2.0 # m/s^2 37 | angular: 38 | z: 39 | has_velocity_limits: true 40 | max_velocity: 2.0 # rad/s 41 | has_acceleration_limits: true 42 | max_acceleration: 6.0 # rad/s^2 43 | -------------------------------------------------------------------------------- /cfg/RoboteqPIDtype.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "roboteq_control" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | # Please either one of the below modes in yaml configuration. Selecting the Motor Control Modes [pag. 90] 9 | 10 | # MMOD [pag. 314] 11 | closed_loop_vel = gen.enum([ gen.const("closed_loop_speed", int_t, 1, "Closed loop speed (Sec. 7 pag. 90)"), 12 | gen.const("closed_loop_speed_position", int_t, 6, "Closed loop speed position (Sec. 7 pag. 91)")], 13 | "Type fo velocity control mode") 14 | gen.add("closed_loop_velocity", int_t, 0, "Configuration closed loop", 6, 1, 6, edit_method=closed_loop_vel) 15 | 16 | # MMOD [pag. 314] 17 | closed_loop_pos = gen.enum([ gen.const("closed_loop_position_relative", int_t, 2, "Closed loop position relative (Sec. 7 pag. 91)"), 18 | gen.const("closed_loop_count_position", int_t, 3, "Closed loop count position (Sec. 7 pag. 92)"), 19 | gen.const("closed_loop_position_tracking", int_t, 4, "Closed loop position tracking (Sec. 10 pag. 92)")], 20 | "Type fo position control mode") 21 | gen.add("closed_loop_position", int_t, 0, "Configuration closed loop", 2, 2, 4, edit_method=closed_loop_pos) 22 | 23 | #Default command 24 | gen.add("restore_defaults", bool_t , 0, "Restore to the original configuration", False ) 25 | 26 | exit(gen.generate(PACKAGE, "roboteq_node", "RoboteqPIDtype")) 27 | -------------------------------------------------------------------------------- /cfg/RoboteqPID.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "roboteq_control" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | # MVEL [pag. 314] 9 | gen.add("position_mode_velocity", int_t, 0, "[RPM] Default speed at which move the motor moves in position mode", 0, 0, 30000) 10 | 11 | # MXTRN [pag. 317] 12 | gen.add("turn_min_to_max", double_t, 0, "When the encoder are used is automatically computed", 0.0, 0, 1000) 13 | 14 | # KP [pag. 311] 15 | gen.add("Kp", double_t, 0, "Kp gain PID", 0.0, 0, 25.5) 16 | # KI [pag. 310] 17 | gen.add("Ki", double_t, 0, "Ki gain PID", 0.0, 0, 25.5) 18 | # KD [pag. 310] 19 | gen.add("Kd", double_t, 0, "Kd gain PID", 0.0, 0, 25.5) 20 | # ICAP [pag. 309] 21 | gen.add("integrator_limit", int_t, 0, "[#] Integrator limit", 100, 0, 100) 22 | 23 | # CLERD [pag. 303] 24 | error_detection = gen.enum([ gen.const("detection_disabled", int_t, 0, "Detection disabled"), 25 | gen.const("250ms_at_error_up_100", int_t, 1, "250ms at error > 100"), 26 | gen.const("500ms_at_error_up_250", int_t, 2, "500ms at error > 250"), 27 | gen.const("1000ms_at_error_up_500", int_t, 3, "1000ms at error > 500")], 28 | "loop error detection") 29 | gen.add("loop_error_detection", int_t, 0, "Loop error detection", 0, 0, 3, edit_method=error_detection) 30 | 31 | #Default command 32 | gen.add("load_roboteq", bool_t , 0, "Load all parameters from Roboteq board", False) 33 | gen.add("restore_defaults", bool_t , 0, "Restore to the original configuration", False ) 34 | 35 | exit(gen.generate(PACKAGE, "roboteq_node", "RoboteqPID")) 36 | -------------------------------------------------------------------------------- /include/configurator/gpio_sensor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (C) 2017, Raffaello Bonghi 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 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * 3. Neither the name of the copyright holder nor the names of its 15 | * contributors may be used to endorse or promote products derived 16 | * from this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND 19 | * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 20 | * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 21 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 22 | * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 23 | * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 24 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 25 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 26 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 28 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #ifndef GPIOSENSOR_H 32 | #define GPIOSENSOR_H 33 | 34 | class GPIOSensor { 35 | public: 36 | virtual double getConversion(double reduction) = 0; 37 | }; 38 | 39 | #endif // GPIOSENSOR_H 40 | -------------------------------------------------------------------------------- /cfg/RoboteqController.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "roboteq_control" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | # PWMF [pag. 320] 9 | gen.add("pwm_frequency", double_t, 0, "[kHz] Frequency PWM motors", 10, 10, 30) 10 | 11 | # OVL [pag. 319] 12 | gen.add("over_voltage_limit", double_t, 0, "[V] Over voltage limit", 10, 0, 60) 13 | 14 | # OVH [pag. 318] 15 | gen.add("over_voltage_hysteresis", double_t, 0, "[V] Over voltage histeresis", 5, 0, 25.5) 16 | 17 | # UVL [pag. 327] 18 | gen.add("under_voltage_limit", double_t, 0, "[V] Under voltage limit", 10, 0, 60) 19 | 20 | # BKD [pag. 301] 21 | gen.add("break_delay", int_t, 0, "[ms] Break activation delay", 250, 0, 65536) 22 | 23 | # ADB [pag. 281] 24 | mixing_mode = gen.enum([ gen.const("separate", int_t, 0, "direct control channels"), 25 | gen.const("mode_1", int_t, 1, "Mixed mode like tank - mode 1 (see reference)"), 26 | gen.const("mode_2", int_t, 2, "Mixed mode like tank - mode 2 (see reference)"), 27 | gen.const("mode_3", int_t, 3, "Mixed mode like tank - mode 3 (see reference)")], 28 | "operation mode") 29 | 30 | # MXMD [pag. 315] 31 | gen.add("mixing", int_t, 0, "Type of mixing mode", 0, 0, 3, edit_method=mixing_mode) 32 | 33 | #Default command 34 | gen.add("load_roboteq", bool_t , 0, "Load all parameters from Roboteq board", False) 35 | gen.add("restore_defaults", bool_t , 0, "Restore to the original configuration", False) 36 | 37 | # Maintenance commands 38 | gen.add("factory_reset", bool_t , 0, "Factory reset the roboteq board", False) 39 | gen.add("store_in_eeprom", bool_t , 0, "Store all parameters in EEPROM", False) 40 | gen.add("load_from_eeprom", bool_t , 0, "Load all parameters from EEPROM", False) 41 | 42 | exit(gen.generate(PACKAGE, "roboteq_node", "RoboteqController")) 43 | -------------------------------------------------------------------------------- /cfg/RoboteqEncoder.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "roboteq_control" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | # Encoder 9 | 10 | # EMOD [pag. 307] 11 | enc_config = gen.enum([ gen.const("unused", int_t, 0, "Unused"), 12 | gen.const("command", int_t, 1, "Command"), 13 | gen.const("feedback", int_t, 2, "Feedback")], 14 | "configuration") 15 | gen.add("configuration", int_t, 0, "Configuration encoder", 0, 0, 2, edit_method=enc_config) 16 | 17 | # EPPR [pag. 281] 18 | motor_enable = gen.enum([ gen.const("disabled", int_t, 0, "Disabled"), 19 | gen.const("enabled", int_t, 1, "enabled")], 20 | "Enable feedback from this motor") 21 | gen.add("input_motor_one", int_t, 0, "Configuration pulse input", 0, 0, 2, edit_method=motor_enable) 22 | gen.add("input_motor_two", int_t, 0, "Configuration pulse input", 0, 0, 2, edit_method=motor_enable) 23 | 24 | # EPPR [pag. 308] 25 | gen.add("PPR", int_t, 0, "[#] An Integer parameter", 1, 0, 5000) 26 | 27 | # Not available in datasheet; but used in software to find out the reduction value 28 | enc_pos = gen.enum([ gen.const("After", int_t, 0, "After gears"), 29 | gen.const("Before", int_t, 1, "Before gears")], 30 | "Position") 31 | gen.add("position", int_t, 0, "Position encoder respect to gears", 1, 0, 1, edit_method=enc_pos) 32 | 33 | # ELL [pag. 306] 34 | gen.add("encoder_low_count_limit", int_t, 0, "[#] Encoder minimum count value", -1000, -2147000000, 2147000000) 35 | 36 | # ADB [pag. 304] 37 | gen.add("encoder_high_count_limit", int_t, 0, "[#] Encoder maximum count value", 1000, -2147000000, 2147000000) 38 | 39 | # ADB [pag. 281] 40 | gen.add("encoder_home_count", int_t, 0, "[#] Encoder home count", 0, -2147000000, 2147000000) 41 | 42 | #Default command 43 | gen.add("load_roboteq", bool_t , 0, "Load all parameters from Roboteq board", False) 44 | gen.add("restore_defaults", bool_t , 0, "Restore to the original configuration", False) 45 | 46 | exit(gen.generate(PACKAGE, "roboteq_node", "RoboteqEncoder")) 47 | -------------------------------------------------------------------------------- /cfg/RoboteqParameter.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "roboteq_control" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | # Not available in datasheet; but used in software to mention the gear ratio 9 | gen.add("ratio", double_t, 0, "[#] Gear ratio", 1, 1, 0) 10 | 11 | # MDIR [pag. 314] 12 | rotation = gen.enum([ gen.const("Clockwise", int_t, -1, "Clockwise rotation"), 13 | gen.const("Counterclockwise", int_t, 1, "Counterclockwise rotation")], 14 | "Rotation versus wheel") 15 | gen.add("rotation", int_t, 0, "Versus of the rotation of the motor", 1, -1, 1, edit_method=rotation) 16 | 17 | # BLSTD [pag. 302] 18 | stall = gen.enum([ gen.const("Disabled", int_t, 0, "Disabled"), 19 | gen.const("250ms_at_10percent", int_t, 1, "250ms at 10% power"), 20 | gen.const("500ms_at_25percent", int_t, 2, "500ms at 25% power"), 21 | gen.const("1000ms_at_50percent", int_t, 3, "1000ms at 50% power")], 22 | "Stall configuration") 23 | gen.add("stall_detection", int_t, 0, "Stall detection", 1, 0, 3, edit_method=stall) 24 | 25 | # ALIM [pag. 298] 26 | gen.add("amper_limit", double_t, 0, "[A] Amper limit", 5, 1, 20) 27 | 28 | # MXPF [pag. 316] 29 | gen.add("max_forward", int_t, 0, "[%] IN MODULE - Maximum power forward", 100, 0, 100) 30 | 31 | # MXPR [pag. 316] 32 | gen.add("max_reverse", int_t, 0, "[%] IN MODULE - Maximum power reverse", 100, 0, 100) 33 | 34 | # MXRPM [pag. 317] 35 | gen.add("max_speed", double_t, 0, "[RPM] max motor speed (after gear reduction)", 10, 0) 36 | 37 | # MAC [pag. 312] 38 | gen.add("max_acceleration", double_t, 0, "[RPM/s] max motor acceleration (after gear reduction)", 1000, 0) 39 | 40 | # MDEC [pag. 312] 41 | gen.add("max_deceleration", double_t, 0, "[RPM/s] max motor deceleration (after gear reduction)", 1000, 0) 42 | 43 | #Default command 44 | gen.add("load_roboteq", bool_t , 0, "Load all parameters from Roboteq board", False) 45 | gen.add("restore_defaults", bool_t , 0, "Restore to the original configuration", False) 46 | 47 | exit(gen.generate(PACKAGE, "roboteq_node", "RoboteqParameter")) 48 | -------------------------------------------------------------------------------- /cfg/RoboteqAnalogInput.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "roboteq_control" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | #from 280 9 | # to check ALIN - analog linearity [pag. 283] 10 | 11 | # AMOD [pag. 286] 12 | conversion_type = gen.enum([ gen.const("conversion_disabled", int_t, 0, "Disable conversion type"), 13 | gen.const("absolute", int_t, 1, "Enable in absolute mode"), 14 | gen.const("relative", int_t, 2, "Enable in relative mode"),], 15 | "Type of analog input conversion") 16 | gen.add("conversion", int_t, 0, "Type of analog input conversion", 0, 0, 2, edit_method=conversion_type) 17 | 18 | # AINA [pag. 282] 19 | input_use_type = gen.enum([ gen.const("input_disabled", int_t, 0, "Disabled"), 20 | gen.const("motor_command", int_t, 1, "Motor command"), 21 | gen.const("feedback", int_t, 2, "Feedback")], 22 | "configuration") 23 | gen.add("input_use", int_t, 0, "Configuration analog input", 0, 0, 2, edit_method=input_use_type) 24 | motor_enable = gen.enum([ gen.const("disabled", int_t, 0, "Disabled"), 25 | gen.const("enabled", int_t, 1, "enabled")], 26 | "Enable feedback from this motor") 27 | gen.add("input_motor_one", int_t, 0, "Configuration pulse input", 0, 0, 2, edit_method=motor_enable) 28 | gen.add("input_motor_two", int_t, 0, "Configuration pulse input", 0, 0, 2, edit_method=motor_enable) 29 | 30 | # APOL [pag. 287] 31 | conversion_polarity = gen.enum([ gen.const("direct", int_t, 0, "After gears"), 32 | gen.const("inverse", int_t, 1, "Before gears")], 33 | "Polarity conversion") 34 | gen.add("conversion_polarity", int_t, 0, "Conversion analog input", 1, 0, 1, edit_method=conversion_polarity) 35 | 36 | # ADB [pag. 281] 37 | gen.add("input_deadband", int_t, 0, "[%] How the movement near to zero is considered zero", 0, 0, 50) 38 | 39 | # AMIN [pag. 285] 40 | gen.add("range_input_min", double_t, 0, "[V] Set the analog input min", 0.1, 0, 10) 41 | 42 | # AMAX [pag. 283] 43 | gen.add("range_input_max", double_t, 0, "[V] Set the analog input max", 4.9, 0, 10) 44 | 45 | # ACTR [pag. 281] 46 | gen.add("range_input_center", double_t, 0, "[V] Set the analog input center", 2.5, 0, 10) 47 | 48 | #Default command 49 | gen.add("load_roboteq", bool_t , 0, "Load all parameters from Roboteq board", False) 50 | gen.add("restore_defaults", bool_t , 0, "Restore to the original configuration", False ) 51 | 52 | exit(gen.generate(PACKAGE, "roboteq_node", "RoboteqAnalogInput")) 53 | -------------------------------------------------------------------------------- /launch/roboteq.launch: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 44 | 45 | 46 | serial_port: $(arg port) 47 | serial_rate: 115200 48 | control_frequency: 50.0 49 | diagnostic_frequency: 5.0 50 | 51 | 52 | 53 | 54 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | roboteq_control 30 | 1.0.0 31 | roboteq_control is a ros control package to control your roboteq_board 32 | 33 | Raffaello Bonghi 34 | MIT 35 | 36 | https://rnext.it/ 37 | https://github.com/rbonghi/roboteq_control 38 | Raffaello Bonghi 39 | 40 | catkin 41 | 42 | genmsg 43 | 44 | roscpp 45 | std_msgs 46 | diagnostic_msgs 47 | serial 48 | joint_limits_interface 49 | controller_manager 50 | dynamic_reconfigure 51 | diagnostic_updater 52 | hardware_interface 53 | 54 | xacro 55 | robot_state_publisher 56 | 57 | -------------------------------------------------------------------------------- /cfg/RoboteqPulseInput.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "roboteq_control" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | # to check PLIN - pulse linearity [pag. 292] 9 | 10 | # PMOD [pag. 295] 11 | conversion_type = gen.enum([ gen.const("conversion_disabled", int_t, 0, "Disable pulse input"), 12 | gen.const("pulse_with", int_t, 1, "Enable in pulse with mode"), 13 | gen.const("frequency", int_t, 2, "Enable in frequency mode"), 14 | gen.const("duty_cycle", int_t, 3, "Enable in duty cycle mode"), 15 | gen.const("magsensor", int_t, 4, "Enable in magsensor mode"),], 16 | "Type of pulse input conversion") 17 | gen.add("conversion", int_t, 0, "Type of pulse input conversion", 0, 0, 4, edit_method=conversion_type) 18 | 19 | # PINA [pag. 291] 20 | input_use_type = gen.enum([ gen.const("input_disabled", int_t, 0, "Disabled"), 21 | gen.const("motor_command", int_t, 1, "Motor command"), 22 | gen.const("feedback", int_t, 2, "Feedback")], 23 | "configuration") 24 | gen.add("input_use", int_t, 0, "Configuration pulse input", 0, 0, 2, edit_method=input_use_type) 25 | motor_enable = gen.enum([ gen.const("disabled", int_t, 0, "Disabled"), 26 | gen.const("enabled", int_t, 1, "enabled")], 27 | "Enable feedback from this motor") 28 | gen.add("input_motor_one", int_t, 0, "Configuration pulse input", 0, 0, 2, edit_method=motor_enable) 29 | gen.add("input_motor_two", int_t, 0, "Configuration pulse input", 0, 0, 2, edit_method=motor_enable) 30 | 31 | # PPOL [pag. 296] 32 | conversion_polarity_type = gen.enum([ gen.const("direct", int_t, 0, "After gears"), 33 | gen.const("inverse", int_t, 1, "Before gears")], 34 | "Polarity conversion") 35 | gen.add("conversion_polarity", int_t, 0, "Conversion pulse input", 1, 0, 1, edit_method=conversion_polarity_type) 36 | 37 | # PDB [pag. 291] 38 | gen.add("input_deadband", int_t, 0, "[%] How the movement near to zero is considered zero", 0, 0, 50) 39 | 40 | # PMIN [pag. 294] 41 | gen.add("range_input_min", double_t, 0, "[ms] Set the pulse input min", 1.0, 0, 65.536) 42 | 43 | # PMAX [pag. 293] 44 | gen.add("range_input_max", double_t, 0, "[ms] Set the pulse input max", 2.0, 0, 65.536) 45 | 46 | # PTCR [pag. 290] 47 | gen.add("range_input_center", double_t, 0, "[ms] Set the pulse input center", 1.5, 0, 65.536) 48 | 49 | #Default command 50 | gen.add("load_roboteq", bool_t , 0, "Load all parameters from Roboteq board", False) 51 | gen.add("restore_defaults", bool_t , 0, "Restore to the original configuration", False ) 52 | 53 | exit(gen.generate(PACKAGE, "roboteq_node", "RoboteqPulseInput")) 54 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | :gear: roboteq control [![Build Status](https://travis-ci.org/rbonghi/roboteq_control.svg?branch=master)](https://travis-ci.org/rbonghi/roboteq_control) 2 | ======= 3 | 4 | roboteq_control is a Roboteq motor control **[ros_control][ros_control]** based. 5 | This package use a Roboteq device with a serial port. 6 | 7 | All parameters, GPIO, Analogs port are controlled by this driver and from dynamic_reconfigure you can setup as you wish this board. 8 | 9 | Device included: 10 | 11 | | Brushed DC | Brushless DC | Sepex | 12 | | ---------- | ------------ | ----- | 13 | | HDC24xx, VDC24xx, MDC22xx, LDC22xx, LDC14xx, SDC1130, SDC21xx | HBL16xx, VBL16xx, HBL23xx, VBL23xx, LBL13xx, MBL16xx, SBL13xx | VSX18xx | 14 | 15 | Advanced Digital Motor Controllers, as described in [this document][roboteq_manual]. 16 | 17 | # Install 18 | 19 | Clone on your catkin workspace this repository, download all dependencies and compile! 20 | 21 | ```bash 22 | # Make catkin workspace if does not exist and clone this repo 23 | mkdir -p ~/catkin_ws/src 24 | cd ~/catkin_ws/src 25 | git clone https://github.com/rbonghi/roboteq_control.git 26 | # Install all dependecies 27 | cd .. 28 | rosdep install --from-paths src --ignore-src -r -y 29 | # Compile package 30 | catkin_make 31 | ``` 32 | 33 | Don't forget to add in your bash your catkin sources 34 | 35 | ```bash 36 | echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc 37 | source ~/.bashrc 38 | ``` 39 | 40 | # Run 41 | 42 | ```bash 43 | roslaunch roboteq_control roboteq.launch 44 | ``` 45 | 46 | :rocket: That's it! 47 | 48 | This launch file can load different parameters such as: 49 | * port - Serial port (defaut: /dev/ttyUSB0) 50 | * config - Configuration file (Example in config file) 51 | 52 | This driver include a dynamic_reconfigure topics to dynamically update all parameters of your robot 53 | 54 | ![roboteq_control](https://github.com/rbonghi/roboteq_control/wiki/images/dynamic_reconfigure.png) 55 | 56 | Detailed information are available on [wiki](https://github.com/rbonghi/roboteq_control/wiki) 57 | 58 | # Example 59 | 60 | This package include a differential drive example to drive a robot. 61 | 62 | ```bash 63 | roslaunch roboteq_control differential_drive.launch 64 | ``` 65 | 66 | There are different parameters than you can setup: 67 | * size - default: 25cm 68 | * radius - default: 8cm 69 | * wheelbase - default: 0.40cm 70 | 71 | ![roboteq_control](https://github.com/rbonghi/roboteq_control/wiki/images/roboteq_control.png) 72 | 73 | To control this robot are available this topics 74 | 75 | **Subscribers:** 76 | * /velocity_controller/cmd_vel [geometry_msgs/Twist] 77 | * /roboteq/emergency_stop [std_msgs/Bool] 78 | 79 | **Publishers:** 80 | * /velocity_controller/odom [nav_msgs/Odometry] 81 | 82 | ![roboteq_control](https://github.com/rbonghi/roboteq_control/wiki/images/rosgraph_simple.png) 83 | 84 | [roboteq_manual]: https://www.roboteq.com/docman-list/motor-controllers-documents-and-files/documentation/user-manual/272-roboteq-controllers-user-manual-v17/file 85 | [ros_control]: http://wiki.ros.org/ros_control 86 | -------------------------------------------------------------------------------- /launch/differential_drive.launch: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 30 | 31 | 32 | 38 | 39 | 40 | 41 | 42 | 43 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /include/configurator/motor_pid.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (C) 2017, Raffaello Bonghi 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 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * 3. Neither the name of the copyright holder nor the names of its 15 | * contributors may be used to endorse or promote products derived 16 | * from this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND 19 | * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 20 | * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 21 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 22 | * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 23 | * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 24 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 25 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 26 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 28 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #ifndef GPIOPIDCONFIGURATOR_H 32 | #define GPIOPICCONFIGURATOR_H 33 | 34 | #include 35 | #include 36 | 37 | #include 38 | 39 | #include "roboteq/serial_controller.h" 40 | 41 | class MotorPIDConfigurator 42 | { 43 | public: 44 | /** 45 | * @brief MotorPIDConfigurator Initialize the dynamic reconfigurator 46 | * @param nh Nodehandle of the system 47 | * @param serial serial port 48 | * @param path original path to start to find all rosparam variable 49 | * @param name name of the PID configuration 50 | * @param number number of motor 51 | */ 52 | MotorPIDConfigurator(const ros::NodeHandle& nh, roboteq::serial_controller *serial, string path, string name, unsigned int number); 53 | 54 | void initConfigurator(bool load_from_board); 55 | 56 | void setPIDconfiguration(); 57 | 58 | private: 59 | /// Setup variable 60 | bool setup_pid; 61 | 62 | /// Associate name space 63 | string mName; 64 | string mType; 65 | /// Number motor 66 | unsigned int mNumber; 67 | /// Private namespace 68 | ros::NodeHandle nh_; 69 | /// Serial port 70 | roboteq::serial_controller* mSerial; 71 | 72 | /// Dynamic reconfigure PID 73 | typedef dynamic_reconfigure::Server ReconfigureServer; 74 | std::shared_ptr mDynRecServer; 75 | boost::recursive_mutex mDynServerMutex; // To avoid Dynamic Reconfigure Server warning 76 | /** 77 | * @brief reconfigureCBEncoder when the dynamic reconfigurator change some values start this method 78 | * @param config variable with all configuration from dynamic reconfigurator 79 | * @param level 80 | */ 81 | void reconfigureCBPID(roboteq_control::RoboteqPIDConfig &config, uint32_t level); 82 | 83 | // Default parameter config 84 | roboteq_control::RoboteqPIDConfig default_pid_config, _last_pid_config; 85 | 86 | /** 87 | * @brief getPIDFromRoboteq Load PID parameters from Roboteq board 88 | */ 89 | void getPIDFromRoboteq(); 90 | }; 91 | 92 | #endif // GPIOPICCONFIGURATOR_H 93 | -------------------------------------------------------------------------------- /include/configurator/gpio_pulse.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (C) 2017, Raffaello Bonghi 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 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * 3. Neither the name of the copyright holder nor the names of its 15 | * contributors may be used to endorse or promote products derived 16 | * from this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND 19 | * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 20 | * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 21 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 22 | * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 23 | * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 24 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 25 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 26 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 28 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #ifndef GPIOPULSECONFIGURATOR_H 32 | #define GPIOPULSECONFIGURATOR_H 33 | 34 | #include 35 | 36 | #include 37 | #include 38 | 39 | #include "roboteq/serial_controller.h" 40 | #include "configurator/gpio_sensor.h" 41 | #include "roboteq/motor.h" 42 | 43 | class GPIOPulseConfigurator : GPIOSensor 44 | { 45 | public: 46 | /** 47 | * @brief GPIOPulseConfigurator 48 | * @param nh 49 | * @param serial 50 | * @param number 51 | */ 52 | GPIOPulseConfigurator(const ros::NodeHandle& nh, roboteq::serial_controller *serial, std::vector motor, string name, unsigned int number); 53 | /** 54 | * @brief initConfigurator Initialize all parameter and syncronize parameters between ros and roboteq board 55 | * @param load_from_board If true load all paramter from roboteq board 56 | */ 57 | void initConfigurator(bool load_from_board); 58 | /** 59 | * @brief getConversion Get conversion from pulse value to real value 60 | * @return the value of reduction before encoder 61 | */ 62 | double getConversion(double reduction) 63 | { 64 | return 0; 65 | } 66 | 67 | private: 68 | /// Setup variable 69 | bool setup_param; 70 | 71 | /// Associate name space 72 | string mName; 73 | /// Number motor 74 | unsigned int mNumber; 75 | /// Private namespace 76 | ros::NodeHandle nh_; 77 | /// Serial port 78 | roboteq::serial_controller* mSerial; 79 | // List of all motors 80 | std::vector _motor; 81 | 82 | /// Dynamic reconfigure encoder 83 | typedef dynamic_reconfigure::Server ReconfigureServer; 84 | std::shared_ptr mDynRecServer; 85 | boost::recursive_mutex mDynServerMutex; // To avoid Dynamic Reconfigure Server warning 86 | /** 87 | * @brief reconfigureCBParam when the dynamic reconfigurator change some values start this method 88 | * @param config variable with all configuration from dynamic reconfigurator 89 | * @param level 90 | */ 91 | void reconfigureCBParam(roboteq_control::RoboteqPulseInputConfig &config, uint32_t level); 92 | 93 | // Default parameter config 94 | roboteq_control::RoboteqPulseInputConfig default_param_config, _last_param_config; 95 | 96 | /** 97 | * @brief getParamFromRoboteq Load parameters from Roboteq board 98 | */ 99 | void getParamFromRoboteq(); 100 | }; 101 | 102 | #endif // GPIOPULSECONFIGURATOR_H 103 | -------------------------------------------------------------------------------- /include/configurator/gpio_encoder.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (C) 2017, Raffaello Bonghi 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 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * 3. Neither the name of the copyright holder nor the names of its 15 | * contributors may be used to endorse or promote products derived 16 | * from this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND 19 | * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 20 | * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 21 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 22 | * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 23 | * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 24 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 25 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 26 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 28 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #ifndef GPIOENCODERCONFIGURATOR_H 32 | #define GPIOENCODERCONFIGURATOR_H 33 | 34 | #include 35 | 36 | #include 37 | #include 38 | 39 | #include "roboteq/serial_controller.h" 40 | 41 | #include "roboteq/motor.h" 42 | #include "configurator/gpio_sensor.h" 43 | 44 | class GPIOEncoderConfigurator : GPIOSensor 45 | { 46 | public: 47 | /** 48 | * @brief GPIOEncoderConfigurator 49 | * @param nh 50 | * @param serial 51 | * @param number 52 | */ 53 | GPIOEncoderConfigurator(const ros::NodeHandle& nh, roboteq::serial_controller *serial, std::vector motor, string name, unsigned int number); 54 | /** 55 | * @brief initConfigurator Initialize all parameter and syncronize parameters between ros and roboteq board 56 | * @param load_from_board If true load all paramter from roboteq board 57 | */ 58 | void initConfigurator(bool load_from_board); 59 | /** 60 | * @brief getConversion Get conversion from pulse value to real value 61 | * @return the value of reduction before encoder 62 | */ 63 | double getConversion(double reduction); 64 | 65 | private: 66 | /// Setup variable 67 | bool setup_encoder; 68 | 69 | /// Associate name space 70 | string mName; 71 | /// Number motor 72 | unsigned int mNumber; 73 | /// Private namespace 74 | ros::NodeHandle nh_; 75 | /// Serial port 76 | roboteq::serial_controller* mSerial; 77 | // List of all motors 78 | std::vector _motor; 79 | 80 | // reduction value 81 | double _reduction; 82 | 83 | /// Dynamic reconfigure encoder 84 | typedef dynamic_reconfigure::Server ReconfigureServer; 85 | std::shared_ptr mDynRecServer; 86 | boost::recursive_mutex mDynServerMutex; // To avoid Dynamic Reconfigure Server warning 87 | /** 88 | * @brief reconfigureCBEncoder when the dynamic reconfigurator change some values start this method 89 | * @param config variable with all configuration from dynamic reconfigurator 90 | * @param level 91 | */ 92 | void reconfigureCBEncoder(roboteq_control::RoboteqEncoderConfig &config, uint32_t level); 93 | 94 | roboteq_control::RoboteqEncoderConfig default_encoder_config, _last_encoder_config; 95 | 96 | /** 97 | * @brief getEncoderFromRoboteq Load Encoder parameters from Roboteq board 98 | */ 99 | void getEncoderFromRoboteq(); 100 | }; 101 | 102 | #endif // GPIOENCODERCONFIGURATOR_H 103 | -------------------------------------------------------------------------------- /include/configurator/gpio_analog.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (C) 2017, Raffaello Bonghi 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 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * 3. Neither the name of the copyright holder nor the names of its 15 | * contributors may be used to endorse or promote products derived 16 | * from this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND 19 | * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 20 | * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 21 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 22 | * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 23 | * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 24 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 25 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 26 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 28 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #ifndef GPIOANALOGCONFIGURATOR_H 32 | #define GPIOANALOGCONFIGURATOR_H 33 | 34 | #include 35 | 36 | #include 37 | #include 38 | 39 | #include "roboteq/serial_controller.h" 40 | #include "configurator/gpio_sensor.h" 41 | #include "roboteq/motor.h" 42 | 43 | class GPIOAnalogConfigurator : GPIOSensor 44 | { 45 | public: 46 | /** 47 | * @brief GPIOAnalogConfigurator 48 | * @param nh 49 | * @param serial 50 | * @param number 51 | */ 52 | GPIOAnalogConfigurator(const ros::NodeHandle& nh, roboteq::serial_controller *serial, std::vector motor, string name, unsigned int number); 53 | /** 54 | * @brief initConfigurator Initialize all parameter and syncronize parameters between ros and roboteq board 55 | * @param load_from_board If true load all paramter from roboteq board 56 | */ 57 | void initConfigurator(bool load_from_board); 58 | /** 59 | * @brief getConversion Get conversion from pulse value to real value 60 | * @return the value of reduction before encoder 61 | */ 62 | double getConversion(double reduction) 63 | { 64 | return 0; 65 | } 66 | 67 | private: 68 | /// Setup variable 69 | bool setup_param; 70 | 71 | /// Associate name space 72 | string mName; 73 | /// Number motor 74 | unsigned int mNumber; 75 | /// Private namespace 76 | ros::NodeHandle nh_; 77 | /// Serial port 78 | roboteq::serial_controller* mSerial; 79 | // List of all motors 80 | std::vector _motor; 81 | 82 | /// Dynamic reconfigure analog parameters 83 | typedef dynamic_reconfigure::Server ReconfigureServer; 84 | std::shared_ptr mDynRecServer; 85 | boost::recursive_mutex mDynServerMutex; // To avoid Dynamic Reconfigure Server warning 86 | /** 87 | * @brief reconfigureCBParam when the dynamic reconfigurator change some values start this method 88 | * @param config variable with all configuration from dynamic reconfigurator 89 | * @param level 90 | */ 91 | void reconfigureCBParam(roboteq_control::RoboteqAnalogInputConfig &config, uint32_t level); 92 | 93 | // Default parameter config 94 | roboteq_control::RoboteqAnalogInputConfig default_param_config, _last_param_config; 95 | 96 | /** 97 | * @brief getParamFromRoboteq Load parameters from Roboteq board 98 | */ 99 | void getParamFromRoboteq(); 100 | }; 101 | 102 | #endif // GPIOANALOGCONFIGURATOR_H 103 | -------------------------------------------------------------------------------- /urdf/differential-robot.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | -------------------------------------------------------------------------------- /config/roboteq.yaml: -------------------------------------------------------------------------------- 1 | # general configuration 2 | mixing: 0 3 | break_delay: 250 4 | over_voltage_limit: 30.0 5 | over_voltage_hysteresis: 5.0 6 | under_voltage_limit: 5.0 7 | pwm_frequency: 16.0 8 | # Definition motors 9 | joint: [left_joint, right_joint] 10 | 11 | #Definition input and output 12 | InOut: 13 | analog: 14 | '1': {conversion: 0, conversion_polarity: 0, input_deadband: 5, input_motor_one: 0, 15 | input_motor_two: 0, input_use: 0, range_input_center: 2.5, range_input_max: 4.75, range_input_min: 0.25} 16 | '2': {conversion: 0, conversion_polarity: 0, input_deadband: 5, input_motor_one: 0, 17 | input_motor_two: 0, input_use: 0, range_input_center: 2.5, range_input_max: 4.75, range_input_min: 0.25} 18 | '3': {conversion: 0, conversion_polarity: 0, input_deadband: 5, input_motor_one: 0, 19 | input_motor_two: 0, input_use: 0, range_input_center: 2.5, range_input_max: 4.75, range_input_min: 0.25} 20 | '4': {conversion: 0, conversion_polarity: 0, input_deadband: 5, input_motor_one: 0, 21 | input_motor_two: 0, input_use: 0, range_input_center: 2.5, range_input_max: 4.75, range_input_min: 0.25} 22 | '5': {conversion: 0, conversion_polarity: 0, input_deadband: 0, input_motor_one: 0, 23 | input_motor_two: 0, input_use: 0, range_input_center: 0.0, range_input_max: 0.0, range_input_min: 0.0} 24 | '6': {conversion: 0, conversion_polarity: 0, input_deadband: 0, input_motor_one: 0, 25 | input_motor_two: 0, input_use: 0, range_input_center: 0.0, range_input_max: 0.0, range_input_min: 0.0} 26 | pulse: 27 | '1': {conversion: 1, conversion_polarity: 0, input_deadband: 5, input_motor_one: 1, input_motor_two: 0, 28 | input_use: 1, range_input_center: 1.5, range_input_max: 2.0, range_input_min: 1.0} 29 | '2': {conversion: 1, conversion_polarity: 0, input_deadband: 5, input_motor_one: 0, input_motor_two: 1, 30 | input_use: 1, range_input_center: 1.5, range_input_max: 2.0, range_input_min: 1.0} 31 | '3': {conversion: 0, conversion_polarity: 0, input_deadband: 5, input_motor_one: 0, input_motor_two: 0, 32 | input_use: 0, range_input_center: 1.5, range_input_max: 2.0, range_input_min: 1.0} 33 | '4': {conversion: 0, conversion_polarity: 0, input_deadband: 5, input_motor_one: 0, input_motor_two: 0, 34 | input_use: 0, range_input_center: 1.5, range_input_max: 2.0, range_input_min: 1.0} 35 | '5': {conversion: 0, conversion_polarity: 0, input_deadband: 5, input_motor_one: 0, input_motor_two: 0, 36 | input_use: 0, range_input_center: 1.5, range_input_max: 2.0, range_input_min: 1.0} 37 | '6': {conversion: 0, conversion_polarity: 0, input_deadband: 0, input_motor_one: 0, input_motor_two: 0, 38 | input_use: 0, range_input_center: 0.0, range_input_max: 0.0, range_input_min: 0.0} 39 | encoder: 40 | '1': {PPR: 400, configuration: 2, encoder_high_count_limit: 20000, encoder_home_count: 0, 41 | encoder_low_count_limit: -20000, input_motor_one: 1, input_motor_two: 0, position: 1} 42 | '2': {PPR: 400, configuration: 2, encoder_high_count_limit: 20000, encoder_home_count: 0, 43 | encoder_low_count_limit: -20000, input_motor_one: 0, input_motor_two: 1, position: 1} 44 | 45 | #Definition motors 46 | left_joint: 47 | number: 2 48 | ratio: 16.0 49 | rotation: 1 50 | stall_detection: 2 51 | amper_limit: 20.0 52 | max_speed: 256.0 53 | max_acceleration: 3125.0 54 | max_deceleration: 3125.0 55 | max_forward: 100 56 | max_reverse: 100 57 | pid: 58 | closed_loop_position: 2 59 | closed_loop_velocity: 6 60 | position: {Kp: 0.0, Ki: 0.0, Kd: 0.0, integrator_limit: 100, loop_error_detection: 0, position_mode_velocity: 0, turn_min_to_max: 0.0} 61 | torque: {Kp: 0.0, Ki: 0.0, Kd: 0.0, integrator_limit: 100, loop_error_detection: 0, position_mode_velocity: 0, turn_min_to_max: 0.0} 62 | velocity: {Kp: 2.0, Ki: 2.0, Kd: 0.0, integrator_limit: 100, loop_error_detection: 2, position_mode_velocity: 1000, turn_min_to_max: 25.0} 63 | 64 | right_joint: 65 | number: 1 66 | ratio: 16.0 67 | rotation: 1 68 | stall_detection: 2 69 | amper_limit: 20.0 70 | max_speed: 256.0 71 | max_acceleration: 3125.0 72 | max_deceleration: 3125.0 73 | max_forward: 100 74 | max_reverse: 100 75 | pid: 76 | closed_loop_position: 2 77 | closed_loop_velocity: 6 78 | position: {Kp: 0.0, Ki: 0.0, Kd: 0.0, integrator_limit: 100, loop_error_detection: 0, position_mode_velocity: 0, turn_min_to_max: 0.0} 79 | torque: {Kp: 0.0, Ki: 0.0, Kd: 0.0, integrator_limit: 100, loop_error_detection: 0, position_mode_velocity: 0, turn_min_to_max: 0.0} 80 | velocity: {Kp: 2.0, Ki: 2.0, Kd: 0.0, integrator_limit: 100, loop_error_detection: 2, position_mode_velocity: 1000, turn_min_to_max: 25.0} -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(roboteq_control) 3 | 4 | ## Add support for C++11, supported in ROS Kinetic and newer 5 | add_definitions(-std=c++11) 6 | 7 | ## Find catkin macros and librariess 8 | find_package(catkin REQUIRED 9 | COMPONENTS 10 | roscpp 11 | genmsg 12 | dynamic_reconfigure 13 | serial 14 | joint_limits_interface 15 | controller_manager 16 | diagnostic_updater 17 | hardware_interface 18 | std_msgs 19 | urdf 20 | ) 21 | ## System dependencies are found with CMake's conventions 22 | find_package(Boost REQUIRED 23 | COMPONENTS 24 | chrono 25 | system 26 | thread 27 | ) 28 | 29 | ################################################ 30 | ## Declare ROS dynamic reconfigure parameters ## 31 | ################################################ 32 | 33 | ## Generate messages in the 'msg' folder 34 | add_message_files( 35 | FILES 36 | ControlStatus.msg 37 | MotorStatus.msg 38 | Peripheral.msg 39 | ) 40 | 41 | ## Generate services in the 'srv' folder 42 | add_service_files( 43 | FILES 44 | Service.srv 45 | ) 46 | 47 | # dynamic reconfigure 48 | generate_dynamic_reconfigure_options( 49 | cfg/RoboteqController.cfg 50 | cfg/RoboteqParameter.cfg 51 | cfg/RoboteqEncoder.cfg 52 | cfg/RoboteqPIDtype.cfg 53 | cfg/RoboteqPID.cfg 54 | cfg/RoboteqAnalogInput.cfg 55 | cfg/RoboteqPulseInput.cfg 56 | ) 57 | 58 | ## Generate added messages and services 59 | generate_messages(DEPENDENCIES std_msgs) 60 | 61 | ################################### 62 | ## catkin specific configuration ## 63 | ################################### 64 | catkin_package( 65 | INCLUDE_DIRS 66 | include 67 | CATKIN_DEPENDS 68 | roscpp 69 | dynamic_reconfigure 70 | serial 71 | joint_limits_interface 72 | controller_manager 73 | diagnostic_updater 74 | hardware_interface 75 | DEPENDS 76 | Boost 77 | ) 78 | 79 | ########### 80 | ## Build ## 81 | ########### 82 | 83 | ## Specify additional locations of header files 84 | ## Your package locations should be listed before other locations 85 | include_directories( 86 | include 87 | ${Boost_INCLUDE_DIRS} 88 | ${catkin_INCLUDE_DIRS} 89 | ) 90 | 91 | set(roboteq_control_SRC 92 | src/roboteq_control.cpp 93 | src/roboteq/serial_controller.cpp 94 | src/roboteq/roboteq.cpp 95 | src/roboteq/motor.cpp 96 | src/configurator/motor_param.cpp 97 | src/configurator/motor_pid.cpp 98 | src/configurator/gpio_analog.cpp 99 | src/configurator/gpio_pulse.cpp 100 | src/configurator/gpio_encoder.cpp 101 | ) 102 | 103 | # Declare a cpp executable 104 | add_executable(${PROJECT_NAME}_node ${roboteq_control_SRC}) 105 | add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS}) 106 | target_link_libraries(${PROJECT_NAME}_node ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 107 | set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME driver_node PREFIX "") 108 | 109 | ## Declare a cpp executable 110 | #add_executable(roboteq_node ${roboteq_control_SRC}) 111 | #target_link_libraries(roboteq_node ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 112 | #add_dependencies(roboteq_node ${${PROJECT_NAME}_EXPORTED_TARGETS}) 113 | 114 | ############# 115 | ## Install ## 116 | ############# 117 | 118 | # all install targets should use catkin DESTINATION variables 119 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 120 | 121 | # Mark executables and/or libraries for installation 122 | install(TARGETS ${PROJECT_NAME}_node 123 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 124 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 125 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 126 | ) 127 | 128 | # Mark cpp header files for installation 129 | install(DIRECTORY include/ 130 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 131 | FILES_MATCHING PATTERN "*.h" 132 | PATTERN ".git" EXCLUDE 133 | ) 134 | 135 | ## Mark other files for installation (e.g. launch and bag files, etc.) 136 | # install(FILES 137 | # # myfile1 138 | # # myfile2 139 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 140 | # ) 141 | 142 | ############# 143 | ## Testing ## 144 | ############# 145 | 146 | ## Add gtest based cpp test target and link libraries 147 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_roboteq_control.cpp) 148 | # if(TARGET ${PROJECT_NAME}-test) 149 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 150 | # endif() 151 | 152 | ## Add folders to be run by python nosetests 153 | # catkin_add_nosetests(test) 154 | -------------------------------------------------------------------------------- /include/configurator/motor_param.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (C) 2017, Raffaello Bonghi 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 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * 3. Neither the name of the copyright holder nor the names of its 15 | * contributors may be used to endorse or promote products derived 16 | * from this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND 19 | * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 20 | * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 21 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 22 | * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 23 | * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 24 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 25 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 26 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 28 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #ifndef GPIOPARAMCONFIGURATOR_H 32 | #define GPIOPARAMCONFIGURATOR_H 33 | 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | #include "roboteq/serial_controller.h" 42 | 43 | class MotorParamConfigurator 44 | { 45 | public: 46 | /** 47 | * @brief MotorParamConfigurator 48 | * @param nh 49 | * @param serial 50 | * @param name 51 | * @param number 52 | */ 53 | MotorParamConfigurator(const ros::NodeHandle& nh, roboteq::serial_controller *serial, std::string name, unsigned int number); 54 | /** 55 | * @brief initConfigurator Initialize all parameter and syncronize parameters between ros and roboteq board 56 | * @param load_from_board If true load all paramter from roboteq board 57 | */ 58 | void initConfigurator(bool load_from_board); 59 | 60 | int getOperativeMode(); 61 | /** 62 | * @brief setOperativeMode Reference in page 321 - MMOD 63 | * @param type The operating_mode 64 | * 0 - open_loop 65 | * 1 - closed_loop_speed 66 | * 2 - closed_loop_position_relative 67 | * 3 - closed_loop_count_position 68 | * 4 - closed_loop_position_tracking 69 | * 5 - torque 70 | * 6 - closed_loop_speed_position 71 | */ 72 | void setOperativeMode(int type); 73 | 74 | private: 75 | /// Setup variable 76 | bool setup_param, setup_pid_type; 77 | 78 | /// Associate name space 79 | string mName; 80 | /// Number motor 81 | unsigned int mNumber; 82 | /// Private namespace 83 | ros::NodeHandle nh_; 84 | /// Serial port 85 | roboteq::serial_controller* mSerial; 86 | 87 | /// Dynamic reconfigure parameters 88 | typedef dynamic_reconfigure::Server ReconfigureServerParam; 89 | std::shared_ptr mDynRecServer_param; 90 | boost::recursive_mutex mDynServerMutex_param; // To avoid Dynamic Reconfigure Server warning 91 | /** 92 | * @brief reconfigureCBParam when the dynamic reconfigurator change some values start this method 93 | * @param config variable with all configuration from dynamic reconfigurator 94 | * @param level 95 | */ 96 | void reconfigureCBParam(roboteq_control::RoboteqParameterConfig &config, uint32_t level); 97 | 98 | /// Dynamic reconfigure PID 99 | typedef dynamic_reconfigure::Server ReconfigureServerPID; 100 | std::shared_ptr mDynRecServer_pid; 101 | boost::recursive_mutex mDynServerMutex_pid; // To avoid Dynamic Reconfigure Server warning 102 | /** 103 | * @brief reconfigureCBEncoder when the dynamic reconfigurator change some values start this method 104 | * @param config variable with all configuration from dynamic reconfigurator 105 | * @param level 106 | */ 107 | void reconfigureCBPIDtype(roboteq_control::RoboteqPIDtypeConfig &config, uint32_t level); 108 | 109 | // Default parameter config 110 | roboteq_control::RoboteqParameterConfig default_param_config, _last_param_config; 111 | roboteq_control::RoboteqPIDtypeConfig default_pid_type_config, _last_pid_type_config; 112 | 113 | /** 114 | * @brief getParamFromRoboteq Load parameters from Roboteq board 115 | */ 116 | void getParamFromRoboteq(); 117 | 118 | }; 119 | 120 | #endif // GPIOPARAMCONFIGURATOR_H 121 | -------------------------------------------------------------------------------- /rviz/urdf.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /Status1 9 | - /Grid1 10 | Splitter Ratio: 0.5 11 | Tree Height: 549 12 | - Class: rviz/Selection 13 | Name: Selection 14 | - Class: rviz/Tool Properties 15 | Expanded: 16 | - /2D Pose Estimate1 17 | - /2D Nav Goal1 18 | - /Publish Point1 19 | Name: Tool Properties 20 | Splitter Ratio: 0.5886790156364441 21 | - Class: rviz/Views 22 | Expanded: 23 | - /Current View1 24 | Name: Views 25 | Splitter Ratio: 0.5 26 | - Class: rviz/Time 27 | Experimental: false 28 | Name: Time 29 | SyncMode: 0 30 | SyncSource: "" 31 | Preferences: 32 | PromptSaveOnExit: true 33 | Toolbars: 34 | toolButtonStyle: 2 35 | Visualization Manager: 36 | Class: "" 37 | Displays: 38 | - Alpha: 0.5 39 | Cell Size: 0.10000000149011612 40 | Class: rviz/Grid 41 | Color: 160; 160; 164 42 | Enabled: true 43 | Line Style: 44 | Line Width: 0.029999999329447746 45 | Value: Lines 46 | Name: Grid 47 | Normal Cell Count: 0 48 | Offset: 49 | X: 0 50 | Y: 0 51 | Z: 0 52 | Plane: XY 53 | Plane Cell Count: 40 54 | Reference Frame: 55 | Value: true 56 | - Alpha: 1 57 | Class: rviz/RobotModel 58 | Collision Enabled: false 59 | Enabled: true 60 | Links: 61 | All Links Enabled: true 62 | Expand Joint Details: false 63 | Expand Link Details: false 64 | Expand Tree: false 65 | Link Tree Style: Links in Alphabetic Order 66 | base_link: 67 | Alpha: 1 68 | Show Axes: false 69 | Show Trail: false 70 | Value: true 71 | left_wheel: 72 | Alpha: 1 73 | Show Axes: false 74 | Show Trail: false 75 | Value: true 76 | right_wheel: 77 | Alpha: 1 78 | Show Axes: false 79 | Show Trail: false 80 | Value: true 81 | Name: RobotModel 82 | Robot Description: robot_description 83 | TF Prefix: "" 84 | Update Interval: 0 85 | Value: true 86 | Visual Enabled: true 87 | - Class: rviz/TF 88 | Enabled: true 89 | Frame Timeout: 15 90 | Frames: 91 | All Enabled: true 92 | base_link: 93 | Value: true 94 | left_wheel: 95 | Value: true 96 | odom: 97 | Value: true 98 | right_wheel: 99 | Value: true 100 | Marker Scale: 0.5 101 | Name: TF 102 | Show Arrows: true 103 | Show Axes: true 104 | Show Names: true 105 | Tree: 106 | odom: 107 | base_link: 108 | left_wheel: 109 | {} 110 | right_wheel: 111 | {} 112 | Update Interval: 0 113 | Value: true 114 | Enabled: true 115 | Global Options: 116 | Background Color: 48; 48; 48 117 | Default Light: true 118 | Fixed Frame: odom 119 | Frame Rate: 30 120 | Name: root 121 | Tools: 122 | - Class: rviz/Interact 123 | Hide Inactive Objects: true 124 | - Class: rviz/MoveCamera 125 | - Class: rviz/Select 126 | - Class: rviz/FocusCamera 127 | - Class: rviz/Measure 128 | - Class: rviz/SetInitialPose 129 | Theta std deviation: 0.2617993950843811 130 | Topic: /initialpose 131 | X std deviation: 0.5 132 | Y std deviation: 0.5 133 | - Class: rviz/SetGoal 134 | Topic: /move_base_simple/goal 135 | - Class: rviz/PublishPoint 136 | Single click: true 137 | Topic: /clicked_point 138 | Value: true 139 | Views: 140 | Current: 141 | Class: rviz/Orbit 142 | Distance: 0.8466232419013977 143 | Enable Stereo Rendering: 144 | Stereo Eye Separation: 0.05999999865889549 145 | Stereo Focal Distance: 1 146 | Swap Stereo Eyes: false 147 | Value: false 148 | Focal Point: 149 | X: 0 150 | Y: 0 151 | Z: 0 152 | Focal Shape Fixed Size: true 153 | Focal Shape Size: 0.05000000074505806 154 | Invert Z Axis: false 155 | Name: Current View 156 | Near Clip Distance: 0.009999999776482582 157 | Pitch: 0.5303983688354492 158 | Target Frame: 159 | Value: Orbit (rviz) 160 | Yaw: 0.575397789478302 161 | Saved: ~ 162 | Window Geometry: 163 | Displays: 164 | collapsed: false 165 | Height: 846 166 | Hide Left Dock: false 167 | Hide Right Dock: true 168 | QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 169 | Selection: 170 | collapsed: false 171 | Time: 172 | collapsed: false 173 | Tool Properties: 174 | collapsed: false 175 | Views: 176 | collapsed: true 177 | Width: 1200 178 | X: 174 179 | Y: 58 180 | -------------------------------------------------------------------------------- /src/roboteq_control.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (C) 2017, Raffaello Bonghi 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 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * 3. Neither the name of the copyright holder nor the names of its 15 | * contributors may be used to endorse or promote products derived 16 | * from this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND 19 | * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 20 | * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 21 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 22 | * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 23 | * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 24 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 25 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 26 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 28 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #include "roboteq/serial_controller.h" 37 | #include "roboteq/roboteq.h" 38 | 39 | #include 40 | #include 41 | 42 | using namespace std; 43 | 44 | typedef boost::chrono::steady_clock time_source; 45 | 46 | ros::Timer control_loop; 47 | ros::Timer diagnostic_loop; 48 | 49 | ros::AsyncSpinner *roboteq_spinner; 50 | roboteq::Roboteq *interface; 51 | roboteq::serial_controller *rSerial; 52 | 53 | // >>>>> Ctrl+C handler 54 | void siginthandler(int param) 55 | { 56 | ROS_INFO("User pressed Ctrl+C Shutting down..."); 57 | ROS_INFO("Stop Control & Diagnostic loop"); 58 | control_loop.stop(); 59 | diagnostic_loop.stop(); 60 | roboteq_spinner->stop(); 61 | delete roboteq_spinner; 62 | ROS_INFO("Release motors"); 63 | // Switch off motors and release 64 | interface->switch_off(); 65 | delete interface; 66 | // Switch off serial 67 | rSerial->stop(); 68 | ROS_INFO_STREAM("--------- ROBOTEQ_NODE STOPPED ---------"); 69 | delete rSerial; 70 | ros::shutdown(); 71 | } 72 | // <<<<< Ctrl+C handler 73 | /** 74 | * Control loop not realtime safe 75 | */ 76 | void controlLoop(roboteq::Roboteq &roboteq, 77 | controller_manager::ControllerManager &cm, 78 | time_source::time_point &last_time) 79 | { 80 | 81 | // Calculate monotonic time difference 82 | time_source::time_point this_time = time_source::now(); 83 | boost::chrono::duration elapsed_duration = this_time - last_time; 84 | ros::Duration elapsed(elapsed_duration.count()); 85 | last_time = this_time; 86 | 87 | //ROS_INFO_STREAM("CONTROL - running"); 88 | // Process control loop 89 | roboteq.read(ros::Time::now(), elapsed); 90 | cm.update(ros::Time::now(), elapsed); 91 | roboteq.write(ros::Time::now(), elapsed); 92 | } 93 | 94 | /** 95 | * Diagnostics loop for Roboteq board, not realtime safe 96 | */ 97 | void diagnosticLoop(roboteq::Roboteq &roboteq) 98 | { 99 | //ROS_INFO_STREAM("DIAGNOSTIC - running"); 100 | roboteq.updateDiagnostics(); 101 | } 102 | 103 | int main(int argc, char **argv) { 104 | 105 | ros::init(argc, argv, "roboteq_control"); 106 | ros::NodeHandle nh, private_nh("~"); 107 | 108 | signal(SIGINT, siginthandler); 109 | ROS_INFO_STREAM("----------------------------------------"); 110 | ROS_INFO_STREAM("------------- ROBOTEQ_NODE -------------"); 111 | //Hardware information 112 | double control_frequency, diagnostic_frequency; 113 | private_nh.param("control_frequency", control_frequency, 1.0); 114 | private_nh.param("diagnostic_frequency", diagnostic_frequency, 1.0); 115 | ROS_INFO_STREAM("Control:" << control_frequency << "Hz - Diagnostic:" << diagnostic_frequency << "Hz"); 116 | 117 | string serial_port_string; 118 | int32_t baud_rate; 119 | 120 | private_nh.param("serial_port", serial_port_string, "/dev/ttyACM0"); 121 | private_nh.param("serial_rate", baud_rate, 115200); 122 | ROS_INFO_STREAM("Open Serial " << serial_port_string << ":" << baud_rate); 123 | 124 | rSerial = new roboteq::serial_controller(serial_port_string, baud_rate); 125 | // Run the serial controller 126 | bool start = rSerial->start(); 127 | // Check connection started 128 | if(start) { 129 | // Initialize roboteq controller 130 | //roboteq::Roboteq interface(nh, private_nh, rSerial); 131 | interface = new roboteq::Roboteq(nh, private_nh, rSerial); 132 | // Initialize the motor parameters 133 | interface->initialize(); 134 | //Initialize all interfaces and setup diagnostic messages 135 | interface->initializeInterfaces(); 136 | 137 | controller_manager::ControllerManager cm(interface, nh); 138 | 139 | // Setup separate queue and single-threaded spinner to process timer callbacks 140 | // that interface with RoboTeq hardware. 141 | // This avoids having to lock around hardware access, but precludes realtime safety 142 | // in the control loop. 143 | ros::CallbackQueue roboteq_queue; 144 | roboteq_spinner = new ros::AsyncSpinner(1, &roboteq_queue); 145 | 146 | time_source::time_point last_time = time_source::now(); 147 | ros::TimerOptions control_timer( 148 | ros::Duration(1 / control_frequency), 149 | boost::bind(controlLoop, boost::ref(*interface), boost::ref(cm), boost::ref(last_time)), 150 | &roboteq_queue); 151 | // Global variable 152 | control_loop = nh.createTimer(control_timer); 153 | 154 | ros::TimerOptions diagnostic_timer( 155 | ros::Duration(1 / diagnostic_frequency), 156 | boost::bind(diagnosticLoop, boost::ref(*interface)), 157 | &roboteq_queue); 158 | diagnostic_loop = nh.createTimer(diagnostic_timer); 159 | 160 | roboteq_spinner->start(); 161 | 162 | std::string name_node = ros::this_node::getName(); 163 | ROS_INFO("Started %s", name_node.c_str()); 164 | 165 | // Process remainder of ROS callbacks separately, mainly ControlManager related 166 | ros::spin(); 167 | // Close node 168 | ROS_INFO_STREAM("----------------------------------------"); 169 | } else { 170 | 171 | ROS_ERROR_STREAM("Error connection, shutting down"); 172 | } 173 | return 0; 174 | 175 | } 176 | -------------------------------------------------------------------------------- /include/roboteq/serial_controller.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (C) 2017, Raffaello Bonghi 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 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * 3. Neither the name of the copyright holder nor the names of its 15 | * contributors may be used to endorse or promote products derived 16 | * from this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND 19 | * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 20 | * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 21 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 22 | * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 23 | * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 24 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 25 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 26 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 28 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #ifndef SERIAL_CONTROLLER_H 32 | #define SERIAL_CONTROLLER_H 33 | 34 | #include 35 | #include 36 | 37 | #include 38 | #include // std::condition_variable 39 | 40 | #include 41 | 42 | using namespace std; 43 | 44 | namespace roboteq { 45 | 46 | /// Read complete callback - Array of callback 47 | typedef function callback_data_t; 48 | 49 | class serial_controller 50 | { 51 | public: 52 | /** 53 | * @brief serial_controller Open the serial controller 54 | * @param port set the port 55 | * @param set the baudrate 56 | */ 57 | serial_controller(string port, unsigned long baudrate); 58 | 59 | ~serial_controller(); 60 | /** 61 | * @brief start Initialize the serial communcation 62 | * @return if open the connection return true 63 | */ 64 | bool start(); 65 | /** 66 | * @brief stop 67 | * @return 68 | */ 69 | bool stop(); 70 | 71 | bool command(string msg, string params="", string type="!"); 72 | 73 | bool query(string msg, string params="", string type="?"); 74 | 75 | string getQuery(string msg, string params="") 76 | { 77 | if(query(msg, params)) 78 | { 79 | return get(); 80 | } 81 | else 82 | { 83 | return ""; 84 | } 85 | } 86 | 87 | bool setParam(string msg, string params="") { 88 | return command(msg, params, "^"); 89 | } 90 | 91 | string getParam(string msg, string params="") { 92 | if(query(msg, params, "~")) 93 | { 94 | return get(); 95 | } 96 | else 97 | { 98 | return ""; 99 | } 100 | } 101 | 102 | bool maintenance(string msg, string params="") 103 | { 104 | return command(msg, params, "%"); 105 | } 106 | 107 | /** 108 | * @brief get Get the message parsed 109 | * @return Return the string received 110 | */ 111 | string get() 112 | { 113 | return sub_data; 114 | } 115 | /** 116 | * @brief getVersionScript The version of the script loaded 117 | * @return The string of roboteq control version 118 | */ 119 | string getVersionScript() 120 | { 121 | return "V" + _script_ver; 122 | } 123 | 124 | /** 125 | * @brief reset Reset the Roboteq board 126 | */ 127 | void reset() 128 | { 129 | // Send reset command 130 | mSerial.write("%RESET 321654987"); 131 | // Wait one second after reset 132 | ros::Duration(1).sleep(); 133 | } 134 | 135 | /** 136 | * @brief factoryReset Factory reset of Roboteq board 137 | * @return the status of write 138 | */ 139 | bool factoryReset() 140 | { 141 | return maintenance("EERST"); 142 | } 143 | /** 144 | * @brief loadFromEEPROM 145 | * @return the status of write 146 | */ 147 | bool loadFromEEPROM() 148 | { 149 | return maintenance("EELD"); 150 | } 151 | 152 | /** 153 | * @brief saveInEEPROM The %EESAV it's a real-time Command must be used to copy the RAM array to Flash. The Flash is copied to RAM every time the device powers up. 154 | * @return the status of write 155 | */ 156 | bool saveInEEPROM() { 157 | return maintenance("EESAV"); 158 | } 159 | 160 | /** 161 | * @brief script Run and stop the script inside the Roboteq 162 | * @param status The status of the script 163 | * @return the status of command 164 | */ 165 | bool script(bool status) { 166 | if(status) 167 | { 168 | return command("R"); 169 | } else 170 | { 171 | return command("R", "0"); 172 | } 173 | } 174 | /** 175 | * @brief echo Enable or disable the echo message 176 | * @param type status of echo 177 | * @return The status of command 178 | */ 179 | bool echo(bool type) { 180 | if(type) { 181 | return setParam("ECHOF", "0"); 182 | } else 183 | { 184 | return setParam("ECHOF", "1"); 185 | } 186 | } 187 | /** 188 | * @brief downloadScript Launch the script update for the Roboteq board 189 | * @return Return true if the script is fully updated 190 | */ 191 | bool downloadScript(); 192 | /** 193 | * @brief addCallback Add callback message 194 | * @param callback The callback function 195 | * @param type The type of message to check 196 | */ 197 | bool addCallback(const callback_data_t &callback, const string data); 198 | /** 199 | * Template to connect a method in callback 200 | */ 201 | template bool addCallback(void(T::*fp)(const string), T* obj, const string data) { 202 | return addCallback(bind(fp, obj, _1), data); 203 | } 204 | private: 205 | // Serial port object 206 | serial::Serial mSerial; 207 | // Serial port name 208 | string mSerialPort; 209 | // Serial port baudrate 210 | uint32_t mBaudrate; 211 | // Timeout open serial port 212 | uint32_t mTimeout; 213 | // Used to stop the serial processing 214 | bool mStopping; 215 | // Last message sent 216 | string mMessage; 217 | string sub_data; 218 | bool sub_data_cmd; 219 | bool data; 220 | // Async reader controller 221 | std::thread first; 222 | // Mutex to sto concurent sending 223 | mutex mWriteMutex; 224 | mutex mReaderMutex; 225 | std::condition_variable cv; 226 | // Hashmap with all type of message 227 | map hashmap; 228 | // HLD mode - To download script - reference [pag. 183] 229 | bool isHLD; 230 | // Version script 231 | string _script_ver; 232 | /** 233 | * @brief async_reader Thread to read realtime all charachters sent from roboteq board 234 | */ 235 | void async_reader(); 236 | /** 237 | * @brief enableDownload Enable writing script 238 | * @return Status of HLD reference [pag. 183] 239 | */ 240 | bool enableDownload(); 241 | }; 242 | 243 | } 244 | 245 | #endif // SERIAL_CONTROLLER_H 246 | -------------------------------------------------------------------------------- /include/roboteq/motor.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (C) 2017, Raffaello Bonghi 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 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * 3. Neither the name of the copyright holder nor the names of its 15 | * contributors may be used to endorse or promote products derived 16 | * from this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND 19 | * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 20 | * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 21 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 22 | * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 23 | * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 24 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 25 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 26 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 28 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #ifndef MOTOR_H 32 | #define MOTOR_H 33 | 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | #include 44 | #include 45 | 46 | #include "roboteq/serial_controller.h" 47 | #include "configurator/gpio_sensor.h" 48 | #include "configurator/motor_param.h" 49 | #include "configurator/motor_pid.h" 50 | 51 | namespace roboteq 52 | { 53 | 54 | typedef struct _motor_status { 55 | uint8_t amps_limit : 1; 56 | uint8_t motor_stalled : 1; 57 | uint8_t loop_error_detect : 1; 58 | uint8_t safety_stop_active : 1; 59 | uint8_t forward_limit_triggered : 1; 60 | uint8_t reverse_limit_triggered : 1; 61 | uint8_t amps_triggered_active : 1; 62 | uint8_t : 1; 63 | } motor_status_t; 64 | 65 | class Motor : public diagnostic_updater::DiagnosticTask 66 | { 67 | public: 68 | /** 69 | * @brief Motor The motor definition and all ros controller initializations are collected in this place 70 | * @param nh The ROS private node handle 71 | * @param serial The serial controller 72 | * @param name The name of the motor 73 | * @param number The number in Roboteq board 74 | */ 75 | explicit Motor(const ros::NodeHandle &nh, serial_controller *serial, string name, unsigned int number); 76 | ~Motor(); 77 | /** 78 | * @brief initializeMotor Initialization oh motor, this routine load parameter from ros server or load from roboteq board 79 | * @param load_from_board forse the load from roboteq board 80 | */ 81 | void initializeMotor(bool load_from_board); 82 | /** 83 | * @brief run Run the diagnostic updater 84 | * @param stat the stat will be updated 85 | */ 86 | void run(diagnostic_updater::DiagnosticStatusWrapper &stat); 87 | /** 88 | * @brief setupLimits setup the maximum velocity, positio and effort 89 | * @param model the robot model 90 | */ 91 | void setupLimits(urdf::Model model); 92 | /** 93 | * @brief resetPosition Reset the motor in a new initial position 94 | * @param position the new position 95 | */ 96 | void resetPosition(double position); 97 | /** 98 | * @brief writeCommandsToHardware Write a command to the hardware interface 99 | * @param period the period update 100 | */ 101 | void writeCommandsToHardware(ros::Duration period); 102 | /** 103 | * @brief switchController Switch the controller from different type of ros controller 104 | * @param type the type of ros controller 105 | */ 106 | void switchController(string type); 107 | /** 108 | * @brief stopMotor Stop the motor 109 | */ 110 | void stopMotor(); 111 | /** 112 | * @brief getNumber The roboteq number 113 | * @return the number associated in the roboteq board 114 | */ 115 | int getNumber() { 116 | return mNumber; 117 | } 118 | 119 | /** 120 | * @brief getName the name of the motor 121 | * @return the string with the name of the motor 122 | */ 123 | string getName() { 124 | return mMotorName; 125 | } 126 | /** 127 | * @brief registerSensor register the sensor 128 | * @param sensor the sensor interface 129 | */ 130 | void registerSensor(GPIOSensor* sensor); 131 | /** 132 | * @brief readVector Decode vector data list 133 | * @param fields field of measures 134 | */ 135 | void readVector(std::vector fields); 136 | 137 | hardware_interface::JointStateHandle joint_state_handle; 138 | hardware_interface::JointHandle joint_handle; 139 | 140 | // Number of motor 141 | unsigned int mNumber; 142 | protected: 143 | /** 144 | * @param x Angular velocity in radians/s. 145 | * @return Angular velocity in RPM. 146 | */ 147 | static double to_rpm(double x) 148 | { 149 | return x * 60 / (2 * M_PI); 150 | } 151 | 152 | /** 153 | * @param x Angular velocity in RPM. 154 | * @return Angular velocity in rad/s. 155 | */ 156 | static double from_rpm(double x) 157 | { 158 | return x * (2 * M_PI) / 60; 159 | } 160 | 161 | /** 162 | * Conversion of radians to encoder ticks. 163 | * 164 | * @param x Angular position in radians. 165 | * @return Angular position in encoder ticks. 166 | */ 167 | double to_encoder_ticks(double x); 168 | 169 | /** 170 | * Conversion of encoder ticks to radians. 171 | * 172 | * @param x Angular position in encoder ticks. 173 | * @return Angular position in radians. 174 | */ 175 | double from_encoder_ticks(double x); 176 | 177 | private: 178 | //Initialization object 179 | //NameSpace for bridge controller 180 | ros::NodeHandle mNh; 181 | // Name of the motor 182 | string mMotorName; 183 | // Serial controller communication 184 | serial_controller *mSerial; 185 | // State of the motor 186 | double position_, max_position_; 187 | double velocity_, max_velocity_; 188 | double effort_, max_effort_; 189 | double command_; 190 | // Motor reduction and ratio 191 | double reduction_; 192 | double ratio_; 193 | // max RPM 194 | double max_rpm_; 195 | 196 | int _control_mode; 197 | motor_status_t status_; 198 | 199 | /// ROS joint limits interface 200 | joint_limits_interface::VelocityJointSoftLimitsInterface vel_limits_interface; 201 | 202 | // Publisher diagnostic information 203 | ros::Publisher pub_status, pub_control; 204 | // Message 205 | roboteq_control::MotorStatus msg_status; 206 | roboteq_control::ControlStatus msg_control; 207 | 208 | MotorParamConfigurator* parameter; 209 | MotorPIDConfigurator* pid_velocity; 210 | MotorPIDConfigurator* pid_torque; 211 | MotorPIDConfigurator* pid_position; 212 | 213 | GPIOSensor* _sensor; 214 | 215 | // Reader motor message 216 | void read(string data); 217 | 218 | void connectionCallback(const ros::SingleSubscriberPublisher& pub); 219 | }; 220 | 221 | } 222 | 223 | #endif // MOTOR_H 224 | -------------------------------------------------------------------------------- /include/roboteq/roboteq.h: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (C) 2017, Raffaello Bonghi 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 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * 3. Neither the name of the copyright holder nor the names of its 15 | * contributors may be used to endorse or promote products derived 16 | * from this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND 19 | * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 20 | * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 21 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 22 | * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 23 | * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 24 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 25 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 26 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 28 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #ifndef ROBOTEQ_H 32 | #define ROBOTEQ_H 33 | 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | #include 40 | 41 | #include 42 | #include 43 | #include 44 | 45 | #include 46 | 47 | #include "configurator/gpio_analog.h" 48 | #include "configurator/gpio_pulse.h" 49 | #include "configurator/gpio_encoder.h" 50 | #include "roboteq/serial_controller.h" 51 | #include "roboteq/motor.h" 52 | 53 | using namespace std; 54 | 55 | namespace roboteq 56 | { 57 | 58 | typedef struct joint 59 | { 60 | Motor *motor; 61 | // State of the motor 62 | double position; 63 | double velocity; 64 | double effort; 65 | double velocity_command; 66 | } joint_t; 67 | 68 | typedef struct _status_flag { 69 | uint8_t serial_mode : 1; 70 | uint8_t pulse_mode : 1; 71 | uint8_t analog_mode : 1; 72 | uint8_t spectrum : 1; 73 | uint8_t power_stage_off : 1; 74 | uint8_t stall_detect : 1; 75 | uint8_t at_limit : 1; 76 | uint8_t microbasic_running : 1; 77 | } status_flag_t; 78 | // Reference in pag 245 79 | typedef struct _status_fault { 80 | uint8_t overheat : 1; 81 | uint8_t overvoltage : 1; 82 | uint8_t undervoltage : 1; 83 | uint8_t short_circuit : 1; 84 | uint8_t emergency_stop : 1; 85 | uint8_t brushless_sensor_fault : 1; 86 | uint8_t mosfet_failure : 1; 87 | } status_fault_t; 88 | 89 | class Roboteq : public hardware_interface::RobotHW, public diagnostic_updater::DiagnosticTask 90 | { 91 | public: 92 | /** 93 | * @brief Roboteq The Roboteq board controller write and read messages about the motor state 94 | * @param nh The ROS public node handle 95 | * @param private_nh the ROS private node handle 96 | * @param serial The serial controller 97 | */ 98 | Roboteq(const ros::NodeHandle &nh, const ros::NodeHandle &private_nh, serial_controller *serial); 99 | /** 100 | * @brief The deconstructor 101 | */ 102 | ~Roboteq(); 103 | /** 104 | * @brief Switch off roboteq board 105 | */ 106 | void switch_off(); 107 | /** 108 | * @brief run Diagnostic thread called every request 109 | * @param stat the status of diagnostic updater 110 | */ 111 | void run(diagnostic_updater::DiagnosticStatusWrapper &stat); 112 | /** 113 | * @brief initialize the roboteq controller 114 | */ 115 | void initialize(); 116 | /** 117 | * @brief initializeInterfaces Initialize all motors. 118 | * Add all Control Interface availbles and add in diagnostic task 119 | */ 120 | void initializeInterfaces(); 121 | /** 122 | * @brief updateDiagnostics 123 | */ 124 | void updateDiagnostics(); 125 | 126 | void initializeDiagnostic(); 127 | 128 | void write(const ros::Time& time, const ros::Duration& period); 129 | 130 | void read(const ros::Time& time, const ros::Duration& period); 131 | 132 | bool prepareSwitch(const std::list& start_list, const std::list& stop_list); 133 | 134 | void doSwitch(const std::list& start_list, const std::list& stop_list); 135 | 136 | private: 137 | //Initialization object 138 | //NameSpace for bridge controller 139 | ros::NodeHandle mNh; 140 | ros::NodeHandle private_mNh; 141 | // Serial controller 142 | serial_controller *mSerial; 143 | // Diagnostic 144 | diagnostic_updater::Updater diagnostic_updater; 145 | // Publisher status periheral 146 | ros::Publisher pub_peripheral; 147 | // stop publisher 148 | ros::Subscriber sub_stop; 149 | // Service board 150 | ros::ServiceServer srv_board; 151 | 152 | /// URDF information about robot 153 | urdf::Model model; 154 | 155 | /// ROS Control interfaces 156 | hardware_interface::JointStateInterface joint_state_interface; 157 | hardware_interface::VelocityJointInterface velocity_joint_interface; 158 | 159 | bool motor_loop_; 160 | // Check if is the first run 161 | bool _first; 162 | // Motor definition 163 | //map mMotor; 164 | std::vector mMotor; 165 | 166 | string _type, _model; 167 | string _version; 168 | string _uid; 169 | // Status Roboteq board 170 | status_flag_t _flag; 171 | // Fault flags Roboteq board 172 | status_fault_t _fault; 173 | // Volts internal 174 | double _volts_internal, _volts_five; 175 | // Tempearture inside the Roboteq board 176 | double _temp_mcu, _temp_bridge; 177 | 178 | // GPIO enable read 179 | bool _isGPIOreading; 180 | roboteq_control::Peripheral msg_peripheral; 181 | std::vector _param_analog; 182 | std::vector _param_pulse; 183 | // Encoder 184 | std::vector _param_encoder; 185 | 186 | 187 | // stop callback 188 | void stop_Callback(const std_msgs::Bool::ConstPtr& msg); 189 | /** 190 | * @brief getRoboteqInformation Load basic information from roboteq board 191 | */ 192 | void getRoboteqInformation(); 193 | 194 | /// Setup variable 195 | bool setup_controller; 196 | 197 | /// Dynamic reconfigure PID 198 | // Dynamic reconfigure 199 | boost::recursive_mutex mDynServerMutex; // To avoid Dynamic Reconfigure Server warning 200 | boost::shared_ptr> mDynRecServer; 201 | /** 202 | * @brief reconfigureCBEncoder when the dynamic reconfigurator change some values start this method 203 | * @param config variable with all configuration from dynamic reconfigurator 204 | * @param level 205 | */ 206 | void reconfigureCBController(roboteq_control::RoboteqControllerConfig &config, uint32_t level); 207 | 208 | // Default parameter config 209 | roboteq_control::RoboteqControllerConfig default_controller_config, _last_controller_config; 210 | 211 | /** 212 | * @brief getPIDFromRoboteq Load PID parameters from Roboteq board 213 | */ 214 | void getControllerFromRoboteq(); 215 | 216 | /** 217 | * @brief service_Callback Internal service to require information from the board connected 218 | * @param req 219 | * @param msg 220 | * @return 221 | */ 222 | bool service_Callback(roboteq_control::Service::Request &req, roboteq_control::Service::Response &msg_system); 223 | /** 224 | * @brief connectionCallback Check how many subscribers are connected 225 | * @param pub The information about the subscriber 226 | */ 227 | void connectionCallback(const ros::SingleSubscriberPublisher& pub); 228 | 229 | }; 230 | 231 | } 232 | 233 | 234 | #endif // ROBOTEQ_H 235 | -------------------------------------------------------------------------------- /src/configurator/gpio_analog.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (C) 2017, Raffaello Bonghi 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 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * 3. Neither the name of the copyright holder nor the names of its 15 | * contributors may be used to endorse or promote products derived 16 | * from this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND 19 | * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 20 | * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 21 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 22 | * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 23 | * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 24 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 25 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 26 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 28 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #include "configurator/gpio_analog.h" 32 | 33 | #define PARAM_ANALOG_STRING "/analog" 34 | 35 | GPIOAnalogConfigurator::GPIOAnalogConfigurator(const ros::NodeHandle &nh, roboteq::serial_controller *serial, std::vector motor, string name, unsigned int number) 36 | : nh_(nh) 37 | , mSerial(serial) 38 | ,_motor(motor) 39 | { 40 | // Find path param 41 | mName = nh_.getNamespace() + name + PARAM_ANALOG_STRING + "/" + std::to_string(number); 42 | // Roboteq motor number 43 | mNumber = number; 44 | // Set false on first run 45 | setup_param = false; 46 | } 47 | 48 | void GPIOAnalogConfigurator::initConfigurator(bool load_from_board) 49 | { 50 | // Check if is required load paramers 51 | if(load_from_board) 52 | { 53 | // Load parameters from roboteq 54 | getParamFromRoboteq(); 55 | } 56 | // Initialize encoder dynamic reconfigure 57 | mDynRecServer = std::make_shared(mDynServerMutex, ros::NodeHandle(mName)); 58 | // Load default configuration 59 | roboteq_control::RoboteqAnalogInputConfig config; 60 | mDynRecServer->getConfigDefault(config); 61 | // Update parameters 62 | mDynServerMutex.lock(); 63 | mDynRecServer->updateConfig(config); 64 | mDynServerMutex.unlock(); 65 | // Set callback 66 | mDynRecServer->setCallback(boost::bind(&GPIOAnalogConfigurator::reconfigureCBParam, this, _1, _2)); 67 | } 68 | 69 | void GPIOAnalogConfigurator::getParamFromRoboteq() 70 | { 71 | try 72 | { 73 | // conversion AMOD 74 | string str_conversion = mSerial->getParam("AMOD", std::to_string(mNumber)); 75 | int conversion = boost::lexical_cast(str_conversion); 76 | // Set params 77 | nh_.setParam(mName + "/conversion", conversion); 78 | 79 | // input AINA 80 | string str_pina = mSerial->getParam("AINA", std::to_string(mNumber)); 81 | // Get AINA from roboteq board 82 | int emod = boost::lexical_cast(str_pina); 83 | // 3 modes: 84 | // 0 - Unsed 85 | // 1 - Command 86 | // 2 - Feedback 87 | int command = (emod & 0b11); 88 | int motors = (emod - command) >> 4; 89 | int tmp1 = ((motors & 0b1) > 0); 90 | int tmp2 = ((motors & 0b10) > 0); 91 | if(tmp1) 92 | { 93 | for (vector::iterator it = _motor.begin() ; it != _motor.end(); ++it) 94 | { 95 | roboteq::Motor* motor = ((roboteq::Motor*)(*it)); 96 | if(motor->getNumber() == 1) 97 | { 98 | motor->registerSensor(this); 99 | ROS_INFO_STREAM("Register analog [" << mNumber << "] to: " << motor->getName()); 100 | break; 101 | } 102 | } 103 | } 104 | if(tmp2) 105 | { 106 | for (vector::iterator it = _motor.begin() ; it != _motor.end(); ++it) 107 | { 108 | roboteq::Motor* motor = ((roboteq::Motor*)(*it)); 109 | if(motor->getNumber() == 2) 110 | { 111 | motor->registerSensor(this); 112 | ROS_INFO_STREAM("Register analog [" << mNumber << "] to: " << motor->getName()); 113 | break; 114 | } 115 | } 116 | } 117 | 118 | // Set parameter 119 | nh_.setParam(mName + "/input_use", command); 120 | nh_.setParam(mName + "/input_motor_one", tmp1); 121 | nh_.setParam(mName + "/input_motor_two", tmp2); 122 | 123 | // polarity APOL 124 | string str_polarity = mSerial->getParam("APOL", std::to_string(mNumber)); 125 | int polarity = boost::lexical_cast(str_polarity); 126 | // Set params 127 | nh_.setParam(mName + "/conversion_polarity", polarity); 128 | 129 | // Input deadband ADB 130 | string str_deadband = mSerial->getParam("ADB", std::to_string(mNumber)); 131 | int deadband = boost::lexical_cast(str_deadband); 132 | // Set params 133 | nh_.setParam(mName + "/input_deadband", deadband); 134 | 135 | // Input AMIN 136 | string str_min = mSerial->getParam("AMIN", std::to_string(mNumber)); 137 | double min = boost::lexical_cast(str_min) / 1000; 138 | // Set params 139 | nh_.setParam(mName + "/range_input_min", min); 140 | 141 | // Input AMAX 142 | string str_max = mSerial->getParam("AMAX", std::to_string(mNumber)); 143 | double max = boost::lexical_cast(str_max) / 1000; 144 | // Set params 145 | nh_.setParam(mName + "/range_input_max", max); 146 | 147 | // Input ACTR 148 | string str_ctr = mSerial->getParam("ACTR", std::to_string(mNumber)); 149 | double ctr = boost::lexical_cast(str_ctr) / 1000; 150 | // Set params 151 | nh_.setParam(mName + "/range_input_center", ctr); 152 | 153 | } catch (std::bad_cast& e) 154 | { 155 | ROS_WARN_STREAM("Failure parsing feedback data. Dropping message." << e.what()); 156 | } 157 | } 158 | 159 | void GPIOAnalogConfigurator::reconfigureCBParam(roboteq_control::RoboteqAnalogInputConfig &config, uint32_t level) 160 | { 161 | //The first time we're called, we just want to make sure we have the 162 | //original configuration 163 | if(!setup_param) 164 | { 165 | _last_param_config = config; 166 | default_param_config = _last_param_config; 167 | setup_param = true; 168 | return; 169 | } 170 | 171 | if(config.restore_defaults) 172 | { 173 | //if someone sets restore defaults on the parameter server, prevent looping 174 | config.restore_defaults = false; 175 | // Overload config with default 176 | config = default_param_config; 177 | } 178 | 179 | if(config.load_roboteq) 180 | { 181 | //if someone sets again the request on the parameter server, prevent looping 182 | config.load_roboteq = false; 183 | // Launch param load 184 | getParamFromRoboteq(); 185 | // Skip other read 186 | return; 187 | } 188 | 189 | // Set conversion AMOD 190 | if(_last_param_config.conversion != config.conversion) 191 | { 192 | // Update operative mode 193 | mSerial->setParam("AMOD", std::to_string(mNumber) + " " + std::to_string(config.conversion)); 194 | } 195 | // Set input AINA 196 | if((_last_param_config.input_use != config.input_use) || 197 | (_last_param_config.input_motor_one != config.input_motor_one) || 198 | (_last_param_config.input_motor_two != config.input_motor_two)) 199 | { 200 | int input = config.input_use + 16*config.input_motor_one + 32*config.input_motor_two; 201 | mSerial->setParam("AINA", std::to_string(mNumber) + " " + std::to_string(input)); 202 | 203 | if(config.input_motor_one) 204 | { 205 | roboteq::Motor* motor = _motor.at(0); 206 | motor->registerSensor(this); 207 | ROS_INFO_STREAM("Register analog [" << mNumber << "] to: " << motor->getName()); 208 | } 209 | if(config.input_motor_two) 210 | { 211 | roboteq::Motor* motor = _motor.at(1); 212 | motor->registerSensor(this); 213 | ROS_INFO_STREAM("Register analog [" << mNumber << "] to: " << motor->getName()); 214 | } 215 | } 216 | // Set polarity APOL 217 | if(_last_param_config.conversion_polarity != config.conversion_polarity) 218 | { 219 | // Update operative mode 220 | mSerial->setParam("APOL", std::to_string(mNumber) + " " + std::to_string(config.conversion_polarity)); 221 | } 222 | // Set deadband ADB 223 | if(_last_param_config.input_deadband != config.input_deadband) 224 | { 225 | // Update operative mode 226 | mSerial->setParam("ADB", std::to_string(mNumber) + " " + std::to_string(config.input_deadband)); 227 | } 228 | // Set input AMIN 229 | if(_last_param_config.range_input_min != config.range_input_min) 230 | { 231 | int range_input_min = config.range_input_min * 1000; 232 | // Update operative mode 233 | mSerial->setParam("AMIN", std::to_string(mNumber) + " " + std::to_string(range_input_min)); 234 | } 235 | // Set input AMAX 236 | if(_last_param_config.range_input_max != config.range_input_max) 237 | { 238 | int range_input_max = config.range_input_max * 1000; 239 | // Update operative mode 240 | mSerial->setParam("AMAX", std::to_string(mNumber) + " " + std::to_string(range_input_max)); 241 | } 242 | // Set input ACTR 243 | if(_last_param_config.range_input_center != config.range_input_center) 244 | { 245 | int range_input_center = config.range_input_center * 1000; 246 | // Update operative mode 247 | mSerial->setParam("ACTR", std::to_string(mNumber) + " " + std::to_string(range_input_center)); 248 | } 249 | } 250 | -------------------------------------------------------------------------------- /src/configurator/gpio_pulse.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (C) 2017, Raffaello Bonghi 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 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * 3. Neither the name of the copyright holder nor the names of its 15 | * contributors may be used to endorse or promote products derived 16 | * from this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND 19 | * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 20 | * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 21 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 22 | * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 23 | * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 24 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 25 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 26 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 28 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #include "configurator/gpio_pulse.h" 32 | 33 | #define PARAM_PULSE_STRING "/pulse" 34 | 35 | GPIOPulseConfigurator::GPIOPulseConfigurator(const ros::NodeHandle &nh, roboteq::serial_controller *serial, std::vector motor, string name, unsigned int number) 36 | : nh_(nh) 37 | , mSerial(serial) 38 | ,_motor(motor) 39 | { 40 | // Find path param 41 | mName = nh_.getNamespace() + name + PARAM_PULSE_STRING + "/" + std::to_string(number); 42 | // Roboteq motor number 43 | mNumber = number; 44 | // Set false on first run 45 | setup_param = false; 46 | } 47 | 48 | void GPIOPulseConfigurator::initConfigurator(bool load_from_board) 49 | { 50 | // Check if is required load paramers 51 | if(load_from_board) 52 | { 53 | // Load parameters from roboteq 54 | getParamFromRoboteq(); 55 | } 56 | // Initialize parameter dynamic reconfigure 57 | mDynRecServer = std::make_shared(mDynServerMutex, ros::NodeHandle(mName)); 58 | // Load default configuration 59 | roboteq_control::RoboteqPulseInputConfig config; 60 | mDynRecServer->getConfigDefault(config); 61 | // Update parameters 62 | mDynServerMutex.lock(); 63 | mDynRecServer->updateConfig(config); 64 | mDynServerMutex.unlock(); 65 | // Set callback 66 | mDynRecServer->setCallback(boost::bind(&GPIOPulseConfigurator::reconfigureCBParam, this, _1, _2)); 67 | } 68 | 69 | void GPIOPulseConfigurator::getParamFromRoboteq() 70 | { 71 | try 72 | { 73 | // conversion PMOD 74 | string str_conversion = mSerial->getParam("PMOD", std::to_string(mNumber)); 75 | int conversion = boost::lexical_cast(str_conversion); 76 | // Set params 77 | nh_.setParam(mName + "/conversion", conversion); 78 | 79 | // input PINA 80 | string str_pina = mSerial->getParam("PINA", std::to_string(mNumber)); 81 | // Get PINA from roboteq board 82 | int emod = boost::lexical_cast(str_pina); 83 | // 3 modes: 84 | // 0 - Unsed 85 | // 1 - Command 86 | // 2 - Feedback 87 | int command = (emod & 0b11); 88 | int motors = (emod - command) >> 4; 89 | int tmp1 = ((motors & 0b1) > 0); 90 | int tmp2 = ((motors & 0b10) > 0); 91 | if(tmp1) 92 | { 93 | for (vector::iterator it = _motor.begin() ; it != _motor.end(); ++it) 94 | { 95 | roboteq::Motor* motor = ((roboteq::Motor*)(*it)); 96 | if(motor->getNumber() == 1) 97 | { 98 | motor->registerSensor(this); 99 | ROS_INFO_STREAM("Register pulse input [" << mNumber << "] to: " << motor->getName()); 100 | break; 101 | } 102 | } 103 | } 104 | if(tmp2) 105 | { 106 | for (vector::iterator it = _motor.begin() ; it != _motor.end(); ++it) 107 | { 108 | roboteq::Motor* motor = ((roboteq::Motor*)(*it)); 109 | if(motor->getNumber() == 2) 110 | { 111 | motor->registerSensor(this); 112 | ROS_INFO_STREAM("Register pulse input [" << mNumber << "] to: " << motor->getName()); 113 | break; 114 | } 115 | } 116 | } 117 | 118 | // Set parameter 119 | nh_.setParam(mName + "/input_use", command); 120 | nh_.setParam(mName + "/input_motor_one", tmp1); 121 | nh_.setParam(mName + "/input_motor_two", tmp2); 122 | 123 | // polarity PPOL 124 | string str_polarity = mSerial->getParam("PPOL", std::to_string(mNumber)); 125 | int polarity = boost::lexical_cast(str_polarity); 126 | // Set params 127 | nh_.setParam(mName + "/conversion_polarity", polarity); 128 | 129 | // Input deadband PDB 130 | string str_deadband = mSerial->getParam("PDB", std::to_string(mNumber)); 131 | int deadband = boost::lexical_cast(str_deadband); 132 | // Set params 133 | nh_.setParam(mName + "/input_deadband", deadband); 134 | 135 | // Input PMIN 136 | string str_min = mSerial->getParam("PMIN", std::to_string(mNumber)); 137 | double min = boost::lexical_cast(str_min) / 1000; 138 | // Set params 139 | nh_.setParam(mName + "/range_input_min", min); 140 | 141 | // Input PMAX 142 | string str_max = mSerial->getParam("PMAX", std::to_string(mNumber)); 143 | double max = boost::lexical_cast(str_max) / 1000; 144 | // Set params 145 | nh_.setParam(mName + "/range_input_max", max); 146 | 147 | // Input PTCR 148 | string str_ctr = mSerial->getParam("PCTR", std::to_string(mNumber)); 149 | double ctr = boost::lexical_cast(str_ctr) / 1000; 150 | // Set params 151 | nh_.setParam(mName + "/range_input_center", ctr); 152 | 153 | } catch (std::bad_cast& e) 154 | { 155 | ROS_WARN_STREAM("Failure parsing feedback data. Dropping message." << e.what()); 156 | } 157 | } 158 | 159 | void GPIOPulseConfigurator::reconfigureCBParam(roboteq_control::RoboteqPulseInputConfig &config, uint32_t level) 160 | { 161 | //The first time we're called, we just want to make sure we have the 162 | //original configuration 163 | if(!setup_param) 164 | { 165 | _last_param_config = config; 166 | default_param_config = _last_param_config; 167 | setup_param = true; 168 | return; 169 | } 170 | 171 | if(config.restore_defaults) 172 | { 173 | //if someone sets restore defaults on the parameter server, prevent looping 174 | config.restore_defaults = false; 175 | // Overload config with default 176 | config = default_param_config; 177 | } 178 | 179 | if(config.load_roboteq) 180 | { 181 | //if someone sets again the request on the parameter server, prevent looping 182 | config.load_roboteq = false; 183 | // Launch param load 184 | getParamFromRoboteq(); 185 | // Skip other read 186 | return; 187 | } 188 | 189 | // Set conversion PMOD 190 | if(_last_param_config.conversion != config.conversion) 191 | { 192 | // Update operative mode 193 | mSerial->setParam("PMOD", std::to_string(mNumber) + " " + std::to_string(config.conversion)); 194 | } 195 | // Set input PINA 196 | if((_last_param_config.input_use != config.input_use) || 197 | (_last_param_config.input_motor_one != config.input_motor_one) || 198 | (_last_param_config.input_motor_two != config.input_motor_two)) 199 | { 200 | int input = config.input_use + 16*config.input_motor_one + 32*config.input_motor_two; 201 | mSerial->setParam("PINA", std::to_string(mNumber) + " " + std::to_string(input)); 202 | 203 | if(config.input_motor_one) 204 | { 205 | roboteq::Motor* motor = _motor.at(0); 206 | motor->registerSensor(this); 207 | ROS_INFO_STREAM("Register pulse input [" << mNumber << "] to: " << motor->getName()); 208 | } 209 | if(config.input_motor_two) 210 | { 211 | roboteq::Motor* motor = _motor.at(1); 212 | motor->registerSensor(this); 213 | ROS_INFO_STREAM("Register pulse input [" << mNumber << "] to: " << motor->getName()); 214 | } 215 | } 216 | // Set polarity PPOL 217 | if(_last_param_config.conversion_polarity != config.conversion_polarity) 218 | { 219 | // Update operative mode 220 | mSerial->setParam("PPOL", std::to_string(mNumber) + " " + std::to_string(config.conversion_polarity)); 221 | } 222 | // Set deadband PDB 223 | if(_last_param_config.input_deadband != config.input_deadband) 224 | { 225 | // Update operative mode 226 | mSerial->setParam("PDB", std::to_string(mNumber) + " " + std::to_string(config.input_deadband)); 227 | } 228 | // Set input PMIN 229 | if(_last_param_config.range_input_min != config.range_input_min) 230 | { 231 | int range_input_min = config.range_input_min * 1000; 232 | // Update operative mode 233 | mSerial->setParam("PMIN", std::to_string(mNumber) + " " + std::to_string(range_input_min)); 234 | } 235 | // Set input PMAX 236 | if(_last_param_config.range_input_max != config.range_input_max) 237 | { 238 | int range_input_max = config.range_input_max * 1000; 239 | // Update operative mode 240 | mSerial->setParam("PMAX", std::to_string(mNumber) + " " + std::to_string(range_input_max)); 241 | } 242 | // Set input PTCR 243 | if(_last_param_config.range_input_center != config.range_input_center) 244 | { 245 | int range_input_center = config.range_input_center * 1000; 246 | // Update operative mode 247 | mSerial->setParam("PCTR", std::to_string(mNumber) + " " + std::to_string(range_input_center)); 248 | } 249 | } 250 | -------------------------------------------------------------------------------- /src/configurator/gpio_encoder.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (C) 2017, Raffaello Bonghi 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 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * 3. Neither the name of the copyright holder nor the names of its 15 | * contributors may be used to endorse or promote products derived 16 | * from this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND 19 | * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 20 | * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 21 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 22 | * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 23 | * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 24 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 25 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 26 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 28 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #include "configurator/gpio_encoder.h" 32 | 33 | #define PARAM_ENCODER_STRING "/encoder" 34 | 35 | GPIOEncoderConfigurator::GPIOEncoderConfigurator(const ros::NodeHandle &nh, roboteq::serial_controller *serial, std::vector motor, string name, unsigned int number) 36 | : nh_(nh) 37 | , mSerial(serial) 38 | ,_motor(motor) 39 | { 40 | // Find path param 41 | mName = nh_.getNamespace() + name + PARAM_ENCODER_STRING + "/" + std::to_string(number); 42 | // Roboteq motor number 43 | mNumber = number; 44 | // Set false on first run 45 | setup_encoder = false; 46 | } 47 | 48 | void GPIOEncoderConfigurator::initConfigurator(bool load_from_board) 49 | { 50 | // Get PPR Encoder parameter 51 | double ppr; 52 | nh_.getParam(mName + "/PPR", ppr); 53 | _reduction = ppr; 54 | // Multiply for quadrature 55 | _reduction *= 4; 56 | 57 | // Check if is required load paramers 58 | if(load_from_board) 59 | { 60 | // Load encoder properties from roboteq 61 | getEncoderFromRoboteq(); 62 | } 63 | 64 | // Initialize encoder dynamic reconfigure 65 | mDynRecServer = std::make_shared(mDynServerMutex, ros::NodeHandle(mName)); 66 | // Load default configuration 67 | roboteq_control::RoboteqEncoderConfig config; 68 | mDynRecServer->getConfigDefault(config); 69 | // Update parameters 70 | mDynServerMutex.lock(); 71 | mDynRecServer->updateConfig(config); 72 | mDynServerMutex.unlock(); 73 | // Set callback 74 | mDynRecServer->setCallback(boost::bind(&GPIOEncoderConfigurator::reconfigureCBEncoder, this, _1, _2)); 75 | } 76 | 77 | double GPIOEncoderConfigurator::getConversion(double reduction) { 78 | // Check if exist ratio variable 79 | if(nh_.hasParam(mName + "/position")) 80 | { 81 | int position; 82 | nh_.getParam(mName + "/position", position); 83 | // Read position if before (1) multiply with ratio 84 | if(position) { 85 | return _reduction * reduction; 86 | } 87 | } 88 | return _reduction; 89 | } 90 | 91 | void GPIOEncoderConfigurator::getEncoderFromRoboteq() { 92 | try 93 | { 94 | // Get Encoder Usage - reference 95 | string str_emode = mSerial->getParam("EMOD", std::to_string(mNumber)); 96 | // Get PPR from roboteq board 97 | int emod = boost::lexical_cast(str_emode); 98 | // 3 modes: 99 | // 0 - Unsed 100 | // 1 - Command 101 | // 2 - Feedback 102 | int command = (emod & 0b11); 103 | int motors = (emod - command) >> 4; 104 | int tmp1 = ((motors & 0b1) > 0); 105 | int tmp2 = ((motors & 0b10) > 0); 106 | // Register reduction 107 | if(tmp1) 108 | { 109 | for (vector::iterator it = _motor.begin() ; it != _motor.end(); ++it) 110 | { 111 | roboteq::Motor* motor = ((roboteq::Motor*)(*it)); 112 | if(motor->getNumber() == 1) 113 | { 114 | motor->registerSensor(this); 115 | ROS_INFO_STREAM("Register encoder [" << mNumber << "] to: " << motor->getName()); 116 | break; 117 | } 118 | } 119 | } 120 | if(tmp2) 121 | { 122 | for (vector::iterator it = _motor.begin() ; it != _motor.end(); ++it) 123 | { 124 | roboteq::Motor* motor = ((roboteq::Motor*)(*it)); 125 | if(motor->getNumber() == 2) 126 | { 127 | motor->registerSensor(this); 128 | ROS_INFO_STREAM("Register encoder [" << mNumber << "] to: " << motor->getName()); 129 | break; 130 | } 131 | } 132 | } 133 | // Set parameter 134 | nh_.setParam(mName + "/configuration", command); 135 | nh_.setParam(mName + "/input_motor_one", tmp1); 136 | nh_.setParam(mName + "/input_motor_two", tmp2); 137 | 138 | // Get Encoder PPR (Pulse/rev) 139 | string str_ppr = mSerial->getParam("EPPR", std::to_string(mNumber)); 140 | // Get PPR from roboteq board 141 | int ppr = boost::lexical_cast(str_ppr); 142 | // Set parameter 143 | nh_.setParam(mName + "/PPR", ppr); 144 | 145 | // Get Encoder ELL - Min limit 146 | string str_ell = mSerial->getParam("ELL", std::to_string(mNumber)); 147 | // Get PPR from roboteq board 148 | int ell = boost::lexical_cast(str_ell); 149 | // Set parameter 150 | nh_.setParam(mName + "/encoder_low_count_limit", ell); 151 | 152 | // Get Encoder EHL - Max limit 153 | string str_ehl = mSerial->getParam("EHL", std::to_string(mNumber)); 154 | // Get PPR from roboteq board 155 | int ehl = boost::lexical_cast(str_ehl); 156 | // Set parameter 157 | nh_.setParam(mName + "/encoder_high_count_limit", ehl); 158 | 159 | // Get Encoder EHOME - Home count 160 | string str_home = mSerial->getParam("EHOME", std::to_string(mNumber)); 161 | // Get PPR from roboteq board 162 | int home = boost::lexical_cast(str_home); 163 | // Set parameter 164 | nh_.setParam(mName + "/encoder_home_count", home); 165 | 166 | 167 | } catch (std::bad_cast& e) 168 | { 169 | ROS_WARN_STREAM("Failure parsing feedback data. Dropping message." << e.what()); 170 | } 171 | } 172 | 173 | void GPIOEncoderConfigurator::reconfigureCBEncoder(roboteq_control::RoboteqEncoderConfig &config, uint32_t level) { 174 | 175 | //The first time we're called, we just want to make sure we have the 176 | //original configuration 177 | if(!setup_encoder) 178 | { 179 | _last_encoder_config = config; 180 | default_encoder_config = _last_encoder_config; 181 | setup_encoder = true; 182 | return; 183 | } 184 | 185 | if(config.restore_defaults) 186 | { 187 | //if someone sets restore defaults on the parameter server, prevent looping 188 | config.restore_defaults = false; 189 | // Overload default configuration 190 | config = default_encoder_config; 191 | } 192 | 193 | if(config.load_roboteq) 194 | { 195 | ROS_INFO_STREAM("LOAD from Roboteq"); 196 | //if someone sets again the request on the parameter server, prevent looping 197 | config.load_roboteq = false; 198 | // Launch encoder load 199 | getEncoderFromRoboteq(); 200 | // Skip other read 201 | return; 202 | } 203 | 204 | // Set Encoder Usage - reference pag. 307 205 | if((_last_encoder_config.configuration != config.configuration) || 206 | (_last_encoder_config.input_motor_one != config.input_motor_one) || 207 | (_last_encoder_config.input_motor_two != config.input_motor_two)) 208 | { 209 | int configuration = config.configuration + 16*config.input_motor_one + 32*config.input_motor_two; 210 | // Update operative mode 211 | mSerial->setParam("EMOD", std::to_string(mNumber) + " " + std::to_string(configuration)); 212 | 213 | if(config.input_motor_one) 214 | { 215 | roboteq::Motor* motor = _motor.at(0); 216 | motor->registerSensor(this); 217 | ROS_INFO_STREAM("Register encoder [" << mNumber << "] to: " << motor->getName()); 218 | } 219 | if(config.input_motor_two) 220 | { 221 | roboteq::Motor* motor = _motor.at(1); 222 | motor->registerSensor(this); 223 | ROS_INFO_STREAM("Register encoder [" << mNumber << "] to: " << motor->getName()); 224 | } 225 | } 226 | // Set Encoder PPR [pag. 308] 227 | if(_last_encoder_config.PPR != config.PPR) 228 | { 229 | // Update reduction value 230 | _reduction = config.PPR; 231 | // Update operative mode 232 | mSerial->setParam("EPPR", std::to_string(mNumber) + " " + std::to_string(config.PPR)); 233 | } 234 | // Set Encoder ELL - Min limit [pag. 306] 235 | if(_last_encoder_config.encoder_low_count_limit != config.encoder_low_count_limit) 236 | { 237 | // Update operative mode 238 | mSerial->setParam("ELL", std::to_string(mNumber) + " " + std::to_string(config.encoder_low_count_limit)); 239 | } 240 | // Set Encoder EHL - Max limit [pag. 304] 241 | if(_last_encoder_config.encoder_high_count_limit != config.encoder_high_count_limit) 242 | { 243 | // Update operative mode 244 | mSerial->setParam("EHL", std::to_string(mNumber) + " " + std::to_string(config.encoder_high_count_limit)); 245 | } 246 | // Set Encoder EHOME - Home count [pag. 306] 247 | if(_last_encoder_config.encoder_home_count != config.encoder_home_count) 248 | { 249 | // Update operative mode 250 | mSerial->setParam("EHOME", std::to_string(mNumber) + " " + std::to_string(config.encoder_home_count)); 251 | } 252 | 253 | // Update last configuration 254 | _last_encoder_config = config; 255 | 256 | } 257 | -------------------------------------------------------------------------------- /src/roboteq/serial_controller.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (C) 2017, Raffaello Bonghi 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 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * 3. Neither the name of the copyright holder nor the names of its 15 | * contributors may be used to endorse or promote products derived 16 | * from this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND 19 | * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 20 | * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 21 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 22 | * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 23 | * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 24 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 25 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 26 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 28 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #include "roboteq/serial_controller.h" 32 | 33 | #include 34 | 35 | namespace roboteq { 36 | 37 | const std::string eol("\r"); 38 | const size_t max_line_length(128); 39 | const std::regex rgx_query("(.+)=(.+)\r"); 40 | const std::regex rgx_cmd("(\\+|-)\r"); 41 | 42 | serial_controller::serial_controller(string port, unsigned long baudrate) 43 | : mSerialPort(port) 44 | , mBaudrate(baudrate) 45 | { 46 | // Default timeout 47 | mTimeout = 500; 48 | } 49 | 50 | serial_controller::~serial_controller() 51 | { 52 | stop(); 53 | } 54 | 55 | bool serial_controller::start() 56 | { 57 | try 58 | { 59 | mSerial.setPort(mSerialPort); 60 | mSerial.open(); 61 | mSerial.setBaudrate(mBaudrate); 62 | 63 | serial::Timeout to = serial::Timeout::simpleTimeout(mTimeout); 64 | mSerial.setTimeout(to); 65 | } 66 | catch (serial::IOException& e) 67 | { 68 | ROS_ERROR_STREAM("Unable to open serial port " << mSerialPort << " - Error: " << e.what() ); 69 | return false; 70 | } 71 | 72 | if(mSerial.isOpen()){ 73 | ROS_DEBUG_STREAM("Serial Port correctly initialized: " << mSerialPort ); 74 | } 75 | else 76 | { 77 | ROS_ERROR_STREAM( "Serial port not opened: " << mSerialPort ); 78 | return false; 79 | } 80 | // Initialize stop function 81 | mStopping = false; 82 | // Launch async reader thread 83 | first = std::thread(&serial_controller::async_reader, this); 84 | ROS_DEBUG_STREAM( "Serial port ready" ); 85 | 86 | /* 87 | // Launch script and check version 88 | script(true); 89 | if(query("VAR", "1")) 90 | { 91 | _script_ver = get(); 92 | // Stop script 93 | script(false); 94 | 95 | if(_script_ver.compare(script_ver) != 0) 96 | { 97 | ROS_WARN_STREAM("Script version mismatch. Updating V" << _script_ver << "->V"<< script_ver << " ..."); 98 | if(downloadScript()) 99 | { 100 | ROS_WARN_STREAM("...Done!"); 101 | } 102 | else 103 | { 104 | ROS_ERROR_STREAM("...ERROR to download the script!"); 105 | return false; 106 | } 107 | } else 108 | { 109 | ROS_DEBUG_STREAM("Script V" << _script_ver); 110 | } 111 | } 112 | */ 113 | return true; 114 | } 115 | 116 | bool serial_controller::stop() 117 | { 118 | // Stop script 119 | script(false); 120 | // Stop the reader 121 | mStopping = true; 122 | // Close the serial port 123 | mSerial.close(); 124 | // Wait stop thread 125 | first.join(); 126 | 127 | } 128 | 129 | bool serial_controller::addCallback(const callback_data_t &callback, const string data) 130 | { 131 | if (hashmap.find(data) != hashmap.end()) 132 | { 133 | return false; 134 | } else 135 | { 136 | hashmap[data] = callback; 137 | return true; 138 | } 139 | } 140 | 141 | bool serial_controller::enableDownload() 142 | { 143 | mWriteMutex.lock(); 144 | // Send SLD. 145 | ROS_INFO("Commanding driver to enter download mode."); 146 | // Set fals HLD mode 147 | isHLD = false; 148 | // Send enable write mode 149 | mSerial.write("%SLD 321654987" + eol); 150 | // Set lock variable and wait a data to return 151 | std::unique_lock lck(mReaderMutex); 152 | // TODO change timeout 153 | cv.wait_for(lck, std::chrono::seconds(1)); 154 | // Look for special ack from SLD. 155 | // ROS_INFO_STREAM("HLD=" << isHLD); 156 | // Unlock mutex 157 | mWriteMutex.unlock(); 158 | // If not received return false 159 | if(!isHLD) 160 | return false; 161 | } 162 | 163 | bool serial_controller::downloadScript() 164 | { 165 | if(enableDownload()) 166 | { 167 | ROS_DEBUG("Writing script..."); 168 | // Launch write script 169 | int line_num = 0; 170 | /* 171 | while(script_lines[line_num]) { 172 | // Build string 173 | std::string line(script_lines[line_num]); 174 | ROS_DEBUG_STREAM("write[" << line_num << "]" << line); 175 | bool cmd = command(line, "", ""); 176 | // ROS_INFO_STREAM("cmd=" << (cmd ? "true" : "false")); 177 | // Check data and received a "+" 178 | if(!cmd) 179 | return false; 180 | // Go to second line 181 | line_num++; 182 | } 183 | */ 184 | ROS_DEBUG("...complete!"); 185 | return true; 186 | } 187 | return false; 188 | } 189 | 190 | bool serial_controller::command(string msg, string params, string type) 191 | { 192 | // Lock the write mutex 193 | mWriteMutex.lock(); 194 | // Build the string 195 | string msg2; 196 | if(params.compare("") == 0) { 197 | msg2 = type + msg + eol; 198 | } else { 199 | msg2 = type + msg + " " + params + eol; 200 | } 201 | unsigned int counter = 0; 202 | while (counter < 5) 203 | { 204 | mSerial.write(msg2.c_str()); 205 | data = false; 206 | // Set lock variable and wait a data to return 207 | std::unique_lock lck(mReaderMutex); 208 | // TODO change timeout 209 | cv.wait_for(lck, std::chrono::seconds(1)); 210 | if(data) 211 | { 212 | ROS_DEBUG_STREAM("N:" << (counter+1) << " CMD:" << msg << " DATA:" << sub_data_cmd); 213 | break; 214 | } else 215 | { 216 | // Increase counter 217 | counter++; 218 | } 219 | } 220 | // Unlock mutex 221 | mWriteMutex.unlock(); 222 | return sub_data_cmd; 223 | } 224 | 225 | bool serial_controller::query(string msg, string params, string type) { 226 | mWriteMutex.lock(); 227 | mMessage = msg; 228 | string msg2; 229 | if(params.compare("") == 0) { 230 | msg2 = type + msg + eol; 231 | } else { 232 | msg2 = type + msg + " " + params + eol; 233 | } 234 | 235 | unsigned int counter = 0; 236 | while (counter < 5) 237 | { 238 | ROS_DEBUG_STREAM("N:" << (counter+1) << " TX: " << msg); 239 | mSerial.write(msg2.c_str()); 240 | data = false; 241 | // Set lock variable and wait a data to return 242 | std::unique_lock lck(mReaderMutex); 243 | cv.wait_for(lck, std::chrono::seconds(1)); 244 | if(data) 245 | { 246 | ROS_DEBUG_STREAM("N:" << (counter+1) << " CMD:" << msg << " DATA:" << sub_data); 247 | break; 248 | } else 249 | { 250 | // Increase counter 251 | counter++; 252 | } 253 | } 254 | // Clear last query request 255 | mMessage = ""; 256 | // Unlock mutex 257 | mWriteMutex.unlock(); 258 | return data; 259 | } 260 | 261 | void serial_controller::async_reader() 262 | { 263 | while (!mStopping) { 264 | // Read how many byte waiting to read 265 | ROS_DEBUG_STREAM_NAMED("serial", "Bytes waiting: " << mSerial.available()); 266 | // Read line 267 | std::string msg = mSerial.readline(max_line_length, eol); 268 | // Decode message 269 | if (!msg.empty()) 270 | { 271 | ROS_DEBUG_STREAM_NAMED("serial", "RX: " << msg); 272 | if (std::regex_match(msg, rgx_cmd)) 273 | { 274 | // Decode if command return true 275 | if(msg[0] == '+') sub_data_cmd = true; 276 | else sub_data_cmd = false; 277 | // Unlock command 278 | data = true; 279 | // Unlock query request 280 | cv.notify_all(); 281 | } 282 | else if(std::regex_match(msg, rgx_query)) 283 | { 284 | // Get command 285 | string sub_cmd = msg.substr(0, msg.find('=')); 286 | // Evaluate end position string 287 | long end_string = (msg.size()-1) - (msg.find('=') + 1); 288 | // Get data 289 | sub_data = msg.substr(msg.find('=') + 1, end_string); 290 | // ROS_INFO_STREAM("CMD=" << sub_cmd << " DATA=" << sub_data); 291 | // Check first of all a message sent require a data to return 292 | if(mMessage.compare("") != 0) { 293 | if(mMessage.compare(sub_cmd) == 0) { 294 | data = true; 295 | // Unlock query request 296 | cv.notify_all(); 297 | // Skip other request 298 | continue; 299 | } 300 | } 301 | // Find in all callback a data to send 302 | if (hashmap.find(sub_cmd) != hashmap.end()) 303 | { 304 | // Get callback from hashmap 305 | callback_data_t callback = hashmap[sub_cmd]; 306 | // Launch callback with return query 307 | callback(sub_data); 308 | } 309 | } 310 | else if(msg.compare("HLD\r") == 0) 311 | { 312 | isHLD = true; 313 | // Unlock query request 314 | cv.notify_one(); 315 | } 316 | else 317 | { 318 | ROS_INFO_STREAM("Other message " << msg); 319 | } 320 | } 321 | } 322 | ROS_INFO("Async serial reader closed"); 323 | } 324 | 325 | } 326 | -------------------------------------------------------------------------------- /src/configurator/motor_pid.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (C) 2017, Raffaello Bonghi 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 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * 3. Neither the name of the copyright holder nor the names of its 15 | * contributors may be used to endorse or promote products derived 16 | * from this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND 19 | * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 20 | * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 21 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 22 | * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 23 | * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 24 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 25 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 26 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 28 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #include "configurator/motor_pid.h" 32 | 33 | MotorPIDConfigurator::MotorPIDConfigurator(const ros::NodeHandle& nh, roboteq::serial_controller *serial, string path, string name, unsigned int number) 34 | : nh_(nh) 35 | , mSerial(serial) 36 | { 37 | // Find path param 38 | mName = nh_.getNamespace() + "/" + path + "/pid/" + name; 39 | mType = name; 40 | ROS_DEBUG_STREAM("Param " << path + "/pid/" + name << " has " << mName << " N:" << number); 41 | // Roboteq motor number 42 | mNumber = number; 43 | // Set false on first run 44 | setup_pid = false; 45 | 46 | } 47 | 48 | void MotorPIDConfigurator::initConfigurator(bool load_from_board) 49 | { 50 | // Check if is required load paramers 51 | if(load_from_board) 52 | { 53 | // Load parameters from roboteq 54 | getPIDFromRoboteq(); 55 | } 56 | 57 | // Initialize parameter dynamic reconfigure 58 | mDynRecServer = std::make_shared(mDynServerMutex, ros::NodeHandle(mName)); 59 | // Load default configuration 60 | roboteq_control::RoboteqPIDConfig config; 61 | mDynRecServer->getConfigDefault(config); 62 | // Update parameters 63 | mDynServerMutex.lock(); 64 | mDynRecServer->updateConfig(config); 65 | mDynServerMutex.unlock(); 66 | // Set callback 67 | mDynRecServer->setCallback(boost::bind(&MotorPIDConfigurator::reconfigureCBPID, this, _1, _2)); 68 | } 69 | 70 | void MotorPIDConfigurator::getPIDFromRoboteq() 71 | { 72 | try 73 | { 74 | // Get Position velocity [pag. 322] 75 | string str_pos_vel = mSerial->getParam("MVEL", std::to_string(mNumber)); 76 | int pos_vel = boost::lexical_cast(str_pos_vel); 77 | // Set params 78 | nh_.setParam(mName + "/position_mode_velocity", pos_vel); 79 | 80 | // Get number of turn between limits [pag. 325] 81 | string str_mxtrn = mSerial->getParam("MXTRN", std::to_string(mNumber)); 82 | unsigned int tmp_mxtrn = boost::lexical_cast(str_mxtrn); 83 | double mxtrn = ((double) tmp_mxtrn) / 100.0; 84 | // Set params 85 | nh_.setParam(mName + "/turn_min_to_max", mxtrn); 86 | 87 | // Get KP gain = kp / 10 [pag 319] 88 | string str_kp = mSerial->getParam("KP", std::to_string(mNumber)); 89 | unsigned int tmp_kp = boost::lexical_cast(str_kp); 90 | double kp = ((double) tmp_kp) / 10.0; 91 | // Set params 92 | nh_.setParam(mName + "/Kp", kp); 93 | 94 | // Get KI gain = ki / 10 [pag 318] 95 | string str_ki = mSerial->getParam("KI", std::to_string(mNumber)); 96 | unsigned int tmp_ki = boost::lexical_cast(str_ki); 97 | double ki = ((double) tmp_ki) / 10.0; 98 | // Set params 99 | nh_.setParam(mName + "/Ki", ki); 100 | 101 | // Get KD gain = kd / 10 [pag 317] 102 | string str_kd = mSerial->getParam("KD", std::to_string(mNumber)); 103 | unsigned int tmp_kd = boost::lexical_cast(str_kd); 104 | double kd = ((double) tmp_kd) / 10.0; 105 | // Set params 106 | nh_.setParam(mName + "/Kd", kd); 107 | 108 | // Get Integral cap [pag. 317] 109 | string str_icap = mSerial->getParam("ICAP", std::to_string(mNumber)); 110 | int icap = boost::lexical_cast(str_icap); 111 | // Set params 112 | nh_.setParam(mName + "/integrator_limit", icap); 113 | 114 | // Get closed loop error detection [pag. 311] 115 | string str_clred = mSerial->getParam("CLERD", std::to_string(mNumber)); 116 | int clerd = boost::lexical_cast(str_clred); 117 | // Set params 118 | nh_.setParam(mName + "/loop_error_detection", clerd); 119 | 120 | } catch (std::bad_cast& e) 121 | { 122 | ROS_WARN_STREAM("Failure parsing feedback data. Dropping message." << e.what()); 123 | } 124 | 125 | } 126 | 127 | void MotorPIDConfigurator::setPIDconfiguration() 128 | { 129 | // Set Position velocity 130 | int pos_vel; 131 | // Set params 132 | nh_.getParam(mName + "/position_mode_velocity", pos_vel); 133 | // Update position velocity 134 | mSerial->setParam("MVEL", std::to_string(mNumber) + " " + std::to_string(pos_vel)); 135 | 136 | // Set number of turn between limits 137 | double mxtrn; 138 | // Set params 139 | nh_.getParam(mName + "/turn_min_to_max", mxtrn); 140 | // Update position velocity 141 | mSerial->setParam("MXTRN", std::to_string(mNumber) + " " + std::to_string(mxtrn * 100)); 142 | 143 | // Set KP gain = kp * 10 144 | double kp; 145 | // Set params 146 | nh_.getParam(mName + "/Kp", kp); 147 | // Update gain position 148 | mSerial->setParam("KP", std::to_string(mNumber) + " " + std::to_string(kp * 10)); 149 | 150 | // Set KI gain = ki * 10 151 | double ki; 152 | // Set params 153 | nh_.getParam(mName + "/Ki", ki); 154 | // Set KI parameter 155 | mSerial->setParam("KI", std::to_string(mNumber) + " " + std::to_string(ki * 10)); 156 | 157 | // Set KD gain = kd * 10 158 | double kd; 159 | // Set params 160 | nh_.getParam(mName + "/Kd", kd); 161 | // Set KD parameter 162 | mSerial->setParam("KD", std::to_string(mNumber) + " " + std::to_string(kd * 10)); 163 | 164 | // Set Integral cap 165 | int icap; 166 | // Set params 167 | nh_.getParam(mName + "/integrator_limit", icap); 168 | // Update integral cap 169 | mSerial->setParam("ICAP", std::to_string(mNumber) + " " + std::to_string(icap)); 170 | 171 | // Set closed loop error detection 172 | int clerd; 173 | // Set params 174 | nh_.getParam(mName + "/loop_error_detection", clerd); 175 | // Update integral cap 176 | mSerial->setParam("CLERD", std::to_string(mNumber) + " " + std::to_string(clerd)); 177 | } 178 | 179 | void MotorPIDConfigurator::reconfigureCBPID(roboteq_control::RoboteqPIDConfig &config, uint32_t level) 180 | { 181 | //The first time we're called, we just want to make sure we have the 182 | //original configuration 183 | if(!setup_pid) 184 | { 185 | _last_pid_config = config; 186 | default_pid_config = _last_pid_config; 187 | setup_pid = true; 188 | return; 189 | } 190 | 191 | // Check type if the PID selected is right 192 | // Operative mode reference in [pag 314] 193 | string str_mode = mSerial->getParam("MMOD", std::to_string(mNumber)); 194 | // Get sign from roboteq board 195 | int mode = boost::lexical_cast(str_mode); 196 | // Check if the roboteq board is set in right configuration for this PID controller 197 | bool check1 = (mType.compare("velocity") == 0 && ((mode == 1) || (mode == 6))); 198 | bool check2 = (mType.compare("position") == 0 && ((mode == 2) || (mode == 3) || (mode == 4))); 199 | bool check3 = (mType.compare("torque") == 0 && ((mode == 5))); 200 | bool status = check1 | check2 | check3; 201 | // ROS_INFO_STREAM("[" << mode << "]" << mType << " ck1:" << check1 << " ck2:" << check2 << " ck3:" << check3 << " status:" << (status ? "true" : "false")); 202 | if(!status) 203 | { 204 | // Restore old configuration 205 | config = _last_pid_config; 206 | return; 207 | } 208 | 209 | if(config.restore_defaults) 210 | { 211 | //if someone sets restore defaults on the parameter server, prevent looping 212 | config.restore_defaults = false; 213 | // Overload config with default 214 | config = default_pid_config; 215 | } 216 | 217 | if(config.load_roboteq) 218 | { 219 | //if someone sets again the request on the parameter server, prevent looping 220 | config.load_roboteq = false; 221 | // Launch param load 222 | getPIDFromRoboteq(); 223 | // Skip other read 224 | return; 225 | } 226 | 227 | // Set Position velocity 228 | if(_last_pid_config.position_mode_velocity != config.position_mode_velocity) 229 | { 230 | // Update position velocity 231 | mSerial->setParam("MVEL", std::to_string(mNumber) + " " + std::to_string(config.position_mode_velocity)); 232 | } 233 | // Set number of turn between limits 234 | if(_last_pid_config.turn_min_to_max != config.turn_min_to_max) 235 | { 236 | // Update position velocity 237 | int gain = config.turn_min_to_max * 100; 238 | mSerial->setParam("MXTRN", std::to_string(mNumber) + " " + std::to_string(gain)); 239 | } 240 | // Set KP gain = kp * 10 241 | if(_last_pid_config.Kp != config.Kp) 242 | { 243 | // Update gain 244 | int gain = config.Kp * 10; 245 | mSerial->setParam("KP", std::to_string(mNumber) + " " + std::to_string(gain)); 246 | } 247 | // Set KI gain = ki * 10 248 | if(_last_pid_config.Ki != config.Ki) 249 | { 250 | // Update gain 251 | int gain = config.Ki * 10; 252 | mSerial->setParam("KI", std::to_string(mNumber) + " " + std::to_string(gain)); 253 | } 254 | // Set KD gain = kd * 10 255 | if(_last_pid_config.Kd != config.Kd) 256 | { 257 | // Update gain 258 | int gain = config.Kd * 10; 259 | mSerial->setParam("KD", std::to_string(mNumber) + " " + std::to_string(gain)); 260 | } 261 | 262 | // Set Integral cap 263 | if(_last_pid_config.integrator_limit != config.integrator_limit) 264 | { 265 | // Update integral cap 266 | mSerial->setParam("ICAP", std::to_string(mNumber) + " " + std::to_string(config.integrator_limit)); 267 | } 268 | // Set closed loop error detection 269 | if(_last_pid_config.loop_error_detection != config.loop_error_detection) 270 | { 271 | // Update integral cap 272 | mSerial->setParam("CLERD", std::to_string(mNumber) + " " + std::to_string(config.loop_error_detection)); 273 | } 274 | 275 | 276 | // Update last configuration 277 | _last_pid_config = config; 278 | } 279 | -------------------------------------------------------------------------------- /src/configurator/motor_param.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (C) 2017, Raffaello Bonghi 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 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * 3. Neither the name of the copyright holder nor the names of its 15 | * contributors may be used to endorse or promote products derived 16 | * from this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND 19 | * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 20 | * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 21 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 22 | * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 23 | * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 24 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 25 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 26 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 28 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #include "configurator/motor_param.h" 32 | 33 | MotorParamConfigurator::MotorParamConfigurator(const ros::NodeHandle& nh, roboteq::serial_controller *serial, std::string name, unsigned int number) 34 | : nh_(nh) 35 | , mSerial(serial) 36 | { 37 | // Find path param 38 | mName = nh_.getNamespace() + "/" + name; 39 | // Roboteq motor number 40 | mNumber = number; 41 | // Set false on first run 42 | setup_param = false; 43 | } 44 | 45 | void MotorParamConfigurator::initConfigurator(bool load_from_board) 46 | { 47 | double ratio; 48 | // Check if exist ratio variable 49 | if(nh_.hasParam(mName + "/ratio")) 50 | { 51 | double temp_double; 52 | nh_.getParam(mName + "/ratio", temp_double); 53 | // Set Ratio 54 | ratio = temp_double; 55 | } else 56 | { 57 | nh_.setParam(mName + "/ratio", 1.0); 58 | // Set Ratio 59 | ratio = 1.0; 60 | // Send alter ratio value 61 | ROS_WARN_STREAM("Default Ratio is " << ratio); 62 | } 63 | 64 | // Check if is required load paramers 65 | if(load_from_board) 66 | { 67 | // Load parameters from roboteq 68 | getParamFromRoboteq(); 69 | } 70 | 71 | // Initialize parameter dynamic reconfigure 72 | mDynRecServer_param = std::make_shared(mDynServerMutex_param, ros::NodeHandle(mName)); 73 | // Load default configuration 74 | roboteq_control::RoboteqParameterConfig config_param; 75 | mDynRecServer_param->getConfigDefault(config_param); 76 | // Update parameters 77 | mDynServerMutex_param.lock(); 78 | mDynRecServer_param->updateConfig(config_param); 79 | mDynServerMutex_param.unlock(); 80 | // Set callback 81 | mDynRecServer_param->setCallback(boost::bind(&MotorParamConfigurator::reconfigureCBParam, this, _1, _2)); 82 | 83 | // Initialize pid type dynamic reconfigure 84 | mDynRecServer_pid = std::make_shared(mDynServerMutex_pid, ros::NodeHandle(mName + "/pid")); 85 | // Load default configuration 86 | roboteq_control::RoboteqPIDtypeConfig config_pid; 87 | mDynRecServer_pid->getConfigDefault(config_pid); 88 | // Update parameters 89 | mDynServerMutex_pid.lock(); 90 | mDynRecServer_pid->updateConfig(config_pid); 91 | mDynServerMutex_pid.unlock(); 92 | // Set callback 93 | mDynRecServer_pid->setCallback(boost::bind(&MotorParamConfigurator::reconfigureCBPIDtype, this, _1, _2)); 94 | } 95 | 96 | void MotorParamConfigurator::setOperativeMode(int type) 97 | { 98 | // Update operative mode 99 | mSerial->setParam("MMOD", std::to_string(mNumber) + " " + std::to_string(type)); 100 | } 101 | 102 | int MotorParamConfigurator::getOperativeMode() 103 | { 104 | // Operative mode reference in [pag 321] 105 | string str_mode = mSerial->getParam("MMOD", std::to_string(mNumber)); 106 | // Get sign from roboteq board 107 | int mode = boost::lexical_cast(str_mode); 108 | // Set parameter 109 | // nh_.setParam(mName + "/operating_mode", mode); 110 | 111 | return mode; 112 | } 113 | 114 | void MotorParamConfigurator::reconfigureCBPIDtype(roboteq_control::RoboteqPIDtypeConfig &config, uint32_t level) 115 | { 116 | //The first time we're called, we just want to make sure we have the 117 | //original configuration 118 | if(!setup_pid_type) 119 | { 120 | _last_pid_type_config = config; 121 | default_pid_type_config = _last_pid_type_config; 122 | setup_pid_type = true; 123 | return; 124 | } 125 | 126 | if(config.restore_defaults) 127 | { 128 | //if someone sets restore defaults on the parameter server, prevent looping 129 | config.restore_defaults = false; 130 | // Overload config with default 131 | config = default_pid_type_config; 132 | } 133 | } 134 | 135 | void MotorParamConfigurator::getParamFromRoboteq() 136 | { 137 | try 138 | { 139 | // Load Ratio 140 | double ratio; 141 | nh_.getParam(mName + "/ratio", ratio); 142 | 143 | // Motor direction {1 (Clockwise), -1 (Underclockwise)} 144 | string str_mdir = mSerial->getParam("MDIR", std::to_string(mNumber)); 145 | // Get sign from roboteq board 146 | int sign = boost::lexical_cast(str_mdir) ? -1 : 1; 147 | // Set parameter 148 | nh_.setParam(mName + "/rotation", sign); 149 | 150 | // Stall detection 151 | string str_stall = mSerial->getParam("BLSTD", std::to_string(mNumber)); 152 | int stall = boost::lexical_cast(str_stall); 153 | // Set params 154 | nh_.setParam(mName + "/stall_detection", stall); 155 | 156 | // Get Max Amper limit = alim / 10 157 | string str_alim = mSerial->getParam("ALIM", std::to_string(mNumber)); 158 | unsigned int tmp = boost::lexical_cast(str_alim); 159 | double alim = ((double) tmp) / 10.0; 160 | // Set params 161 | nh_.setParam(mName + "/amper_limit", alim); 162 | 163 | // Max power forward 164 | string str_max_fw = mSerial->getParam("MXPF", std::to_string(mNumber)); 165 | // Get max forward 166 | int max_forward = boost::lexical_cast(str_max_fw); 167 | // Set parameter 168 | nh_.setParam(mName + "/max_acceleration", max_forward); 169 | 170 | // Max power forward reverse 171 | string str_max_re = mSerial->getParam("MXPR", std::to_string(mNumber)); 172 | // Get max reverse 173 | int max_reverse = boost::lexical_cast(str_max_re); 174 | // Set parameter 175 | nh_.setParam(mName + "/max_deceleration", max_reverse); 176 | 177 | // Get Max RPM motor 178 | string str_rpm_motor = mSerial->getParam("MXRPM", std::to_string(mNumber)); 179 | // Get RPM from board 180 | unsigned int rpm_motor = boost::lexical_cast(str_rpm_motor); 181 | // Convert in max RPM 182 | double max_rpm = ((double) rpm_motor) / ratio; 183 | // Set parameter 184 | nh_.setParam(mName + "/max_speed", max_rpm); 185 | 186 | // Get Max RPM acceleration rate 187 | string str_rpm_acceleration_motor = mSerial->getParam("MAC", std::to_string(mNumber)); 188 | // Get RPM from board 189 | unsigned int rpm_acceleration_motor = boost::lexical_cast(str_rpm_acceleration_motor); 190 | // Convert in max RPM 191 | double rpm_acceleration = ((double) rpm_acceleration_motor) / ratio; 192 | // Set parameter 193 | nh_.setParam(mName + "/max_acceleration", rpm_acceleration); 194 | 195 | // Get Max RPM deceleration rate 196 | string str_rpm_deceleration_motor = mSerial->getParam("MDEC", std::to_string(mNumber)); 197 | // Get RPM from board 198 | unsigned int rpm_deceleration_motor = boost::lexical_cast(str_rpm_deceleration_motor); 199 | // Convert in max RPM 200 | double rpm_deceleration = ((double) rpm_deceleration_motor) / ratio; 201 | // Set parameter 202 | nh_.setParam(mName + "/max_deceleration", rpm_deceleration); 203 | 204 | } catch (std::bad_cast& e) 205 | { 206 | ROS_WARN_STREAM("Failure parsing feedback data. Dropping message." << e.what()); 207 | } 208 | } 209 | 210 | void MotorParamConfigurator::reconfigureCBParam(roboteq_control::RoboteqParameterConfig &config, uint32_t level) { 211 | 212 | //The first time we're called, we just want to make sure we have the 213 | //original configuration 214 | if(!setup_param) 215 | { 216 | _last_param_config = config; 217 | default_param_config = _last_param_config; 218 | setup_param = true; 219 | return; 220 | } 221 | 222 | if(config.restore_defaults) 223 | { 224 | //if someone sets restore defaults on the parameter server, prevent looping 225 | config.restore_defaults = false; 226 | // Overload config with default 227 | config = default_param_config; 228 | } 229 | 230 | if(config.load_roboteq) 231 | { 232 | //if someone sets again the request on the parameter server, prevent looping 233 | config.load_roboteq = false; 234 | // Launch param load 235 | getParamFromRoboteq(); 236 | // Skip other read 237 | return; 238 | } 239 | 240 | // Update ratio 241 | // Get old max speed, acceleration and deceleartion and evaluate new equivalent value 242 | if(_last_param_config.ratio != config.ratio) 243 | { 244 | // Get Max RPM motor 245 | string str_rpm_motor = mSerial->getParam("MXRPM", std::to_string(mNumber)); 246 | // Get RPM from board 247 | unsigned int rpm_motor = boost::lexical_cast(str_rpm_motor); 248 | // Update with new max speed 249 | config.max_speed = ((double) rpm_motor) / config.ratio; 250 | 251 | // Get Max RPM acceleration rate 252 | string str_rpm_acceleration_motor = mSerial->getParam("MAC", std::to_string(mNumber)); 253 | // Get RPM from board 254 | unsigned int rpm_acceleration_motor = boost::lexical_cast(str_rpm_acceleration_motor); 255 | // Convert in max RPM 256 | config.max_acceleration = ((double) rpm_acceleration_motor) / config.ratio; 257 | 258 | // Get Max RPM deceleration rate 259 | string str_rpm_deceleration_motor = mSerial->getParam("MDEC", std::to_string(mNumber)); 260 | // Get RPM from board 261 | unsigned int rpm_deceleration_motor = boost::lexical_cast(str_rpm_deceleration_motor); 262 | // Convert in max RPM 263 | config.max_deceleration = ((double) rpm_deceleration_motor) / config.ratio; 264 | } 265 | 266 | if(_last_param_config.rotation != config.rotation) 267 | { 268 | // Update direction 269 | int direction = (config.rotation == -1) ? 1 : 0; 270 | mSerial->setParam("MDIR", std::to_string(mNumber) + " " + std::to_string(direction)); 271 | } 272 | // Stall detection [pag. 310] 273 | if(_last_param_config.stall_detection != config.stall_detection) 274 | { 275 | // Update stall detection value 276 | mSerial->setParam("BLSTD", std::to_string(mNumber) + " " + std::to_string(config.stall_detection)); 277 | } 278 | // Get Max Amper limit = alim * 10 [pag 306] 279 | if(_last_param_config.amper_limit != config.amper_limit) 280 | { 281 | // Update stall detection value 282 | int alim = config.amper_limit * 10; 283 | mSerial->setParam("ALIM", std::to_string(mNumber) + " " + std::to_string(alim)); 284 | } 285 | // Max power forward [pag. 323] 286 | if(_last_param_config.max_forward != config.max_forward) 287 | { 288 | // Update max forward 289 | mSerial->setParam("MXPF", std::to_string(mNumber) + " " + std::to_string(config.max_forward)); 290 | } 291 | // Max power forward reverse [pag. 324] 292 | if(_last_param_config.max_forward != config.max_reverse) 293 | { 294 | // Update max forward reverse 295 | mSerial->setParam("MXPR", std::to_string(mNumber) + " " + std::to_string(config.max_reverse)); 296 | } 297 | 298 | // Set Max RPM motor 299 | if(_last_param_config.max_speed != config.max_speed) 300 | { 301 | // Update max RPM motor 302 | long int max_speed_motor = config.ratio * config.max_speed; 303 | mSerial->setParam("MXRPM", std::to_string(mNumber) + " " + std::to_string(max_speed_motor)); 304 | } 305 | 306 | // Set Max RPM acceleration rate 307 | if(_last_param_config.max_acceleration != config.max_acceleration) 308 | { 309 | // Update max acceleration RPM/s motor 310 | long int max_acceleration_motor = config.ratio * config.max_acceleration; 311 | mSerial->setParam("MAC", std::to_string(mNumber) + " " + std::to_string(max_acceleration_motor)); 312 | } 313 | // Set Max RPM deceleration rate 314 | if(_last_param_config.max_deceleration != config.max_deceleration) 315 | { 316 | // Update max deceleration RPM/s motor 317 | long int max_deceleration_motor = config.ratio * config.max_deceleration; 318 | mSerial->setParam("MDEC", std::to_string(mNumber) + " " + std::to_string(max_deceleration_motor)); 319 | } 320 | 321 | // Update last configuration 322 | _last_param_config = config; 323 | } 324 | -------------------------------------------------------------------------------- /src/roboteq/motor.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (C) 2017, Raffaello Bonghi 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 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * 3. Neither the name of the copyright holder nor the names of its 15 | * contributors may be used to endorse or promote products derived 16 | * from this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND 19 | * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 20 | * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 21 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 22 | * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 23 | * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 24 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 25 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 26 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 28 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | #include "roboteq/motor.h" 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | 40 | namespace roboteq { 41 | 42 | Motor::Motor(const ros::NodeHandle& nh, serial_controller *serial, string name, unsigned int number) 43 | : DiagnosticTask(name + "_status") 44 | , joint_state_handle(name, &position_, &velocity_, &effort_) 45 | , joint_handle(joint_state_handle, &command_) 46 | , mNh(nh) 47 | , mSerial(serial) 48 | { 49 | // Initialize all variables 50 | mNumber = number; 51 | mMotorName = name; 52 | command_ = 0; 53 | // Reset variables 54 | position_ = 0; 55 | velocity_ = 0; 56 | effort_ = 0; 57 | // Initialize control mode 58 | _control_mode = -1; 59 | // Initialize reduction and get ratio 60 | reduction_ = 0; 61 | // Get ratio 62 | mNh.getParam(mMotorName + "/ratio", ratio_); 63 | ROS_INFO_STREAM("Motor" << mNumber << " " << ratio_); 64 | // Get encoder max speed parameter 65 | max_rpm_ = 0; 66 | mNh.getParam(mMotorName + "/max_speed", max_rpm_); 67 | 68 | // Initialize Dynamic reconfigurator for generic parameters 69 | parameter = new MotorParamConfigurator(nh, serial, mMotorName, number); 70 | // Initialize Dynamic reconfigurator for generic parameters 71 | pid_velocity = new MotorPIDConfigurator(nh, serial, mMotorName, "velocity", number); 72 | pid_torque = new MotorPIDConfigurator(nh, serial, mMotorName, "torque", number); 73 | pid_position = new MotorPIDConfigurator(nh, serial, mMotorName, "position", number); 74 | 75 | // Add a status motor publisher 76 | pub_status = mNh.advertise(mMotorName + "/status", 10); 77 | pub_control = mNh.advertise(mMotorName + "/control", 10); 78 | 79 | // Add callback 80 | // mSerial->addCallback(&Motor::read, this, "F" + std::to_string(mNumber)); 81 | } 82 | 83 | Motor::~Motor() { 84 | delete parameter; 85 | delete pid_velocity; 86 | delete pid_torque; 87 | delete pid_position; 88 | } 89 | 90 | void Motor::connectionCallback(const ros::SingleSubscriberPublisher& pub) 91 | { 92 | ROS_DEBUG_STREAM("Update: " << pub.getSubscriberName() << " - " << pub.getTopic()); 93 | } 94 | 95 | void Motor::initializeMotor(bool load_from_board) 96 | { 97 | // Initialize parameters 98 | parameter->initConfigurator(load_from_board); 99 | // Load PID configuration from roboteq board 100 | // Get operative mode 101 | _control_mode = parameter->getOperativeMode(); 102 | bool tmp_pos = load_from_board & ((_control_mode == 2) || (_control_mode == 3) || (_control_mode == 4)); 103 | bool tmp_vel = load_from_board & ((_control_mode == 1) || (_control_mode == 6)); 104 | bool tmp_tor = load_from_board & (_control_mode == 5); 105 | 106 | // ROS_INFO_STREAM("Type pos:" << tmp_pos << " vel:" << tmp_vel << " tor:" << tmp_tor); 107 | 108 | // Initialize pid loader 109 | pid_position->initConfigurator(tmp_pos); 110 | // Initialize pid loader 111 | pid_velocity->initConfigurator(tmp_vel); 112 | // Initialize pid loader 113 | pid_torque->initConfigurator(tmp_tor); 114 | 115 | // stop the motor 116 | stopMotor(); 117 | } 118 | 119 | /** 120 | * @brief registerSensor register the sensor 121 | * 122 | * @param sensor the sensor interface 123 | */ 124 | void Motor::registerSensor(GPIOSensor* sensor) 125 | { 126 | _sensor = sensor; 127 | mNh.getParam(mMotorName + "/ratio", reduction_); 128 | reduction_ = _sensor->getConversion(reduction_); 129 | } 130 | 131 | /** 132 | * Conversion of radians to encoder ticks. 133 | * 134 | * @param x Angular position in radians. 135 | * @return Angular position in encoder ticks. 136 | */ 137 | double Motor::to_encoder_ticks(double x) 138 | { 139 | // Return the value converted 140 | return x * (reduction_) / (2 * M_PI); 141 | } 142 | 143 | /** 144 | * Conversion of encoder ticks to radians. 145 | * 146 | * @param x Angular position in encoder ticks. 147 | * @return Angular position in radians. 148 | */ 149 | double Motor::from_encoder_ticks(double x) 150 | { 151 | // Return the value converted 152 | return x * (2 * M_PI) / (reduction_); 153 | } 154 | 155 | void Motor::setupLimits(urdf::Model model) 156 | { 157 | /// Add a velocity joint limits infomations 158 | joint_limits_interface::JointLimits limits; 159 | joint_limits_interface::SoftJointLimits soft_limits; 160 | 161 | bool state = true; 162 | 163 | // Manual value setting 164 | limits.has_velocity_limits = true; 165 | limits.max_velocity = 5.0; 166 | limits.has_effort_limits = true; 167 | limits.max_effort = 2.0; 168 | 169 | // Populate (soft) joint limits from URDF 170 | // Limits specified in URDF overwrite existing values in 'limits' and 'soft_limits' 171 | // Limits not specified in URDF preserve their existing values 172 | 173 | urdf::JointConstSharedPtr urdf_joint = model.getJoint(mMotorName); 174 | const bool urdf_limits_ok = getJointLimits(urdf_joint, limits); 175 | const bool urdf_soft_limits_ok = getSoftJointLimits(urdf_joint, soft_limits); 176 | 177 | if(urdf_limits_ok) { 178 | ROS_INFO_STREAM("LOAD [" << mMotorName << "] limits from URDF: |" << limits.max_velocity << "| rad/s & |" << limits.max_effort << "| Nm"); 179 | state = false; 180 | } 181 | 182 | if(urdf_soft_limits_ok) { 183 | ROS_INFO_STREAM("LOAD [" << mMotorName << "] soft limits from URDF: |" << limits.max_velocity << "| rad/s & |" << limits.max_effort << "| Nm"); 184 | state = false; 185 | } 186 | 187 | // Populate (soft) joint limits from the ros parameter server 188 | // Limits specified in the parameter server overwrite existing values in 'limits' and 'soft_limits' 189 | // Limits not specified in the parameter server preserve their existing values 190 | const bool rosparam_limits_ok = getJointLimits(mMotorName, mNh, limits); 191 | if(rosparam_limits_ok) { 192 | ROS_WARN_STREAM("OVERLOAD [" << mMotorName << "] limits from ROSPARAM: |" << limits.max_velocity << "| rad/s & |" << limits.max_effort << "| Nm"); 193 | state = false; 194 | } 195 | else 196 | { 197 | ROS_DEBUG("Setup limits, PARAM NOT available"); 198 | } 199 | // If does not read any parameter from URDF or rosparm load default parameter 200 | if(state) 201 | { 202 | ROS_WARN_STREAM("LOAD [" << mMotorName << "] with DEFAULT limit = |" << limits.max_velocity << "| rad/s & |" << limits.max_effort << "| Nm"); 203 | } 204 | 205 | // Set maximum limits if doesn't have limit 206 | if(limits.has_position_limits == false) 207 | { 208 | limits.max_position = 6.28; 209 | } 210 | // <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<< 211 | // TODO CHECK HOW TO INITIALIZE 212 | // Update limits 213 | // max_position = limits.max_position; 214 | // max_velocity = limits.max_velocity * params.ratio; 215 | // max_effort = limits.max_effort; 216 | // updateLimits(limits.max_position, limits.max_velocity, limits.max_effort); 217 | // >>>>>>>>>>>>>>>>>>>>>>>>>>>>>> 218 | 219 | joint_limits_interface::VelocityJointSoftLimitsHandle handle(joint_handle, // We read the state and read/write the command 220 | limits, // Limits spec 221 | soft_limits); // Soft limits spec 222 | 223 | vel_limits_interface.registerHandle(handle); 224 | } 225 | 226 | void Motor::run(diagnostic_updater::DiagnosticStatusWrapper &stat) 227 | { 228 | string control; 229 | switch(_control_mode) 230 | { 231 | case -1: 232 | control = "unset"; 233 | break; 234 | case 0: 235 | control = "Open loop"; 236 | break; 237 | case 1: 238 | control = "Closed loop speed"; 239 | break; 240 | case 2: 241 | control = "Closed loop position relative"; 242 | break; 243 | case 3: 244 | control = "Closed loop count position"; 245 | break; 246 | case 4: 247 | control = "Closed loop position tracking"; 248 | break; 249 | case 5: 250 | control = "Closed loop torque"; 251 | break; 252 | case 6: 253 | control = "Closed loop speed position"; 254 | break; 255 | default: 256 | control = "Error"; 257 | break; 258 | } 259 | stat.add("Control", control); 260 | 261 | stat.add("Motor number", mNumber); 262 | stat.add("PWM rate (%)", msg_control.pwm); 263 | stat.add("Voltage (V)", msg_status.volts); 264 | stat.add("Battery (A)", msg_status.amps_batt); 265 | stat.add("Watt motor (W)", msg_status.volts * msg_status.amps_motor); 266 | stat.add("Watt batt (W)", msg_status.volts * msg_status.amps_batt); 267 | stat.add("Loop error", msg_control.loop_error); 268 | stat.add("Track", msg_status.track); 269 | stat.add("Position (deg)", position_); 270 | stat.add("Velociy (RPM)", to_rpm(velocity_)); 271 | stat.add("Current (A)", msg_status.amps_motor); 272 | stat.add("Torque (Nm)", effort_); 273 | 274 | 275 | stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Motor Ready!"); 276 | 277 | if(status_.amps_limit) 278 | { 279 | stat.mergeSummaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "Amps limits motor=%.2f", msg_status.amps_motor); 280 | } 281 | 282 | if(status_.amps_triggered_active) 283 | { 284 | stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Amps trigger active"); 285 | } 286 | 287 | if(status_.forward_limit_triggered) 288 | { 289 | stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Forward limit triggered"); 290 | } 291 | 292 | if(status_.reverse_limit_triggered) 293 | { 294 | stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Reverse limit triggered"); 295 | } 296 | 297 | if(status_.loop_error_detect) 298 | { 299 | stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "Loop error detection"); 300 | } 301 | 302 | if(status_.motor_stalled) 303 | { 304 | stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "Motor stalled"); 305 | } 306 | 307 | if(status_.safety_stop_active) 308 | { 309 | stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Safety stop active"); 310 | } 311 | } 312 | 313 | void Motor::stopMotor() 314 | { 315 | // set to zero the reference 316 | mSerial->command("G ", std::to_string(mNumber) + " 0"); 317 | // Stop motor [pag 222] 318 | mSerial->command("MS", std::to_string(mNumber)); 319 | } 320 | 321 | void Motor::switchController(string type) 322 | { 323 | if(type.compare("diff_drive_controller/DiffDriveController") == 0) 324 | { 325 | // Load type of PID velocity 326 | int pid_vel; 327 | mNh.getParam(mMotorName + "/pid/closed_loop_velocity", pid_vel); 328 | // ROS_INFO_STREAM("VEL mode:" << pid_vel); 329 | // Set in speed position mode 330 | parameter->setOperativeMode(pid_vel); 331 | _control_mode = pid_vel; 332 | } 333 | else 334 | { 335 | _control_mode = -1; 336 | // stop motor 337 | stopMotor(); 338 | } 339 | } 340 | 341 | void Motor::resetPosition(double position) 342 | { 343 | // Send reset position 344 | double enc_conv = to_encoder_ticks(position); 345 | mSerial->command("C ", std::to_string(mNumber) + " " + std::to_string(enc_conv)); 346 | } 347 | 348 | void Motor::writeCommandsToHardware(ros::Duration period) 349 | { 350 | // Enforce joint limits for all registered handles 351 | // Note: one can also enforce limits on a per-handle basis: handle.enforceLimits(period) 352 | vel_limits_interface.enforceLimits(period); 353 | // Get encoder max speed parameter 354 | double max_rpm; 355 | mNh.getParam(mMotorName + "/max_speed", max_rpm); 356 | // Build a command message 357 | long long int roboteq_velocity = static_cast(to_rpm(command_) / max_rpm * 1000.0); 358 | 359 | // ROS_INFO_STREAM("Velocity" << mNumber << " val=" << command << " " << roboteq_velocity); 360 | 361 | mSerial->command("G ", std::to_string(mNumber) + " " + std::to_string(roboteq_velocity)); 362 | } 363 | 364 | void Motor::read(string data) { 365 | std::vector fields; 366 | boost::split(fields, data, boost::algorithm::is_any_of(":")); 367 | // Decode list 368 | readVector(fields); 369 | } 370 | 371 | void Motor::readVector(std::vector fields) { 372 | double position, vel, volts, amps_motor; 373 | // Scale factors as outlined in the relevant portions of the user manual, please 374 | // see mbs/script.mbs for URL and specific page references. 375 | try 376 | { 377 | // reference command FM <-> _MOTFLAG [pag. 246] 378 | unsigned char status = boost::lexical_cast(fields[0]); 379 | memcpy(&status_, &status, sizeof(status)); 380 | // reference command M <-> _MOTCMD [pag. 250] 381 | double cmd = boost::lexical_cast(fields[1]) * max_rpm_ / 1000.0; 382 | // reference command F <-> _FEEDBK [pag. 244] 383 | vel = boost::lexical_cast(fields[2]) * max_rpm_ / 1000.0; 384 | // reference command E <-> _LPERR [pag. 243] 385 | double loop_error = boost::lexical_cast(fields[3]) * max_rpm_ / 1000.0; 386 | // reference command P <-> _MOTPWR [pag. 255] 387 | double pwm = boost::lexical_cast(fields[4]); 388 | // reference voltage V <-> _VOLTS [pag. ---] 389 | volts = boost::lexical_cast(fields[5]) / 10; 390 | // reference command A <-> _MOTAMPS [pag. 230] 391 | amps_motor = boost::lexical_cast(fields[6]) / 10; 392 | // reference command BA <-> _BATAMPS [pag. 233] 393 | double amps_batt = boost::lexical_cast(fields[7]) / 10; 394 | // Reference command CR <-> _RELCNTR [pag. 241] 395 | // To check and substitute with C 396 | // Reference command C <-> _ABCNTR [pag. ---] 397 | position = boost::lexical_cast(fields[8]); 398 | // reference command TR <-> _TR [pag. 260] 399 | double track = boost::lexical_cast(fields[9]); 400 | 401 | // Build messages 402 | msg_control.header.stamp = ros::Time::now(); 403 | // Fill fields 404 | msg_control.reference = (cmd / ratio_); 405 | msg_control.feedback = (vel / ratio_); 406 | msg_control.loop_error = (loop_error / ratio_); 407 | msg_control.pwm = pwm; 408 | // Publish status control motor 409 | pub_control.publish(msg_control); 410 | 411 | // Build control message 412 | msg_status.header.stamp = ros::Time::now(); 413 | // Fill fields 414 | msg_status.volts = volts; 415 | msg_status.amps_motor = amps_motor; 416 | msg_status.amps_batt = amps_batt; 417 | msg_status.track = track; 418 | // Publish status motor 419 | pub_status.publish(msg_status); 420 | } 421 | catch (std::bad_cast& e) 422 | { 423 | ROS_WARN_STREAM(" [" << mNumber << "] " << mMotorName << ": Failure parsing feedback data. Dropping message. " << e.what()); 424 | // Load same values 425 | position = to_encoder_ticks(position_); 426 | vel = ratio_ * velocity_; 427 | volts = 0; 428 | amps_motor = 0; 429 | } 430 | // Update position 431 | position_ = from_encoder_ticks(position); 432 | // Update velocity motor 433 | velocity_ = (vel / ratio_); 434 | // Evaluate effort 435 | if(velocity_ != 0) 436 | { 437 | effort_ = ((volts * amps_motor) / velocity_) * ratio_; 438 | } 439 | else 440 | { 441 | effort_ = 0; 442 | } 443 | //ROS_INFO_STREAM("[" << mNumber << "] track:" << msg_status.track); 444 | //ROS_INFO_STREAM("[" << mNumber << "] volts:" << msg_status.volts << " - amps:" << msg_status.amps_motor); 445 | //ROS_INFO_STREAM("[" << mNumber << "] status:" << status << " - pos:"<< position << " - vel:" << velocity << " - torque:"); 446 | 447 | } 448 | 449 | } 450 | -------------------------------------------------------------------------------- /src/roboteq/roboteq.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (C) 2017, Raffaello Bonghi 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 7 | * are met: 8 | * 9 | * 1. Redistributions of source code must retain the above copyright 10 | * notice, this list of conditions and the following disclaimer. 11 | * 2. Redistributions in binary form must reproduce the above copyright 12 | * notice, this list of conditions and the following disclaimer in the 13 | * documentation and/or other materials provided with the distribution. 14 | * 3. Neither the name of the copyright holder nor the names of its 15 | * contributors may be used to endorse or promote products derived 16 | * from this software without specific prior written permission. 17 | * 18 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND 19 | * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, 20 | * BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 21 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT 22 | * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 23 | * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 24 | * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; 25 | * OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, 26 | * WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE 27 | * OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, 28 | * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 29 | */ 30 | 31 | 32 | #include "roboteq/roboteq.h" 33 | #include 34 | 35 | namespace roboteq 36 | { 37 | 38 | Roboteq::Roboteq(const ros::NodeHandle &nh, const ros::NodeHandle &private_nh, serial_controller *serial) 39 | : DiagnosticTask("Roboteq") 40 | , mNh(nh) 41 | , private_mNh(private_nh) 42 | , mSerial(serial) 43 | { 44 | // First run dynamic reconfigurator 45 | setup_controller = false; 46 | // Initialize GPIO reading 47 | _isGPIOreading = false; 48 | // Load default configuration roboteq board 49 | getRoboteqInformation(); 50 | 51 | //Services 52 | srv_board = private_mNh.advertiseService("system", &Roboteq::service_Callback, this); 53 | 54 | motor_loop_ = false; 55 | _first = false; 56 | std::vector joint_list; 57 | if(private_nh.hasParam("joint")) 58 | { 59 | private_nh.getParam("joint", joint_list); 60 | } 61 | else 62 | { 63 | ROS_WARN("No joint list!"); 64 | joint_list.push_back("joint_0"); 65 | joint_list.push_back("joint_1"); 66 | private_nh.setParam("joint", joint_list); 67 | } 68 | // Disable ECHO 69 | mSerial->echo(false); 70 | // Disable Script and wait to load all parameters 71 | mSerial->script(false); 72 | // Stop motors 73 | bool stop_motor = mSerial->command("EX"); 74 | ROS_DEBUG_STREAM("Stop motor: " << (stop_motor ? "true" : "false")); 75 | 76 | // Initialize Joints 77 | for(unsigned i=0; i < joint_list.size(); ++i) 78 | { 79 | string motor_name = joint_list.at(i); 80 | int number = i + 1; 81 | 82 | if(!private_nh.hasParam(motor_name)) 83 | { 84 | _first = true; 85 | ROS_WARN_STREAM("Load " << motor_name << " parameters from Roboteq board"); 86 | } 87 | 88 | if(private_nh.hasParam(motor_name + "/number")) 89 | { 90 | private_nh.getParam(motor_name + "/number", number); 91 | } 92 | else 93 | { 94 | ROS_WARN_STREAM("Default number selected for Motor: " << motor_name << " is " << number); 95 | private_nh.setParam(motor_name + "/number", number); 96 | } 97 | 98 | ROS_INFO_STREAM("Motor[" << number << "] name: " << motor_name); 99 | //mMotor[motor_name] = new Motor(private_mNh, serial, motor_name, number); 100 | mMotor.push_back(new Motor(private_mNh, serial, motor_name, number)); 101 | } 102 | 103 | // Launch initialization input/output 104 | for(int i = 0; i < 6; ++i) 105 | { 106 | _param_pulse.push_back(new GPIOPulseConfigurator(private_mNh, serial, mMotor, "/InOut", i+1)); 107 | } 108 | for(int i = 0; i < 6; ++i) 109 | { 110 | _param_analog.push_back(new GPIOAnalogConfigurator(private_mNh, serial, mMotor, "/InOut", i+1)); 111 | } 112 | for(int i = 0; i < 2; ++i) 113 | { 114 | _param_encoder.push_back(new GPIOEncoderConfigurator(private_mNh, serial, mMotor, "/InOut", i+1)); 115 | } 116 | 117 | // Add subscriber stop 118 | sub_stop = private_mNh.subscribe("emergency_stop", 1, &Roboteq::stop_Callback, this); 119 | // Initialize the peripheral publisher 120 | pub_peripheral = private_mNh.advertise("peripheral", 10, 121 | boost::bind(&Roboteq::connectionCallback, this, _1), boost::bind(&Roboteq::connectionCallback, this, _1)); 122 | 123 | } 124 | 125 | void Roboteq::connectionCallback(const ros::SingleSubscriberPublisher& pub) { 126 | // Information about the subscriber 127 | ROS_INFO_STREAM("Update: " << pub.getSubscriberName() << " - " << pub.getTopic()); 128 | // Check if some subscriber is connected with peripheral publisher 129 | _isGPIOreading = (pub_peripheral.getNumSubscribers() >= 1); 130 | } 131 | 132 | void Roboteq::switch_off() 133 | { 134 | // TODO: Add unregister all interfaces 135 | // Prevent 136 | // Controller Spawner error while taking down controllers: 137 | // transport error completing service call: unable to receive data from sender, check sender's logs for details 138 | // Send emergency stop 139 | // And release motors 140 | mSerial->command("EX"); 141 | } 142 | 143 | void Roboteq::stop_Callback(const std_msgs::Bool::ConstPtr& msg) 144 | { 145 | // Wait end motor loop 146 | while(motor_loop_); 147 | // Read status message 148 | bool status = (int)msg.get()->data; 149 | if(status) 150 | { 151 | // Send emergency stop 152 | mSerial->command("EX"); 153 | ROS_WARN_STREAM("Emergency stop"); 154 | } else 155 | { 156 | // Safety release 157 | mSerial->command("MG"); 158 | ROS_WARN_STREAM("Safety release"); 159 | } 160 | } 161 | 162 | void Roboteq::getRoboteqInformation() 163 | { 164 | // Load model roboeq board 165 | string trn = mSerial->getQuery("TRN"); 166 | std::vector fields; 167 | boost::split(fields, trn, boost::algorithm::is_any_of(":")); 168 | _type = fields[0]; 169 | _model = fields[1]; 170 | // ROS_INFO_STREAM("Model " << _model); 171 | // Load firmware version 172 | _version = mSerial->getQuery("FID"); 173 | // Load UID 174 | _uid = mSerial->getQuery("UID"); 175 | } 176 | 177 | Roboteq::~Roboteq() 178 | { 179 | // ROS_INFO_STREAM("Script: " << script(false)); 180 | int i = 0; 181 | for ( i = 0; i < mMotor.size(); i++) 182 | { 183 | // Stop the motor and delete the object 184 | mMotor[i]->stopMotor(); 185 | delete mMotor[i]; 186 | } 187 | for ( i = 0; i < _param_pulse.size(); i++) 188 | { 189 | delete _param_pulse[i]; 190 | } 191 | for ( i = 0; i < _param_analog.size(); i++) 192 | { 193 | delete _param_analog[i]; 194 | } 195 | for ( i = 0; i < _param_encoder.size(); i++) 196 | { 197 | delete _param_encoder[i]; 198 | } 199 | } 200 | 201 | void Roboteq::initialize() 202 | { 203 | // Check if is required load paramers 204 | if(_first) 205 | { 206 | // Load parameters from roboteq 207 | getControllerFromRoboteq(); 208 | } 209 | 210 | // Initialize parameter dynamic reconfigure 211 | mDynRecServer = boost::make_shared>(mDynServerMutex, private_mNh); 212 | // Load default configuration 213 | roboteq_control::RoboteqControllerConfig config; 214 | mDynRecServer->getConfigDefault(config); 215 | // Update parameters 216 | mDynServerMutex.lock(); 217 | mDynRecServer->updateConfig(config); 218 | mDynServerMutex.unlock(); 219 | // Set callback 220 | mDynRecServer->setCallback(boost::bind(&Roboteq::reconfigureCBController, this, _1, _2)); 221 | 222 | // Launch initialization GPIO 223 | for (vector::iterator it = _param_pulse.begin() ; it != _param_pulse.end(); ++it) 224 | { 225 | ((GPIOPulseConfigurator*)(*it))->initConfigurator(true); 226 | } 227 | for (vector::iterator it = _param_analog.begin() ; it != _param_analog.end(); ++it) 228 | { 229 | ((GPIOAnalogConfigurator*)(*it))->initConfigurator(true); 230 | } 231 | for (vector::iterator it = _param_encoder.begin() ; it != _param_encoder.end(); ++it) 232 | { 233 | ((GPIOEncoderConfigurator*)(*it))->initConfigurator(true); 234 | } 235 | 236 | // Initialize all motors in list 237 | for (vector::iterator it = mMotor.begin() ; it != mMotor.end(); ++it) 238 | { 239 | Motor* motor = ((Motor*)(*it)); 240 | // Launch initialization motors 241 | motor->initializeMotor(_first); 242 | ROS_DEBUG_STREAM("Motor [" << motor->getName() << "] Initialized"); 243 | } 244 | } 245 | 246 | void Roboteq::initializeInterfaces() 247 | { 248 | // Initialize the diagnostic from the primitive object 249 | initializeDiagnostic(); 250 | 251 | if (!model.initParam("robot_description")){ 252 | ROS_ERROR("Failed to parse urdf file"); 253 | } 254 | else 255 | { 256 | ROS_INFO_STREAM("robot_description found! " << model.name_ << " parsed!"); 257 | } 258 | 259 | for (vector::iterator it = mMotor.begin() ; it != mMotor.end(); ++it) 260 | { 261 | Motor* motor = ((Motor*)(*it)); 262 | /// State interface 263 | joint_state_interface.registerHandle(motor->joint_state_handle); 264 | /// Velocity interface 265 | velocity_joint_interface.registerHandle(motor->joint_handle); 266 | 267 | // Setup limits 268 | motor->setupLimits(model); 269 | 270 | // reset position joint 271 | double position = 0; 272 | ROS_DEBUG_STREAM("Motor [" << motor->getName() << "] reset position to: " << position); 273 | motor->resetPosition(position); 274 | 275 | //Add motor in diagnostic updater 276 | diagnostic_updater.add(*(motor)); 277 | ROS_INFO_STREAM("Motor [" << motor->getName() << "] Registered"); 278 | } 279 | 280 | ROS_DEBUG_STREAM("Send all Constraint configuration"); 281 | 282 | /// Register interfaces 283 | registerInterface(&joint_state_interface); 284 | registerInterface(&velocity_joint_interface); 285 | } 286 | 287 | void Roboteq::initializeDiagnostic() 288 | { 289 | ROS_INFO_STREAM("Roboteq " << _type << ":" << _model); 290 | ROS_INFO_STREAM(_version); 291 | 292 | diagnostic_updater.setHardwareID(_model); 293 | 294 | // Initialize this diagnostic interface 295 | diagnostic_updater.add(*this); 296 | } 297 | 298 | void Roboteq::updateDiagnostics() 299 | { 300 | ROS_DEBUG_STREAM("Update diagnostic"); 301 | 302 | // Scale factors as outlined in the relevant portions of the user manual, please 303 | // see mbs/script.mbs for URL and specific page references. 304 | try 305 | { 306 | // Fault flag [pag. 245] 307 | string fault_flag = mSerial->getQuery("FF"); 308 | unsigned char fault = boost::lexical_cast(fault_flag); 309 | memcpy(&_fault, &fault, sizeof(fault)); 310 | // Status flag [pag. 247] 311 | string status_flag = mSerial->getQuery("FS"); 312 | unsigned char status = boost::lexical_cast(status_flag); 313 | memcpy(&_flag, &status, sizeof(status)); 314 | // power supply voltage 315 | string supply_voltage_1 = mSerial->getQuery("V", "1"); 316 | _volts_internal = boost::lexical_cast(supply_voltage_1) / 10; 317 | string supply_voltage_3 = mSerial->getQuery("V", "3"); 318 | _volts_five = boost::lexical_cast(supply_voltage_3) / 1000; 319 | // temperature channels [pag. 259] 320 | string temperature_1 = mSerial->getQuery("T", "1"); 321 | _temp_mcu = boost::lexical_cast(temperature_1); 322 | string temperature_2 = mSerial->getQuery("T", "2"); 323 | _temp_bridge = boost::lexical_cast(temperature_2); 324 | // Force update all diagnostic parts 325 | diagnostic_updater.force_update(); 326 | } 327 | catch (std::bad_cast& e) 328 | { 329 | ROS_WARN_STREAM("Diagnostic: Failure parsing feedback data. Dropping message." << e.what()); 330 | } 331 | } 332 | 333 | void Roboteq::read(const ros::Time& time, const ros::Duration& period) { 334 | //ROS_DEBUG_STREAM("Get measure from Roboteq"); 335 | motor_loop_ = true; 336 | 337 | std::vector motors[mMotor.size()]; 338 | std::vector fields; 339 | 340 | // Read status motor 341 | // motor status flags [pag. 246] 342 | string str_status = mSerial->getQuery("FM"); 343 | // ROS_INFO_STREAM("FM=" << str_status); 344 | boost::split(fields, str_status, boost::algorithm::is_any_of(":")); 345 | for(int i = 0; i < fields.size(); ++i) { 346 | motors[i].push_back(fields[i]); 347 | } 348 | // motor command [pag. 250] 349 | string str_motor = mSerial->getQuery("M"); 350 | // ROS_INFO_STREAM("M =" << str_motor); 351 | boost::split(fields, str_motor, boost::algorithm::is_any_of(":")); 352 | for(int i = 0; i < fields.size(); ++i) { 353 | motors[i].push_back(fields[i]); 354 | } 355 | // motor feedback [pag. 244] 356 | string str_feedback = mSerial->getQuery("F"); 357 | //ROS_INFO_STREAM("F =" << str_feedback); 358 | boost::split(fields, str_feedback, boost::algorithm::is_any_of(":")); 359 | for(int i = 0; i < fields.size(); ++i) { 360 | motors[i].push_back(fields[i]); 361 | } 362 | // motor loop error [pag. 244] 363 | string str_loop_error = mSerial->getQuery("E"); 364 | // ROS_INFO_STREAM("E =" << str_loop_error); 365 | boost::split(fields, str_loop_error, boost::algorithm::is_any_of(":")); 366 | for(int i = 0; i < fields.size(); ++i) { 367 | motors[i].push_back(fields[i]); 368 | } 369 | // motor power [pag. 255] 370 | string str_motor_power = mSerial->getQuery("P"); 371 | // ROS_INFO_STREAM("P =" << str_motor_power); 372 | boost::split(fields, str_motor_power, boost::algorithm::is_any_of(":")); 373 | for(int i = 0; i < fields.size(); ++i) { 374 | motors[i].push_back(fields[i]); 375 | } 376 | // power supply voltage [pag. 262] 377 | string str_voltage_supply = mSerial->getQuery("V", "2"); 378 | //ROS_INFO_STREAM("V2=" << str_voltage_supply); 379 | for(int i = 0; i < fields.size(); ++i) { 380 | motors[i].push_back(str_voltage_supply); 381 | } 382 | // motor Amps [pag. 230] 383 | string str_motor_amps = mSerial->getQuery("A"); 384 | //ROS_INFO_STREAM("A =" << str_motor_amps); 385 | boost::split(fields, str_motor_amps, boost::algorithm::is_any_of(":")); 386 | for(int i = 0; i < fields.size(); ++i) { 387 | motors[i].push_back(fields[i]); 388 | } 389 | // motor battery amps [pag. 233] 390 | string str_motor_battery_amps = mSerial->getQuery("BA"); 391 | //ROS_INFO_STREAM("BA=" << str_motor_battery_amps); 392 | boost::split(fields, str_motor_battery_amps, boost::algorithm::is_any_of(":")); 393 | for(int i = 0; i < fields.size(); ++i) { 394 | motors[i].push_back(fields[i]); 395 | } 396 | // position encoder value [pag. 236] 397 | string str_position_encoder_absolute = mSerial->getQuery("C"); 398 | //ROS_INFO_STREAM("C =" << str_position_encoder_absolute); 399 | boost::split(fields, str_position_encoder_absolute, boost::algorithm::is_any_of(":")); 400 | for(int i = 0; i < fields.size(); ++i) { 401 | motors[i].push_back(fields[i]); 402 | } 403 | // motor track [pag. 260] 404 | string str_motor_track = mSerial->getQuery("TR"); 405 | //ROS_INFO_STREAM("TR=" << str_motor_track); 406 | boost::split(fields, str_motor_track, boost::algorithm::is_any_of(":")); 407 | for(int i = 0; i < fields.size(); ++i) { 408 | motors[i].push_back(fields[i]); 409 | } 410 | // send list 411 | for(int i = 0; i < mMotor.size(); ++i) { 412 | //get number motor initialization 413 | unsigned int idx = mMotor[i]->mNumber-1; 414 | // Read and decode vector 415 | mMotor[i]->readVector(motors[idx]); 416 | } 417 | 418 | // Read data from GPIO 419 | if(_isGPIOreading) 420 | { 421 | msg_peripheral.header.stamp = ros::Time::now(); 422 | std::vector fields; 423 | // Get Pulse in status [pag. 256] 424 | string pulse_in = mSerial->getQuery("PI"); 425 | boost::split(fields, pulse_in, boost::algorithm::is_any_of(":")); 426 | // Clear msg list 427 | msg_peripheral.pulse_in.clear(); 428 | for(int i = 0; i < fields.size(); ++i) 429 | { 430 | try 431 | { 432 | msg_peripheral.pulse_in.push_back(boost::lexical_cast(fields[i])); 433 | } 434 | catch (std::bad_cast& e) 435 | { 436 | msg_peripheral.pulse_in.push_back(0); 437 | } 438 | } 439 | // Get analog input values [pag. 231] 440 | string analog = mSerial->getQuery("AI"); 441 | boost::split(fields, analog, boost::algorithm::is_any_of(":")); 442 | // Clear msg list 443 | msg_peripheral.analog.clear(); 444 | for(int i = 0; i < fields.size(); ++i) 445 | { 446 | try 447 | { 448 | msg_peripheral.analog.push_back(boost::lexical_cast(fields[i]) / 1000.0); 449 | } 450 | catch (std::bad_cast& e) 451 | { 452 | msg_peripheral.analog.push_back(0); 453 | } 454 | } 455 | 456 | // Get Digital input values [pag. 242] 457 | string digital_in = mSerial->getQuery("DI"); 458 | boost::split(fields, digital_in, boost::algorithm::is_any_of(":")); 459 | // Clear msg list 460 | msg_peripheral.digital_in.clear(); 461 | for(int i = 0; i < fields.size(); ++i) 462 | { 463 | try 464 | { 465 | msg_peripheral.digital_in.push_back(boost::lexical_cast(fields[i])); 466 | } 467 | catch (std::bad_cast& e) 468 | { 469 | msg_peripheral.digital_in.push_back(0); 470 | } 471 | } 472 | 473 | string digital_out = mSerial->getQuery("DO"); 474 | unsigned int num = 0; 475 | try 476 | { 477 | num = boost::lexical_cast(digital_out); 478 | } 479 | catch (std::bad_cast& e) 480 | { 481 | num = 0; 482 | } 483 | int mask = 0x0; 484 | // Clear msg list 485 | msg_peripheral.digital_out.clear(); 486 | for(int i = 0; i < 8; ++i) 487 | { 488 | msg_peripheral.digital_out.push_back((mask & num)); 489 | mask <<= 1; 490 | } 491 | 492 | // Send GPIO status 493 | pub_peripheral.publish(msg_peripheral); 494 | } 495 | motor_loop_ = false; 496 | } 497 | 498 | void Roboteq::write(const ros::Time& time, const ros::Duration& period) { 499 | //ROS_DEBUG_STREAM("Write command to Roboteq"); 500 | motor_loop_ = true; 501 | for (vector::iterator it = mMotor.begin() ; it != mMotor.end(); ++it) 502 | { 503 | Motor* motor = ((Motor*)(*it)); 504 | // Launch initialization motors 505 | motor->writeCommandsToHardware(period); 506 | ROS_DEBUG_STREAM("Motor [" << motor->getName() << "] Send commands"); 507 | } 508 | motor_loop_ = false; 509 | } 510 | 511 | bool Roboteq::prepareSwitch(const std::list& start_list, const std::list& stop_list) 512 | { 513 | ROS_INFO_STREAM("Prepare to switch!"); 514 | return true; 515 | } 516 | 517 | void Roboteq::doSwitch(const std::list& start_list, const std::list& stop_list) 518 | { 519 | // Stop all controller in list 520 | for(std::list::const_iterator it = stop_list.begin(); it != stop_list.end(); ++it) 521 | { 522 | //ROS_INFO_STREAM("DO SWITCH STOP name: " << it->name << " - type: " << it->type); 523 | const hardware_interface::InterfaceResources& iface_res = it->claimed_resources.front(); 524 | for (std::set::const_iterator res_it = iface_res.resources.begin(); res_it != iface_res.resources.end(); ++res_it) 525 | { 526 | ROS_INFO_STREAM(it->name << "[" << *res_it << "] STOP"); 527 | 528 | for (vector::iterator m_it = mMotor.begin() ; m_it != mMotor.end(); ++m_it) 529 | { 530 | Motor* motor = ((Motor*)(*m_it)); 531 | if(motor->getName().compare(*res_it) == 0) 532 | { 533 | // switch initialization motors 534 | motor->switchController("disable"); 535 | break; 536 | } 537 | } 538 | } 539 | } 540 | // Stop motors 541 | mSerial->command("EX"); 542 | // Run all new controllers 543 | for(std::list::const_iterator it = start_list.begin(); it != start_list.end(); ++it) 544 | { 545 | //ROS_INFO_STREAM("DO SWITCH START name: " << it->name << " - type: " << it->type); 546 | const hardware_interface::InterfaceResources& iface_res = it->claimed_resources.front(); 547 | for (std::set::const_iterator res_it = iface_res.resources.begin(); res_it != iface_res.resources.end(); ++res_it) 548 | { 549 | ROS_INFO_STREAM(it->name << "[" << *res_it << "] START"); 550 | 551 | for (vector::iterator m_it = mMotor.begin() ; m_it != mMotor.end(); ++m_it) 552 | { 553 | Motor* motor = ((Motor*)(*m_it)); 554 | if(motor->getName().compare(*res_it) == 0) 555 | { 556 | // switch initialization motors 557 | motor->switchController(it->type); 558 | break; 559 | } 560 | } 561 | } 562 | } 563 | // Enable motor 564 | mSerial->command("MG"); 565 | } 566 | 567 | void Roboteq::run(diagnostic_updater::DiagnosticStatusWrapper &stat) 568 | { 569 | stat.add("Type board", _type); 570 | stat.add("Name board", _model); 571 | stat.add("Version", _version); 572 | stat.add("UID", _uid); 573 | 574 | stat.add("Temp MCU (deg)", _temp_mcu); 575 | stat.add("Temp Bridge (deg)", _temp_bridge); 576 | 577 | stat.add("Internal (V)", _volts_internal); 578 | stat.add("5v regulator (V)", _volts_five); 579 | 580 | string mode = "[ "; 581 | if(_flag.serial_mode) 582 | mode += "serial "; 583 | if(_flag.pulse_mode) 584 | mode += "pulse "; 585 | if(_flag.analog_mode) 586 | mode += "analog "; 587 | mode += "]"; 588 | // Mode roboteq board 589 | stat.add("Mode", mode); 590 | // Spectrum 591 | stat.add("Spectrum", (bool)_flag.spectrum); 592 | // Microbasic 593 | stat.add("Micro basic running", (bool)_flag.microbasic_running); 594 | 595 | stat.summary(diagnostic_msgs::DiagnosticStatus::OK, "Board ready!"); 596 | 597 | if(_flag.power_stage_off) 598 | { 599 | stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Power stage OFF"); 600 | } 601 | 602 | if(_flag.stall_detect) 603 | { 604 | stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "Stall detect"); 605 | } 606 | 607 | if(_flag.at_limit) 608 | { 609 | stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "At limit"); 610 | } 611 | 612 | if(_fault.brushless_sensor_fault) 613 | { 614 | stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "Brushless sensor fault"); 615 | } 616 | 617 | if(_fault.emergency_stop) 618 | { 619 | stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::WARN, "Emergency stop"); 620 | } 621 | 622 | if(_fault.mosfet_failure) 623 | { 624 | stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "MOSFET failure"); 625 | } 626 | 627 | if(_fault.overheat) 628 | { 629 | stat.mergeSummaryf(diagnostic_msgs::DiagnosticStatus::ERROR, "Over heat MCU=%f, Bridge=%f", _temp_mcu, _temp_bridge); 630 | } 631 | 632 | if(_fault.overvoltage) 633 | { 634 | stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "Over voltage"); 635 | } 636 | 637 | if(_fault.undervoltage) 638 | { 639 | stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "Under voltage"); 640 | } 641 | 642 | if(_fault.short_circuit) 643 | { 644 | stat.mergeSummary(diagnostic_msgs::DiagnosticStatus::ERROR, "Short circuit"); 645 | } 646 | } 647 | 648 | void Roboteq::getControllerFromRoboteq() 649 | { 650 | try 651 | { 652 | // Get PWM frequency PWMF 653 | string str_pwm = mSerial->getParam("PWMF"); 654 | unsigned int tmp_pwm = boost::lexical_cast(str_pwm); 655 | double pwm = ((double) tmp_pwm) / 10.0; 656 | // Set params 657 | private_mNh.setParam("pwm_frequency", pwm); 658 | 659 | // Get over voltage limit OVL 660 | string str_ovl = mSerial->getParam("OVL"); 661 | unsigned int tmp_ovl = boost::lexical_cast(str_ovl); 662 | double ovl = ((double) tmp_ovl) / 10.0; 663 | // Set params 664 | private_mNh.setParam("over_voltage_limit", ovl); 665 | 666 | // Get over voltage hystersis OVH 667 | string str_ovh = mSerial->getParam("OVH"); 668 | unsigned int tmp_ovh = boost::lexical_cast(str_ovh); 669 | double ovh = ((double) tmp_ovh) / 10.0; 670 | // Set params 671 | private_mNh.setParam("over_voltage_hysteresis", ovh); 672 | 673 | // Get under voltage limit UVL 674 | string str_uvl = mSerial->getParam("UVL"); 675 | unsigned int tmp_uvl = boost::lexical_cast(str_uvl); 676 | double uvl = ((double) tmp_uvl) / 10.0; 677 | // Set params 678 | private_mNh.setParam("under_voltage_limit", uvl); 679 | 680 | // Get brake activation delay BKD 681 | string str_break = mSerial->getParam("BKD"); 682 | int break_delay = boost::lexical_cast(str_break); 683 | // Set params 684 | private_mNh.setParam("break_delay", break_delay); 685 | 686 | // Get Mixing mode MXMD 687 | //string str_mxd = mSerial->getParam("MXMD", "1"); 688 | string str_mxd = mSerial->getParam("MXMD"); 689 | // ROS_INFO_STREAM("MXMD "<< str_mxd); 690 | int mixed = boost::lexical_cast(str_mxd); 691 | // Set params 692 | private_mNh.setParam("mixing", mixed); 693 | 694 | } catch (std::bad_cast& e) 695 | { 696 | ROS_WARN_STREAM("Failure parsing feedback data. Dropping message." << e.what()); 697 | } 698 | } 699 | 700 | void Roboteq::reconfigureCBController(roboteq_control::RoboteqControllerConfig &config, uint32_t level) 701 | { 702 | //The first time we're called, we just want to make sure we have the 703 | //original configuration 704 | if(!setup_controller) 705 | { 706 | _last_controller_config = config; 707 | default_controller_config = _last_controller_config; 708 | setup_controller = true; 709 | return; 710 | } 711 | 712 | if(config.restore_defaults) 713 | { 714 | //if someone sets restore defaults on the parameter server, prevent looping 715 | config.restore_defaults = false; 716 | // Overload config with default 717 | config = default_controller_config; 718 | } 719 | 720 | if(config.factory_reset) 721 | { 722 | //if someone sets restore defaults on the parameter server, prevent looping 723 | config.factory_reset = false; 724 | mSerial->factoryReset(); 725 | // Enable load from roboteq board 726 | config.load_roboteq = true; 727 | } 728 | 729 | if(config.load_from_eeprom) 730 | { 731 | //if someone sets again the request on the parameter server, prevent looping 732 | config.load_from_eeprom = false; 733 | mSerial->loadFromEEPROM(); 734 | // Enable load from roboteq board 735 | config.load_roboteq = true; 736 | } 737 | 738 | if(config.load_roboteq) 739 | { 740 | //if someone sets again the request on the parameter server, prevent looping 741 | config.load_roboteq = false; 742 | // Launch param load 743 | getControllerFromRoboteq(); 744 | // Skip other read 745 | return; 746 | } 747 | 748 | if(config.store_in_eeprom) 749 | { 750 | //if someone sets again the request on the parameter server, prevent looping 751 | config.store_in_eeprom = false; 752 | // Save all data in eeprom 753 | mSerial->saveInEEPROM(); 754 | } 755 | 756 | // Set PWM frequency PWMF [pag. 320] 757 | if(_last_controller_config.pwm_frequency != config.pwm_frequency) 758 | { 759 | // Update PWM 760 | int pwm = config.pwm_frequency * 10; 761 | mSerial->setParam("PWMF", std::to_string(pwm)); 762 | } 763 | // Set over voltage limit OVL [pag. 319] 764 | if(_last_controller_config.over_voltage_limit != config.over_voltage_limit) 765 | { 766 | // Update over voltage limit [pag. 319] 767 | int ovl = config.over_voltage_limit * 10; 768 | mSerial->setParam("OVL", std::to_string(ovl)); 769 | } 770 | // Set over voltage hystersis OVH [pag. 318] 771 | if(_last_controller_config.over_voltage_hysteresis != config.over_voltage_hysteresis) 772 | { 773 | // Update over voltage hysteresis 774 | int ovh = config.over_voltage_hysteresis * 10; 775 | mSerial->setParam("OVH", std::to_string(ovh)); 776 | } 777 | // Set under voltage limit UVL [pag. 327] 778 | if(_last_controller_config.under_voltage_limit != config.under_voltage_limit) 779 | { 780 | // Update under voltage limit 781 | int uvl = config.under_voltage_limit * 10; 782 | mSerial->setParam("UVL", std::to_string(uvl)); 783 | } 784 | // Set brake activation delay BKD [pag. 301] 785 | if(_last_controller_config.break_delay != config.break_delay) 786 | { 787 | // Update brake activation delay 788 | mSerial->setParam("BKD", std::to_string(config.break_delay)); 789 | } 790 | 791 | // Set Mixing mode MXMD [pag. 315] 792 | if(_last_controller_config.mixing != config.mixing) 793 | { 794 | // Update Mixing mode 795 | //mSerial->setParam("MXMD", std::to_string(config.mixing) + ":0"); 796 | mSerial->setParam("MXMD", std::to_string(config.mixing)); 797 | } 798 | 799 | // Update last configuration 800 | _last_controller_config = config; 801 | } 802 | 803 | bool Roboteq::service_Callback(roboteq_control::Service::Request &req, roboteq_control::Service::Response &msg) { 804 | // Convert to lower case 805 | std::transform(req.service.begin(), req.service.end(), req.service.begin(), ::tolower); 806 | //ROS_INFO_STREAM("Name request: " << req.service); 807 | if(req.service.compare("info") == 0) 808 | { 809 | msg.information = "\nBoard type: " + _type + "\n" 810 | "Name board: " + _model + "\n" 811 | "Version: " + _version + "\n" 812 | "UID: " + _uid + "\n"; 813 | } 814 | else if(req.service.compare("reset") == 0) 815 | { 816 | // Launch reset command 817 | mSerial->reset(); 818 | // return message 819 | msg.information = "System reset"; 820 | } 821 | else if(req.service.compare("save") == 0) 822 | { 823 | // Launch reset command 824 | mSerial->saveInEEPROM(); 825 | // return message 826 | msg.information = "Parameters saved"; 827 | } 828 | else 829 | { 830 | msg.information = "\nList of commands availabes: \n" 831 | "* info - information about this board \n" 832 | "* reset - " + _model + " board software reset\n" 833 | "* save - Save all paramters in EEPROM \n" 834 | "* help - this help."; 835 | } 836 | return true; 837 | } 838 | 839 | } 840 | --------------------------------------------------------------------------------