From 950c0b028ef214df11e3e3baf5daf92a6a620106 Mon Sep 17 00:00:00 2001 From: mmaralit-adi Date: Wed, 10 May 2023 21:33:25 +0800 Subject: [PATCH] Adjustments for position scaler and status, and added script to get all axis parameter values Updated for the following features: - Added Position Scaler consideration in position calculation (higher prio than encoder steps) - Added Status information in published /tmc_info - Changed default /tmc_info publish rate to 10Hz (previously 1Hz) - Added GAP_params.sh to query YAML's axis parameters' values - Made TMCM-1636 FollowEepromConfig as False and then used calibrated values in YAML - Updated Maintainer email Co-Authored-By: Christian Joseph Acar <124771470+CAcarADI@users.noreply.github.com> --- CMakeLists.txt | 1 + README.md | 22 +++++ config/TMCM-1617_Ext.yaml | 26 +++--- config/TMCM-1636_Ext.yaml | 40 ++++----- config/autogenerated/TMCM-1617.yaml | 6 ++ config/autogenerated/TMCM-1636.yaml | 6 ++ include/tmcl_ros/tmcl_interpreter.h | 4 +- include/tmcl_ros/tmcl_ros.h | 6 +- msg/TmcInfo.msg | 2 + package.xml | 4 +- scripts/GAP_params.sh | 134 ++++++++++++++++++++++++++++ src/tmcl_ros.cpp | 58 +++++++++++- src/tmcl_ros_publisher.cpp | 53 ++++++++++- src/tmcl_ros_subscriber.cpp | 60 +++++++++++-- 14 files changed, 373 insertions(+), 49 deletions(-) create mode 100644 scripts/GAP_params.sh diff --git a/CMakeLists.txt b/CMakeLists.txt index e0905e3..73eb578 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -9,6 +9,7 @@ project(tmcl_ros) ## is used, also find other catkin packages find_package(catkin REQUIRED COMPONENTS roscpp + geometry_msgs message_generation std_msgs ) diff --git a/README.md b/README.md index 74e3d81..21c04a7 100644 --- a/README.md +++ b/README.md @@ -356,6 +356,28 @@ $ ./CAN_interface.sh > :memo: _Note: If bitrate is changed, it is advised to restart the board and initialize the CAN again using the new bitrate this time._ +## Querying the Axis Parameters' Values + +The GAP_params.sh (or Get Access Parameters script) collects actual values of the axis parameters listed on the generated yaml. One particular useful case is if the user decided to enable FollowEepromConfig and wanted to know all the parameter values stored in EEPROM. To use this script, it is expected that the tmcl_ros node is running on another terminal. To make the script executable: + +```bash +$ cd ~/catkin_ws/src/tmcl_ros/scripts +$ chmod +x GAP_params.sh +``` +Then, to execute the script: +```bash +$ ./GAP_params.sh +``` +Where: +- YAML filename is the name of the autogenerated yaml, without the extension, used by the node running from another terminal +- motor number is the number of which motor should the user want to extract the parameters from +- enable description is a boolean argument (represented by Y/N) which can display (or not display) the additional descriptions/range of each parameter in the output + +For example, to get the axis parameters' values of TMCM-1636's motor 0 with the description enabled: +```bash +$ ./GAP_params.sh TMCM-1636 0 Y +``` + # Limitations 1. No support for multiple/concurrent CAN or TMCL connections yet. 2. No support for interfaces other than CAN yet. diff --git a/config/TMCM-1617_Ext.yaml b/config/TMCM-1617_Ext.yaml index 1335ebd..ec298e2 100644 --- a/config/TMCM-1617_Ext.yaml +++ b/config/TMCM-1617_Ext.yaml @@ -63,35 +63,35 @@ comm_exec_cmd_retries: 1 # Description: tmc_info topics that will contain chosen TMC info that will be published # Possible values: # - Any tmc_info topic name -tmc_info_topic: [/tmc_info_0, /tmc_info_1, /tmc_info] +tmc_info_topic: [/tmc_info_0, /tmc_info, /tmc_info] # cmd_vel topics to subscribe from # Description: Twist topics that will be the source of target velocity to be set on the TMC # Possible values: # - Any Twist topic name # Note: Only its linear.x will be used (range: -200000 to 200000) -tmc_cmd_vel_topic: [/cmd_vel_0, /cmd_vel_1, /cmd_vel] +tmc_cmd_vel_topic: [/cmd_vel_0, /cmd_vel, /cmd_vel] # cmd_pos topics to subscribe from # Description: Int32 topics that will be the source of target position to be set on the TMC # Possible values: # - Any Int32 topic name (range: -2147483648 to -2147483647) -tmc_cmd_abspos_topic: [/cmd_abspos_0, /cmd_abspos_1, /cmd_abspos] +tmc_cmd_abspos_topic: [/cmd_abspos_0, /cmd_abspos, /cmd_abspos] # cmd_pos topics to subscribe from # Description: Int32 topics that will be the source of target position to be set on the TMC # Possible values: # - Any Int32 topic name (range: -2147483648 to -2147483647) -tmc_cmd_relpos_topic: [/cmd_relpos_0, /cmd_relpos_1, /cmd_relpos] +tmc_cmd_relpos_topic: [/cmd_relpos_0, /cmd_relpos, /cmd_relpos] # cmd_trq topics to subscribe from # Description: Int32 topics that will be the source of target torque to be set on the TMC # Possible values: # - Any Int32 topic name (range: -60000 to 60000 mA) -tmc_cmd_trq_topic: [/cmd_trq_0, /cmd_trq_1, /cmd_trq] +tmc_cmd_trq_topic: [/cmd_trq_0, /cmd_trq, /cmd_trq] # Wheel diameter attached to the motor (in meters) -# Description: Wheel diameter that is attached on the motor shaft directly. This is to convert linear values to rpm +# Description: Wheel diameter that is attached on the motor. This is to convert linear values to rpm # Note: If wheel diameter is 0, cmd_vel is equal to rpm wheel_diameter: 0.25 @@ -100,27 +100,27 @@ wheel_diameter: 0.25 # Possible values: # - 0: Not used # - 1: Active -en_motors: [1, 1, 0] +en_motors: [1, 0, 0] # Publish TMC information # Decription: Enables/disables publishing of TMC information # Possible values: # - true: Enabled # - false: Disabled -en_pub_tmc_info: [true, true, false] +en_pub_tmc_info: [true, false, false] # Publish rate of TMC information # Decription: Publish rate (hertz) of TMC information -# Possible values: 0 - 50 -pub_rate_tmc_info: 1 +# Possible values: 10 - 100 +pub_rate_tmc_info: 10 ########## TMC Information # Enable/Disable axis parameters that the user can optionally publish every publish rate as long as en_pub_tmc_info is true -pub_actual_vel: [true, true, false] -pub_actual_trq: [true, true, false] -pub_actual_pos: [true, true, false] +pub_actual_vel: [true, false, false] +pub_actual_trq: [true, false, false] +pub_actual_pos: [true, false, false] ########################################################## ########################################################## diff --git a/config/TMCM-1636_Ext.yaml b/config/TMCM-1636_Ext.yaml index 76831c2..94ebe00 100644 --- a/config/TMCM-1636_Ext.yaml +++ b/config/TMCM-1636_Ext.yaml @@ -63,35 +63,35 @@ comm_exec_cmd_retries: 1 # Description: tmc_info topics that will contain chosen TMC info that will be published # Possible values: # - Any tmc_info topic name -tmc_info_topic: [/tmc_info_0, /tmc_info_1, /tmc_info] +tmc_info_topic: [/tmc_info_0, /tmc_info, /tmc_info] # cmd_vel topics to subscribe from # Description: Twist topics that will be the source of target velocity to be set on the TMC # Possible values: # - Any Twist topic name # Note: Only its linear.x will be used (range: -200000 to 200000) -tmc_cmd_vel_topic: [/cmd_vel_0, /cmd_vel_1, /cmd_vel] +tmc_cmd_vel_topic: [/cmd_vel_0, /cmd_vel, /cmd_vel] # cmd_pos topics to subscribe from # Description: Int32 topics that will be the source of target position to be set on the TMC # Possible values: # - Any Int32 topic name (range: -2147483648 to -2147483647) -tmc_cmd_abspos_topic: [/cmd_abspos_0, /cmd_abspos_1, /cmd_abspos] +tmc_cmd_abspos_topic: [/cmd_abspos_0, /cmd_abspos, /cmd_abspos] # cmd_pos topics to subscribe from # Description: Int32 topics that will be the source of target position to be set on the TMC # Possible values: # - Any Int32 topic name (range: -2147483648 to -2147483647) -tmc_cmd_relpos_topic: [/cmd_relpos_0, /cmd_relpos_1, /cmd_relpos] +tmc_cmd_relpos_topic: [/cmd_relpos_0, /cmd_relpos, /cmd_relpos] # cmd_trq topics to subscribe from # Description: Int32 topics that will be the source of target torque to be set on the TMC # Possible values: # - Any Int32 topic name (range: -60000 to 60000 mA) -tmc_cmd_trq_topic: [/cmd_trq_0, /cmd_trq_1, /cmd_trq] +tmc_cmd_trq_topic: [/cmd_trq_0, /cmd_trq, /cmd_trq] # Wheel diameter attached to the motor (in meters) -# Description: Wheel diameter that is attached on the motor shaft directly. This is to convert linear values to rpm +# Description: Wheel diameter that is attached on the motor. This is to convert linear values to rpm # Note: If wheel diameter is 0, cmd_vel is equal to rpm wheel_diameter: 0.25 @@ -100,27 +100,27 @@ wheel_diameter: 0.25 # Possible values: # - 0: Not used # - 1: Active -en_motors: [1, 1, 0] +en_motors: [1, 0, 0] # Publish TMC information # Decription: Enables/disables publishing of TMC information # Possible values: # - true: Enabled # - false: Disabled -en_pub_tmc_info: [true, true, false] +en_pub_tmc_info: [true, false, false] # Publish rate of TMC information # Decription: Publish rate (hertz) of TMC information -# Possible values: 0 - 50 -pub_rate_tmc_info: 1 +# Possible values: 10 - 100 +pub_rate_tmc_info: 10 ########## TMC Information # Enable/Disable axis parameters that the user can optionally publish every publish rate as long as en_pub_tmc_info is true -pub_actual_vel: [true, true, false] -pub_actual_trq: [true, true, false] -pub_actual_pos: [true, true, false] +pub_actual_vel: [true, false, false] +pub_actual_trq: [true, false, false] +pub_actual_pos: [true, false, false] ########################################################## ########################################################## @@ -137,7 +137,7 @@ pub_actual_pos: [true, true, false] # Possible values: # - true: Follow all the motor configuration as-is (EEPROM values set as part of TMC firmware or as set in TMCL-IDE) # - false: Have user set all motor configurations in this YAML (below) -FollowEepromConfig: true +FollowEepromConfig: false ########## Sensor Selection Settings @@ -197,11 +197,11 @@ HallSensorOffset_Ext: [0, 0, 0] # Encoder direction # Description: Encoder direction in a way that ROR increases position counter -EncoderDirection_Ext: [1, 0, 0] +EncoderDirection_Ext: [0, 0, 0] # Encoder steps # Description: Encoder steps per full motor rotation -EncoderSteps_Ext: [4096, 0, 0] +EncoderSteps_Ext: [16384, 0, 0] # Encoder init mode # Description: Encoder init mode @@ -211,7 +211,7 @@ EncoderInitMode_Ext: [0, 0, 0] # Torque P # Description: P parameter for current PID regulator -TorqueP_Ext: [300, 0, 0] +TorqueP_Ext: [1500, 0, 0] # Torque I # Description: I parameter for current PID regulator @@ -219,15 +219,15 @@ TorqueI_Ext: [500, 0, 0] # Velocity P # Description: P parameter for velocity PID regulator -VelocityP_Ext: [400, 0, 0] +VelocityP_Ext: [131, 0, 0] # Velocity I # Description: I parameter for velocity PID regulator -VelocityI_Ext: [100, 0, 0] +VelocityI_Ext: [49, 0, 0] # Position P # Description: P parameter for position PID regulator -PositionP_Ext: [10, 0, 0] +PositionP_Ext: [11, 0, 0] ########## Brake Chopper Settings diff --git a/config/autogenerated/TMCM-1617.yaml b/config/autogenerated/TMCM-1617.yaml index a28a174..e3aebae 100644 --- a/config/autogenerated/TMCM-1617.yaml +++ b/config/autogenerated/TMCM-1617.yaml @@ -15,6 +15,12 @@ TotalMotors: 1 +# Descriptions for possible values of StatusFlags: +# Size:: 32 [bit] +StatusFlags: [1, 156, 0, 0, 0] +StatusFlagsRegName: [OVERCURRENT, UNDERVOLTAGE, OVERVOLTAGE, OVERTEMPERATURE, MOTORHALTED, HALLERROR, DRIVER_ERROR, INIT_ERROR, STOP_MODE, VELOCITY_MODE, POSITION_MODE, TORQUE_MODE, POSITION_END, MODULE_INITIALIZED] +StatusFlagsRegShift: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 14, 15] + # Descriptions for possible values of SupplyVoltage: # Range:: 0 to 1000 [0.1V] SupplyVoltage: [1, 220, 240, 0, 1000] diff --git a/config/autogenerated/TMCM-1636.yaml b/config/autogenerated/TMCM-1636.yaml index 2de799f..bbd746b 100644 --- a/config/autogenerated/TMCM-1636.yaml +++ b/config/autogenerated/TMCM-1636.yaml @@ -15,6 +15,12 @@ TotalMotors: 1 +# Descriptions for possible values of StatusFlags: +# Size:: 32 [bit] +StatusFlags: [1, 156, 0, 0, 0] +StatusFlagsRegName: [OVERCURRENT, UNDERVOLTAGE, OVERVOLTAGE, OVERTEMPERATURE, MOTORHALTED, HALLERROR, DRIVER_ERROR, INIT_ERROR, STOP_MODE, VELOCITY_MODE, POSITION_MODE, TORQUE_MODE, POSITION_END, MODULE_INITIALIZED, IIT_EXCEEDED_1, IIT_EXCEEDED_2] +StatusFlagsRegShift: [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 14, 15, 16, 17] + # Descriptions for possible values of SupplyVoltage: # Range:: 0 to 1000 [100mV] SupplyVoltage: [1, 220, 480, 0, 1000] diff --git a/include/tmcl_ros/tmcl_interpreter.h b/include/tmcl_ros/tmcl_interpreter.h index 35f3082..4e96a29 100644 --- a/include/tmcl_ros/tmcl_interpreter.h +++ b/include/tmcl_ros/tmcl_interpreter.h @@ -139,7 +139,8 @@ typedef enum /* LUT row indeces of get-able/set-able Axis Parameters */ typedef enum { - ROW_IDX_SupplyVoltage = 0, + ROW_IDX_StatusFlags = 0, + ROW_IDX_SupplyVoltage, ROW_IDX_TargetVelocity, ROW_IDX_ActualVelocity, ROW_IDX_TargetPosition, @@ -175,6 +176,7 @@ typedef enum /* LUT row strings of get-able/set-able Axis Parameters */ std::vector tmcl_lut_row_str = { + "StatusFlags", "SupplyVoltage", "TargetVelocity", "ActualVelocity", diff --git a/include/tmcl_ros/tmcl_ros.h b/include/tmcl_ros/tmcl_ros.h index 15c6075..1813780 100644 --- a/include/tmcl_ros/tmcl_ros.h +++ b/include/tmcl_ros/tmcl_ros.h @@ -17,7 +17,7 @@ #define TIMEOUT_MS_DEFAULT 10 #define EXEC_CMD_RETRIES_MAX 3 #define EXEC_CMD_RETRIES_DEFAULT 1 -#define PUB_RATE_MAX 50 +#define PUB_RATE_MAX 100 #define RATE_20KBPS 20000 #define RATE_50KBPS 50000 #define RATE_100KBPS 100000 @@ -97,6 +97,10 @@ class TmclROS std::vector> ap_cfg; std::vector> ap_cfg_ext; + /* Separate vectors for registers of Statusflags */ + std::vector param_status_flags_reg_name; + std::vector param_status_flags_reg_shift; + /* Handle for TmclInterpreter */ TmclInterpreter *tmcl; diff --git a/msg/TmcInfo.msg b/msg/TmcInfo.msg index 8772bc7..04bf4ca 100644 --- a/msg/TmcInfo.msg +++ b/msg/TmcInfo.msg @@ -1,6 +1,8 @@ std_msgs/Header header string interface_name float32 board_voltage +int32 status_flag +string status uint8 motor_num float32 velocity int32 position diff --git a/package.xml b/package.xml index c2aba50..778af99 100644 --- a/package.xml +++ b/package.xml @@ -4,15 +4,17 @@ 1.0.0 The tmcl_ros package - guest + ROS@analog.com MIT catkin roscpp + geometry_msgs message_generation roscpp roscpp + geometry_msgs message_runtime std_msgs diff --git a/scripts/GAP_params.sh b/scripts/GAP_params.sh new file mode 100644 index 0000000..7548481 --- /dev/null +++ b/scripts/GAP_params.sh @@ -0,0 +1,134 @@ +#!/bin/bash + +# Text colors +CYAN='\033[0;36m' +GREEN='\033[0;32m' +RED='\033[0;31m' +BLUE='\033[1;34m' +YELLOW='\033[1;33m' +NC='\033[0m' # No Color + +en_description=false + +parameter_name=("StatusFlags" "SupplyVoltage" "TargetVelocity" "ActualVelocity" "TargetPosition" "ActualPosition" "TargetTorque" "ActualTorque" "CommutationMode" "CommutationModeVelocity" "CommutationModePosition" "MaxTorque" "OpenLoopCurrent" "Acceleration" "MotorPolePairs" "PWMFrequency" "HallSensorPolarity" "HallSensorDirection" "HallInterpolation" "HallSensorOffset" "EncoderDirection" "EncoderSteps" "EncoderInitMode" "TorqueP" "TorqueI" "VelocityP" "VelocityI" "PositionP" "BrakeChopperEnabled" "BrakeChopperVoltage" "BrakeChopperHysteresis" "PositionScalerM") + +if [[ $# -ne 3 ]] +then + echo -e "${RED}Error! Illegal number of parameters.${NC}" + echo "To run this scipt:" + echo " $0 " + echo "Example:" + echo " $0 TMCM-1636 0 Y" + echo -e "${YELLOW}Note: Running that example will refer to TMCM-1636.yaml from the \"tmcl_ros/config/autogenerated\" directory${NC}, Motor0 and will print description" + exit 1 +fi + +cd src +# Check if yaml file is in the directory autogenerated +yaml_location=tmcl_ros/config/autogenerated/$1.yaml +if [ -f "$yaml_location" ] +then + echo "YAML file exists." +else + echo -e "${RED}YAML file does not exist, Exiting!${NC}" + exit 1 +fi + +# Check if motor is available +MN=$(grep -w "TotalMotors:" $yaml_location) +motornum=${MN##*": "} +motornum=$((motornum - 1)) +if [ $2 -gt "$motornum" ] +then + echo -e "${RED}Error! Motor is not available, Exiting!${NC}" + exit 1 +fi + +# Check if 3rd argument is correct (Y/N) +if [ $3 == "Y" -o $3 == "y" ] +then + en_description=true +elif [ $3 == "N" -o $3 == "n" ] +then + en_description=false +else + echo -e "${RED}Error! Wrong argument. Only accepts Y/N character, Exiting!${NC}" + exit 1 +fi + + +# Extract servicename from tmcl_ros_service.cpp file +SN=$(grep -w "s_node_name +" tmcl_ros/src/tmcl_ros_service.cpp) +servicename=${SN%\"*} +servicename=${servicename##*\"} + +for entry in `rosservice list` +do + if [[ $entry == *"$servicename"* ]] + then + break + fi +done +echo -e "${CYAN}Service name: $entry${NC}" +echo " " +echo -e "${YELLOW}Please DO NOT close / end the script. Getting Axis Parameters on progress...${NC}" +echo " " + +for ap in ${parameter_name[@]} +do + AP=$(grep -w "$ap: " tmcl_ros/config/autogenerated/$1.yaml) + axisparam=${AP##*"[1, "} + axisparam=${axisparam%%", "*} + + if [ "$en_description" = true ] + then + FD=$(echo "$(<$yaml_location)" | sed 's/"\n$ap: "//') + description=${FD%%"$ap: "*} + description=${description##*"$ap:"} + description=$(echo $description|tr -d '\n') + descarray+=("$description") + fi + + OP=`rosservice call $entry "instruction: 'GAP' +instruction_type: $axisparam +motor_num: $2 +value: 0"` + OP=$(echo $OP|tr -d '\n') + output=${OP#*": "} + output=${output%%" "*} + + outputarray+=("$output") +done + +echo " " +echo "===========================================================" +echo -e "${BLUE}Parameters for Motor$2${NC}" +echo "===========================================================" +echo " " + +for ((index=0; index<${#parameter_name[@]}; index++)) +do + echo "${parameter_name[index]}: ${outputarray[index]}" + if [ "$en_description" = true ] + then + echo "${descarray[index]}" + fi + echo " " +done + +# If user wanted to add GAP different command, uncomment below script (remove ": '" and "'" characters) +# And edit "param_name" and "type" to your custom command +: ' +param_name="Custom CMD" +type=0 + +OP=`rosservice call $entry "instruction: 'GAP' +instruction_type: $intruction_type +motor_num: $2 +value: 0"` +OP=$(echo $OP|tr -d '\n') +output=${OP#*": "} +output=${output%%" "*} + +echo "$param_name: $output" +' \ No newline at end of file diff --git a/src/tmcl_ros.cpp b/src/tmcl_ros.cpp index d5fd2fa..134d051 100644 --- a/src/tmcl_ros.cpp +++ b/src/tmcl_ros.cpp @@ -112,6 +112,17 @@ bool TmclROS::getInfo() b_result = false; } + /* Get status flags if it will be published as part of TMC info */ + if (b_result && exec_tmcl_cmd(TMCL_CMD_GAP, ap_cfg[ROW_IDX_StatusFlags][COL_IDX_AP], (tmcl_motor_t) i, &val)) + { + ap_cfg_ext[ROW_IDX_StatusFlags][i] = val; + ROS_DEBUG_STREAM("[" << __func__ << "] Able to get status flags"); + } + else + { + b_result = false; + } + /* Get actual velocity if it will be published as part of TMC info */ if(b_result && ap_cfg_ext[ROW_IDX_CommutationMode][i] > 0 && param_pub_actual_vel[i]) { @@ -524,14 +535,14 @@ bool TmclROS::validate_params(ros::NodeHandle nh) const std::string s_pub_rate_tmc_info = s_node_name + "/pub_rate_tmc_info"; if(!nh.getParam(s_pub_rate_tmc_info, param_pub_rate_tmc_info)) { - param_pub_rate_tmc_info = 1; + param_pub_rate_tmc_info = 10; ROS_WARN_STREAM("Failed to get pub_rate_tmc_info, setting to default value: " << param_pub_rate_tmc_info); } else { - if (param_pub_rate_tmc_info < 0 || param_pub_rate_tmc_info > PUB_RATE_MAX) + if (param_pub_rate_tmc_info < 10 || param_pub_rate_tmc_info > PUB_RATE_MAX) { - param_pub_rate_tmc_info = 1; + param_pub_rate_tmc_info = 10; ROS_WARN_STREAM("Set value to pub_rate_tmc_info is out of range, setting pub_rate_tmc_info value to default: " << param_pub_rate_tmc_info); } } @@ -567,6 +578,45 @@ bool TmclROS::validate_params(ros::NodeHandle nh) // Non-user-configurable ones //////////////////////////////// + /* StatusFlags checks */ + const std::string s_status_flags = s_node_name + "/StatusFlags"; + if(nh.getParam(s_status_flags, ap_cfg[ROW_IDX_StatusFlags])) + { + ROS_INFO_STREAM("Succeeded to get StatusFlags."); + } + else + { + ROS_ERROR_STREAM("Failed to get StatusFlags. Double check your YAMLs. Exiting!"); + return false; + } + const std::string s_status_flags_reg_name = s_node_name + "/StatusFlagsRegName"; + if(nh.getParam(s_status_flags_reg_name, param_status_flags_reg_name)) + { + ROS_INFO_STREAM("Succeeded to get StatusFlagsRegName."); + } + else + { + ROS_ERROR_STREAM("Failed to get StatusFlagsRegName. Double check your YAMLs. Exiting!"); + return false; + } + const std::string s_status_flags_reg_shift = s_node_name + "/StatusFlagsRegShift"; + if(nh.getParam(s_status_flags_reg_shift, param_status_flags_reg_shift)) + { + ROS_INFO_STREAM("Succeeded to get StatusFlagsRegShift."); + } + else + { + ROS_ERROR_STREAM("Failed to get StatusFlagsRegShift. Double check your YAMLs. Exiting!"); + return false; + } + + /* Both StatusFlagsRegName and StatusFlagsRegShift should have the same size of vector*/ + if (param_status_flags_reg_name.size() != param_status_flags_reg_shift.size()) + { + ROS_ERROR_STREAM("StatusFlagsRegName and StatusFlagsRegShift have different size of vector. Double check your YAMLs. Exiting!"); + return false; + } + /* SupplyVoltage checks */ const std::string s_SupplyVoltage = s_node_name + "/SupplyVoltage"; if(nh.getParam(s_SupplyVoltage, ap_cfg[ROW_IDX_SupplyVoltage])) @@ -736,7 +786,7 @@ bool TmclROS::validate_configurable_axis_params(ros::NodeHandle nh) } // Validate TMC configurations - for(index = 7; index < ROW_IDX_MAX; index++) // Configurable setting starts at 7th ROW_IDX_* + for(index = 8; index < ROW_IDX_MAX; index++) // Configurable setting starts at 7th ROW_IDX_* { if(!validate_ap_cfg(nh, tmcl_lut_row_str[index], index)) { diff --git a/src/tmcl_ros_publisher.cpp b/src/tmcl_ros_publisher.cpp index fff284b..c9ec709 100644 --- a/src/tmcl_ros_publisher.cpp +++ b/src/tmcl_ros_publisher.cpp @@ -44,6 +44,12 @@ bool TmclRosPublisher::rosPublishTmcInfo(TmclROS *tmc_obj) { bool result = false; + int32_t status_flag = 0; + uint8_t bin_index = 0; + uint8_t array_index = 0; + std::string binary = " "; + std::string status = " "; + for(int i = 0; i < tmc_obj->param_total_motors; i++) { if(tmc_obj->param_en_motors[i] && tmc_obj->param_en_pub_tmc_info[i]) @@ -54,10 +60,43 @@ bool TmclRosPublisher::rosPublishTmcInfo(TmclROS *tmc_obj) tmc_info_msg.header.frame_id = "tmcl_frame"; tmc_info_msg.interface_name = tmc_obj->param_comm_interface_name; tmc_info_msg.motor_num = i; - tmc_info_msg.board_voltage = tmc_obj->ap_cfg_ext[ROW_IDX_SupplyVoltage][i]; - tmc_info_msg.board_voltage = tmc_info_msg.board_voltage / 10; //converts mV to V + tmc_info_msg.status_flag = tmc_obj->ap_cfg_ext[ROW_IDX_StatusFlags][i]; + status_flag = tmc_info_msg.status_flag; + + //string str; + while(status_flag) + { + if(status_flag & 1) + { + binary+='1'; + } + else + { + binary+='0'; + } + status_flag>>=1; + } + + for (bin_index = 0; bin_index < binary.length(); bin_index++) + { + if(binary[bin_index] == '1') + { + while(array_index < tmc_obj->param_status_flags_reg_shift.size()) + { + if (bin_index == tmc_obj->param_status_flags_reg_shift[array_index]) + { + status += tmc_obj->param_status_flags_reg_name[array_index - 1] + ", "; + array_index = 0; + break; + } + array_index++; + } + } + } + tmc_info_msg.status = status; + if(tmc_obj->param_pub_actual_vel[i]) { @@ -79,7 +118,15 @@ bool TmclRosPublisher::rosPublishTmcInfo(TmclROS *tmc_obj) tmc_info_msg.position = tmc_obj->ap_cfg_ext[ROW_IDX_ActualPosition][i]; //converts units to degrees - tmc_info_msg.position = tmc_info_msg.position / (tmc_obj->ap_cfg_ext[ROW_IDX_EncoderSteps][i] / ANGULAR_FULL_ROTATION); + if (tmc_obj->ap_cfg_ext[ROW_IDX_PositionScalerM][i] > 0) + { + tmc_info_msg.position = tmc_info_msg.position / (tmc_obj->ap_cfg_ext[ROW_IDX_PositionScalerM][i] / ANGULAR_FULL_ROTATION); + } + else + { + tmc_info_msg.position = tmc_info_msg.position / (tmc_obj->ap_cfg_ext[ROW_IDX_EncoderSteps][i] / ANGULAR_FULL_ROTATION); + } + } else { diff --git a/src/tmcl_ros_subscriber.cpp b/src/tmcl_ros_subscriber.cpp index faba336..28ca273 100644 --- a/src/tmcl_ros_subscriber.cpp +++ b/src/tmcl_ros_subscriber.cpp @@ -227,7 +227,15 @@ void cmd_abspos_0_callback(const std_msgs::Int32 msg) int32_t val = msg.data; //convert input(degrees) to unit - convert_const_deg = tmc_obj_sub->ap_cfg_ext[ROW_IDX_EncoderSteps][TMCL_MOTOR_0] / (float) ANGULAR_FULL_ROTATION; + if (tmc_obj_sub->ap_cfg_ext[ROW_IDX_PositionScalerM][TMCL_MOTOR_0] > 0) + { + convert_const_deg = tmc_obj_sub->ap_cfg_ext[ROW_IDX_PositionScalerM][TMCL_MOTOR_0] / (float) ANGULAR_FULL_ROTATION; + } + else + { + convert_const_deg = tmc_obj_sub->ap_cfg_ext[ROW_IDX_EncoderSteps][TMCL_MOTOR_0] / (float) ANGULAR_FULL_ROTATION; + } + unit_val = val * convert_const_deg; ROS_DEBUG_STREAM("Subscriber callback " << __func__ << " entered, received: " << val << " board value: " << unit_val); @@ -255,7 +263,15 @@ void cmd_abspos_1_callback(const std_msgs::Int32 msg) int32_t val = msg.data; //convert input(degrees) to unit - convert_const_deg = tmc_obj_sub->ap_cfg_ext[ROW_IDX_EncoderSteps][TMCL_MOTOR_1] / (float) ANGULAR_FULL_ROTATION; + if (tmc_obj_sub->ap_cfg_ext[ROW_IDX_PositionScalerM][TMCL_MOTOR_1] > 0) + { + convert_const_deg = tmc_obj_sub->ap_cfg_ext[ROW_IDX_PositionScalerM][TMCL_MOTOR_1] / (float) ANGULAR_FULL_ROTATION; + } + else + { + convert_const_deg = tmc_obj_sub->ap_cfg_ext[ROW_IDX_EncoderSteps][TMCL_MOTOR_1] / (float) ANGULAR_FULL_ROTATION; + } + unit_val = val * convert_const_deg; ROS_DEBUG_STREAM("Subscriber callback " << __func__ << " entered, received: " << val << " board value: " << unit_val); @@ -283,7 +299,15 @@ void cmd_abspos_2_callback(const std_msgs::Int32 msg) int32_t val = msg.data; //convert input(degrees) to unit - convert_const_deg = tmc_obj_sub->ap_cfg_ext[ROW_IDX_EncoderSteps][TMCL_MOTOR_2] / (float) ANGULAR_FULL_ROTATION; + if (tmc_obj_sub->ap_cfg_ext[ROW_IDX_PositionScalerM][TMCL_MOTOR_2] > 0) + { + convert_const_deg = tmc_obj_sub->ap_cfg_ext[ROW_IDX_PositionScalerM][TMCL_MOTOR_2] / (float) ANGULAR_FULL_ROTATION; + } + else + { + convert_const_deg = tmc_obj_sub->ap_cfg_ext[ROW_IDX_EncoderSteps][TMCL_MOTOR_2] / (float) ANGULAR_FULL_ROTATION; + } + unit_val = val * convert_const_deg; ROS_DEBUG_STREAM("Subscriber callback " << __func__ << " entered, received: " << val << " board value: " << unit_val); @@ -312,7 +336,15 @@ void cmd_relpos_0_callback(const std_msgs::Int32 msg) int32_t abspos_val = 0; //convert input(degrees) to unit - convert_const_deg = tmc_obj_sub->ap_cfg_ext[ROW_IDX_EncoderSteps][TMCL_MOTOR_0] / (float) ANGULAR_FULL_ROTATION; + if (tmc_obj_sub->ap_cfg_ext[ROW_IDX_PositionScalerM][TMCL_MOTOR_0] > 0) + { + convert_const_deg = tmc_obj_sub->ap_cfg_ext[ROW_IDX_PositionScalerM][TMCL_MOTOR_0] / (float) ANGULAR_FULL_ROTATION; + } + else + { + convert_const_deg = tmc_obj_sub->ap_cfg_ext[ROW_IDX_EncoderSteps][TMCL_MOTOR_0] / (float) ANGULAR_FULL_ROTATION; + } + unit_val = val * convert_const_deg; ROS_DEBUG_STREAM("Subscriber callback " << __func__ << " entered, received: " << val << " board value: " << unit_val); @@ -346,7 +378,15 @@ void cmd_relpos_1_callback(const std_msgs::Int32 msg) int32_t abspos_val = 0; //convert input(degrees) to unit - convert_const_deg = tmc_obj_sub->ap_cfg_ext[ROW_IDX_EncoderSteps][TMCL_MOTOR_1] / (float) ANGULAR_FULL_ROTATION; + if (tmc_obj_sub->ap_cfg_ext[ROW_IDX_PositionScalerM][TMCL_MOTOR_1] > 0) + { + convert_const_deg = tmc_obj_sub->ap_cfg_ext[ROW_IDX_PositionScalerM][TMCL_MOTOR_1] / (float) ANGULAR_FULL_ROTATION; + } + else + { + convert_const_deg = tmc_obj_sub->ap_cfg_ext[ROW_IDX_EncoderSteps][TMCL_MOTOR_1] / (float) ANGULAR_FULL_ROTATION; + } + unit_val = val * convert_const_deg; ROS_DEBUG_STREAM("Subscriber callback " << __func__ << " entered, received: " << val << " board value: " << unit_val); @@ -380,7 +420,15 @@ void cmd_relpos_2_callback(const std_msgs::Int32 msg) int32_t abspos_val = 0; //convert input(degrees) to unit - convert_const_deg = tmc_obj_sub->ap_cfg_ext[ROW_IDX_EncoderSteps][TMCL_MOTOR_2] / (float) ANGULAR_FULL_ROTATION; + if (tmc_obj_sub->ap_cfg_ext[ROW_IDX_PositionScalerM][TMCL_MOTOR_2] > 0) + { + convert_const_deg = tmc_obj_sub->ap_cfg_ext[ROW_IDX_PositionScalerM][TMCL_MOTOR_2] / (float) ANGULAR_FULL_ROTATION; + } + else + { + convert_const_deg = tmc_obj_sub->ap_cfg_ext[ROW_IDX_EncoderSteps][TMCL_MOTOR_2] / (float) ANGULAR_FULL_ROTATION; + } + unit_val = val * convert_const_deg; ROS_DEBUG_STREAM("Subscriber callback " << __func__ << " entered, received: " << val << " board value: " << unit_val);