diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 8f92a60..86fcd07 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package dynamixel_hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.4.7 (2025-06-19) +------------------ +* Added virtual_dxl and support for rcu +* Contributors: Woojin Wie + 1.4.6 (2025-05-30) ------------------ * Changed dynamixel_sdk_TARGETS to dynamixel_sdk_LIBRARIES in target_link_libraries diff --git a/include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp b/include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp index ee73695..5e6db38 100644 --- a/include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp +++ b/include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp @@ -45,6 +45,9 @@ namespace dynamixel_hardware_interface #define SYNC 0 ///< Synchronous communication. #define BULK 1 ///< Bulk communication. +/// @brief Maximum number of retries for communication operations +#define MAX_COMM_RETRIES 5 ///< Maximum number of retries for communication operations + /// @brief Error codes for Dynamixel operations. enum DxlError { @@ -99,7 +102,8 @@ typedef struct */ typedef struct { - uint8_t id; ///< ID of the Dynamixel motor. + uint8_t comm_id; ///< ID of the Dynamixel to be communicated. + std::vector id_arr; ///< IDs of the Dynamixel motors. std::vector item_name; ///< List of control item names. std::vector item_size; ///< Sizes of the control items. std::vector item_addr; ///< Addresses of the control items. @@ -156,6 +160,8 @@ class Dynamixel // direct inform for bulk write std::map direct_info_write_; + std::map comm_id_; + public: explicit Dynamixel(const char * path); ~Dynamixel(); @@ -167,13 +173,13 @@ class Dynamixel // DXL Read Setting DxlError SetDxlReadItems( - uint8_t id, std::vector item_names, + uint8_t id, uint8_t comm_id, std::vector item_names, std::vector> data_vec_ptr); DxlError SetMultiDxlRead(); // DXL Write Setting DxlError SetDxlWriteItems( - uint8_t id, std::vector item_names, + uint8_t id, uint8_t comm_id, std::vector item_names, std::vector> data_vec_ptr); DxlError SetMultiDxlWrite(); @@ -183,7 +189,7 @@ class Dynamixel DxlError WriteMultiDxlData(); // Set Dxl Option - DxlError SetOperatingMode(uint8_t id, uint8_t dynamixel_mode); + // DxlError SetOperatingMode(uint8_t id, uint8_t dynamixel_mode); DxlError DynamixelEnable(std::vector id_arr); DxlError DynamixelDisable(std::vector id_arr); @@ -205,6 +211,12 @@ class Dynamixel static std::string DxlErrorToString(DxlError error_num); + DxlError ReadDxlModelFile(uint8_t id, uint16_t model_num); + + void SetCommId(uint8_t id, uint8_t comm_id) {comm_id_[id] = comm_id;} + + DxlError InitTorqueStates(std::vector id_arr, bool disable_torque = false); + private: bool checkReadType(); bool checkWriteType(); @@ -237,8 +249,9 @@ class Dynamixel // Read - Data Processing DxlError ProcessReadData( - uint8_t id, + uint8_t comm_id, uint16_t indirect_addr, + const std::vector & id_arr, const std::vector & item_names, const std::vector & item_sizes, const std::vector> & data_ptrs, diff --git a/include/dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp b/include/dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp index d0abe52..9770e83 100644 --- a/include/dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp +++ b/include/dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp @@ -47,6 +47,7 @@ typedef struct int32_t value_of_max_radian_position; int32_t value_of_min_radian_position; uint16_t model_num; + double velocity_unit; std::vector item; } DxlInfo; @@ -72,24 +73,25 @@ class DynamixelInfo void ReadDxlModelFile(uint8_t id, uint16_t model_num); bool GetDxlControlItem(uint8_t id, std::string item_name, uint16_t & addr, uint8_t & size); bool CheckDxlControlItem(uint8_t id, std::string item_name); - bool GetDxlTypeInfo( - uint8_t id, - int32_t & value_of_zero_radian_position, - int32_t & value_of_max_radian_position, - int32_t & value_of_min_radian_position, - double & min_radian, - double & max_radian, - double & torque_constant); + // bool GetDxlTypeInfo( + // uint8_t id, + // int32_t & value_of_zero_radian_position, + // int32_t & value_of_max_radian_position, + // int32_t & value_of_min_radian_position, + // double & min_radian, + // double & max_radian, + // double & torque_constant, + // double & velocity_unit); int32_t ConvertRadianToValue(uint8_t id, double radian); double ConvertValueToRadian(uint8_t id, int32_t value); inline int16_t ConvertEffortToCurrent(uint8_t id, double effort) {return static_cast(effort / dxl_info_[id].torque_constant);} inline double ConvertCurrentToEffort(uint8_t id, int16_t current) {return static_cast(current * dxl_info_[id].torque_constant);} - inline double ConvertValueRPMToVelocityRPS(int32_t value_rpm) - {return static_cast(value_rpm * 0.01 / 60.0 * 2.0 * M_PI);} - inline int32_t ConvertVelocityRPSToValueRPM(double vel_rps) - {return static_cast(vel_rps * 100.0 * 60.0 / 2.0 / M_PI);} + inline double ConvertValueRPMToVelocityRPS(uint8_t id, int32_t value_rpm) + {return static_cast(value_rpm * dxl_info_[id].velocity_unit / 60.0 * 2.0 * M_PI);} + inline int32_t ConvertVelocityRPSToValueRPM(uint8_t id, double vel_rps) + {return static_cast(vel_rps / dxl_info_[id].velocity_unit * 60.0 / 2.0 / M_PI);} }; } // namespace dynamixel_hardware_interface diff --git a/include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp b/include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp index 44c5ca8..9006dad 100644 --- a/include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp +++ b/include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp @@ -62,6 +62,7 @@ constexpr char HW_IF_TORQUE_ENABLE[] = "torque_enable"; typedef struct HandlerVarType_ { uint8_t id; /**< ID of the Dynamixel component. */ + uint8_t comm_id; /**< ID of the Dynamixel to be communicated. */ std::string name; /**< Name of the component. */ std::vector interface_name_vec; /**< Vector of interface names. */ std::vector> value_ptr_vec; /**< Vector interface values. */ @@ -227,6 +228,7 @@ class DynamixelHardware : public std::string port_name_; std::string baud_rate_; std::vector dxl_id_; + std::vector virtual_dxl_id_; std::vector sensor_id_; std::map sensor_item_; @@ -245,6 +247,7 @@ class DynamixelHardware : public std::vector hdl_sensor_states_; ///// handler controller variable + std::vector hdl_gpio_controller_states_; std::vector hdl_gpio_controller_commands_; bool is_set_hdl_{false}; @@ -262,15 +265,6 @@ class DynamixelHardware : public */ bool initItems(const std::string & type_filter); - /** - * @brief Helper function to retry writing an item to a Dynamixel device. - * @param id The ID of the Dynamixel device. - * @param item_name The name of the item to write. - * @param value The value to write. - * @return True if write was successful, false if timeout occurred. - */ - bool retryWriteItem(uint8_t id, const std::string & item_name, uint32_t value); - /** * @brief Initializes Dynamixel items. * @return True if initialization was successful, false otherwise. @@ -362,7 +356,7 @@ class DynamixelHardware : public size_t outer_size, size_t inner_size, std::vector & outer_handlers, - const std::vector & inner_handlers, + std::vector & inner_handlers, double ** matrix, const std::unordered_map> & iface_map, const std::string & conversion_iface = "", diff --git a/package.xml b/package.xml index 85c7781..a30a5cd 100644 --- a/package.xml +++ b/package.xml @@ -2,7 +2,7 @@ dynamixel_hardware_interface - 1.4.6 + 1.4.7 ROS 2 package providing a hardware interface for controlling Dynamixel motors via the ROS 2 control framework. diff --git a/param/dxl_model/2xc430_w250.model b/param/dxl_model/2xc430_w250.model index 59e7d1f..6a1f335 100644 --- a/param/dxl_model/2xc430_w250.model +++ b/param/dxl_model/2xc430_w250.model @@ -5,6 +5,7 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.229 [control table] Address Size Data Name diff --git a/param/dxl_model/2xl430_w250.model b/param/dxl_model/2xl430_w250.model index 59e7d1f..6a1f335 100644 --- a/param/dxl_model/2xl430_w250.model +++ b/param/dxl_model/2xl430_w250.model @@ -5,6 +5,7 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.229 [control table] Address Size Data Name diff --git a/param/dxl_model/dynamixel.model b/param/dxl_model/dynamixel.model index 2d9354a..3cdc33b 100644 --- a/param/dxl_model/dynamixel.model +++ b/param/dxl_model/dynamixel.model @@ -48,3 +48,10 @@ Number Name 230 omy_end.model 536 sensorxel_joy.model 600 sensorxel_joy.model +602 ffw_g10_rcu.model +620 ffw_sg2_steer_1.model +621 ffw_sg2_steer_2.model +622 ffw_sg2_steer_3.model +623 ffw_sg2_drive_1.model +624 ffw_sg2_drive_2.model +625 ffw_sg2_drive_3.model diff --git a/param/dxl_model/ffw_g10_rcu.model b/param/dxl_model/ffw_g10_rcu.model new file mode 100644 index 0000000..6a2c4e4 --- /dev/null +++ b/param/dxl_model/ffw_g10_rcu.model @@ -0,0 +1,3228 @@ +[control table] +Address Size Data Name +0 2 Model_Number +2 1 Robot_Generation +3 1 Board_Number +4 1 Board_Version +5 1 Firmware_Version_Major +6 1 Firmware_Version_Minor +7 1 ID +8 1 Baud Rate (Bus) +10 1 Bootloader_Version +12 4 Error_Code +17 1 Operation_Command +18 1 Operation_Busy +20 4 Micros +24 4 Millis +28 4 Sec_Since_Power_On +32 4 Sec_Since_Reset +40 1 Estop_Input +41 1 Remote_Connected +42 1 System_State +43 1 Board_Temperature +50 1 Host_Connection +51 1 CLI_Channel +52 1 Bypass_channel +60 1 Orin_Power_State +61 1 Orin_Power_Control +70 4 RTC_Set_Date +74 4 RTC_Set_Time +90 1 Drive_Base_Type +91 1 Linear_Velocity_Limit +92 2 Motor_Wheel_Diameter +94 4 Motor_Velocity_Limit +100 1 Motor1_Connection_Status +101 1 Motor1_Connection_Status +102 1 Motor1_Connection_Status +104 1 Swerve_DXL1_Connection_Status +105 1 Swerve_DXL2_Connection_Status +106 1 Swerve_DXL3_Connection_Status +110 1024 Motor1_Control_Table +110 2 Motor1_Model Number +112 4 Motor1_Model Information +116 1 Motor1_Firmware Version +117 1 Motor1_ID +118 1 Motor1_Bus Watchdog +120 1 Motor1_Virtual ID +121 1 Motor1_Protocol Type +122 1 Motor1_Baud Rate (Bus) +123 1 Motor1_Return Delay Time +124 1 Motor1_Bit Rate (CAN) +125 1 Motor1_Status Return Level +126 1 Motor1_Registered Instruction +127 1 Motor1_Dump Control +128 2 Motor1_Dump Time +130 2 Motor1_Dump Step Time +132 2 Motor1_Dump Interval Time +134 1 Motor1_Dump Item +138 4 Motor1_Build Date +142 1 Motor1_Drive Mode +143 1 Motor1_Operating Mode +144 1 Motor1_Startup Configuration +150 4 Motor1_In-Position Threshold +154 4 Motor1_Following Error Threshold +158 4 Motor1_Moving Threshold +162 4 Motor1_Homing Offset +166 1 Motor1_Temperature Limit +167 1 Motor1_Temperature Limit (Motor) +170 2 Motor1_Max Voltage Limit +172 2 Motor1_Min Voltage Limit +174 2 Motor1_PWM Limit +176 2 Motor1_Current Limit +178 4 Motor1_Acceleration Limit +182 4 Motor1_Velocity Limit +186 4 Motor1_Max Position Limit +190 4 Motor1_Min Position Limit +249 1 Motor1_Controller State +292 1 Motor1_Error Code +293 1 Motor1_Error Code History 1 +294 1 Motor1_Error Code History 2 +295 1 Motor1_Error Code History 3 +296 1 Motor1_Error Code History 4 +297 1 Motor1_Error Code History 5 +298 1 Motor1_Error Code History 6 +299 1 Motor1_Error Code History 7 +300 1 Motor1_Error Code History 8 +301 1 Motor1_Error Code History 9 +302 1 Motor1_Error Code History 10 +303 1 Motor1_Error Code History 11 +304 1 Motor1_Error Code History 12 +305 1 Motor1_Error Code History 13 +306 1 Motor1_Error Code History 14 +307 1 Motor1_Error Code History 15 +308 1 Motor1_Error Code History 16 +366 2 Motor1_Indirect Address 1 +368 2 Motor1_Indirect Address 2 +370 2 Motor1_Indirect Address 3 +372 2 Motor1_Indirect Address 4 +374 2 Motor1_Indirect Address 5 +376 2 Motor1_Indirect Address 6 +378 2 Motor1_Indirect Address 7 +380 2 Motor1_Indirect Address 8 +382 2 Motor1_Indirect Address 9 +384 2 Motor1_Indirect Address 10 +386 2 Motor1_Indirect Address 11 +388 2 Motor1_Indirect Address 12 +390 2 Motor1_Indirect Address 13 +392 2 Motor1_Indirect Address 14 +394 2 Motor1_Indirect Address 15 +396 2 Motor1_Indirect Address 16 +398 2 Motor1_Indirect Address 17 +400 2 Motor1_Indirect Address 18 +402 2 Motor1_Indirect Address 19 +404 2 Motor1_Indirect Address 20 +406 2 Motor1_Indirect Address 21 +408 2 Motor1_Indirect Address 22 +410 2 Motor1_Indirect Address 23 +412 2 Motor1_Indirect Address 24 +414 2 Motor1_Indirect Address 25 +416 2 Motor1_Indirect Address 26 +418 2 Motor1_Indirect Address 27 +420 2 Motor1_Indirect Address 28 +422 2 Motor1_Indirect Address 29 +424 2 Motor1_Indirect Address 30 +426 2 Motor1_Indirect Address 31 +428 2 Motor1_Indirect Address 32 +430 2 Motor1_Indirect Address 33 +432 2 Motor1_Indirect Address 34 +434 2 Motor1_Indirect Address 35 +436 2 Motor1_Indirect Address 36 +438 2 Motor1_Indirect Address 37 +440 2 Motor1_Indirect Address 38 +442 2 Motor1_Indirect Address 39 +444 2 Motor1_Indirect Address 40 +446 2 Motor1_Indirect Address 41 +448 2 Motor1_Indirect Address 42 +450 2 Motor1_Indirect Address 43 +452 2 Motor1_Indirect Address 44 +454 2 Motor1_Indirect Address 45 +456 2 Motor1_Indirect Address 46 +458 2 Motor1_Indirect Address 47 +460 2 Motor1_Indirect Address 48 +462 2 Motor1_Indirect Address 49 +464 2 Motor1_Indirect Address 50 +466 2 Motor1_Indirect Address 51 +468 2 Motor1_Indirect Address 52 +470 2 Motor1_Indirect Address 53 +472 2 Motor1_Indirect Address 54 +474 2 Motor1_Indirect Address 55 +476 2 Motor1_Indirect Address 56 +478 2 Motor1_Indirect Address 57 +480 2 Motor1_Indirect Address 58 +482 2 Motor1_Indirect Address 59 +484 2 Motor1_Indirect Address 60 +486 2 Motor1_Indirect Address 61 +488 2 Motor1_Indirect Address 62 +490 2 Motor1_Indirect Address 63 +492 2 Motor1_Indirect Address 64 +494 2 Motor1_Indirect Address 65 +496 2 Motor1_Indirect Address 66 +498 2 Motor1_Indirect Address 67 +500 2 Motor1_Indirect Address 68 +502 2 Motor1_Indirect Address 69 +504 2 Motor1_Indirect Address 70 +506 2 Motor1_Indirect Address 71 +508 2 Motor1_Indirect Address 72 +510 2 Motor1_Indirect Address 73 +512 2 Motor1_Indirect Address 74 +514 2 Motor1_Indirect Address 75 +516 2 Motor1_Indirect Address 76 +518 2 Motor1_Indirect Address 77 +520 2 Motor1_Indirect Address 78 +522 2 Motor1_Indirect Address 79 +524 2 Motor1_Indirect Address 80 +526 2 Motor1_Indirect Address 81 +528 2 Motor1_Indirect Address 82 +530 2 Motor1_Indirect Address 83 +532 2 Motor1_Indirect Address 84 +534 2 Motor1_Indirect Address 85 +536 2 Motor1_Indirect Address 86 +538 2 Motor1_Indirect Address 87 +540 2 Motor1_Indirect Address 88 +542 2 Motor1_Indirect Address 89 +544 2 Motor1_Indirect Address 90 +546 2 Motor1_Indirect Address 91 +548 2 Motor1_Indirect Address 92 +550 2 Motor1_Indirect Address 93 +552 2 Motor1_Indirect Address 94 +554 2 Motor1_Indirect Address 95 +556 2 Motor1_Indirect Address 96 +558 2 Motor1_Indirect Address 97 +560 2 Motor1_Indirect Address 98 +562 2 Motor1_Indirect Address 99 +564 2 Motor1_Indirect Address 100 +566 2 Motor1_Indirect Address 101 +568 2 Motor1_Indirect Address 102 +570 2 Motor1_Indirect Address 103 +572 2 Motor1_Indirect Address 104 +574 2 Motor1_Indirect Address 105 +576 2 Motor1_Indirect Address 106 +578 2 Motor1_Indirect Address 107 +580 2 Motor1_Indirect Address 108 +582 2 Motor1_Indirect Address 109 +584 2 Motor1_Indirect Address 110 +586 2 Motor1_Indirect Address 111 +588 2 Motor1_Indirect Address 112 +590 2 Motor1_Indirect Address 113 +592 2 Motor1_Indirect Address 114 +594 2 Motor1_Indirect Address 115 +596 2 Motor1_Indirect Address 116 +598 2 Motor1_Indirect Address 117 +600 2 Motor1_Indirect Address 118 +602 2 Motor1_Indirect Address 119 +604 2 Motor1_Indirect Address 120 +606 2 Motor1_Indirect Address 121 +608 2 Motor1_Indirect Address 122 +610 2 Motor1_Indirect Address 123 +612 2 Motor1_Indirect Address 124 +614 2 Motor1_Indirect Address 125 +616 2 Motor1_Indirect Address 126 +618 2 Motor1_Indirect Address 127 +620 2 Motor1_Indirect Address 128 +622 1 Motor1_Torque Enable +623 1 Motor1_LED +624 1 Motor1_Gain Save +662 4 Motor1_Current I Gain +666 4 Motor1_Current P Gain +670 4 Motor1_Velocity I Gain +674 4 Motor1_Velocity P Gain +678 4 Motor1_Velocity FF Gain +682 4 Motor1_Position D Gain +686 4 Motor1_Position I Gain +690 4 Motor1_Position P Gain +694 4 Motor1_Position FF Gain +698 4 Motor1_Profile Acceleration +702 4 Motor1_Profile Velocity +706 4 Motor1_Profile Acceleration Time +710 4 Motor1_Profile Time +714 2 Motor1_PWM Offset +716 2 Motor1_Current Offset +718 4 Motor1_Velocity Offset +722 2 Motor1_Goal PWM +724 2 Motor1_Goal Current +726 4 Motor1_Goal Velocity +730 4 Motor1_Goal Position +734 4 Motor1_Goal Position (H) +739 1 Motor1_Moving Status +740 2 Motor1_Realtime Tick +742 2 Motor1_Present PWM +744 2 Motor1_Present Current +746 4 Motor1_Present Velocity +750 4 Motor1_Present Position +754 4 Motor1_Present Position (H) +758 4 Motor1_Position Trajectory +762 4 Motor1_Velocity Trajectory +766 2 Motor1_Present Input Voltage +768 1 Motor1_Present Temperature +769 1 Motor1_Present Temperature (Motor) +798 2 Motor1_Voltage A +800 2 Motor1_Voltage B +802 2 Motor1_Voltage C +804 2 Motor1_Present Impact +824 2 Motor1_Current A +826 2 Motor1_Current B +828 2 Motor1_Current C +830 4 Motor1_Present Single Position +834 4 Motor1_Present Multi Position +838 2 Motor1_Elec. Angle +1001 1 Motor1_Backup Ready +1006 1 Motor1_Indirect Data 1 +1007 1 Motor1_Indirect Data 2 +1008 1 Motor1_Indirect Data 3 +1009 1 Motor1_Indirect Data 4 +1010 1 Motor1_Indirect Data 5 +1011 1 Motor1_Indirect Data 6 +1012 1 Motor1_Indirect Data 7 +1013 1 Motor1_Indirect Data 8 +1014 1 Motor1_Indirect Data 9 +1015 1 Motor1_Indirect Data 10 +1016 1 Motor1_Indirect Data 11 +1017 1 Motor1_Indirect Data 12 +1018 1 Motor1_Indirect Data 13 +1019 1 Motor1_Indirect Data 14 +1020 1 Motor1_Indirect Data 15 +1021 1 Motor1_Indirect Data 16 +1022 1 Motor1_Indirect Data 17 +1023 1 Motor1_Indirect Data 18 +1024 1 Motor1_Indirect Data 19 +1025 1 Motor1_Indirect Data 20 +1026 1 Motor1_Indirect Data 21 +1027 1 Motor1_Indirect Data 22 +1028 1 Motor1_Indirect Data 23 +1029 1 Motor1_Indirect Data 24 +1030 1 Motor1_Indirect Data 25 +1031 1 Motor1_Indirect Data 26 +1032 1 Motor1_Indirect Data 27 +1033 1 Motor1_Indirect Data 28 +1034 1 Motor1_Indirect Data 29 +1035 1 Motor1_Indirect Data 30 +1036 1 Motor1_Indirect Data 31 +1037 1 Motor1_Indirect Data 32 +1038 1 Motor1_Indirect Data 33 +1039 1 Motor1_Indirect Data 34 +1040 1 Motor1_Indirect Data 35 +1041 1 Motor1_Indirect Data 36 +1042 1 Motor1_Indirect Data 37 +1043 1 Motor1_Indirect Data 38 +1044 1 Motor1_Indirect Data 39 +1045 1 Motor1_Indirect Data 40 +1046 1 Motor1_Indirect Data 41 +1047 1 Motor1_Indirect Data 42 +1048 1 Motor1_Indirect Data 43 +1049 1 Motor1_Indirect Data 44 +1050 1 Motor1_Indirect Data 45 +1051 1 Motor1_Indirect Data 46 +1052 1 Motor1_Indirect Data 47 +1053 1 Motor1_Indirect Data 48 +1054 1 Motor1_Indirect Data 49 +1055 1 Motor1_Indirect Data 50 +1056 1 Motor1_Indirect Data 51 +1057 1 Motor1_Indirect Data 52 +1058 1 Motor1_Indirect Data 53 +1059 1 Motor1_Indirect Data 54 +1060 1 Motor1_Indirect Data 55 +1061 1 Motor1_Indirect Data 56 +1062 1 Motor1_Indirect Data 57 +1063 1 Motor1_Indirect Data 58 +1064 1 Motor1_Indirect Data 59 +1065 1 Motor1_Indirect Data 60 +1066 1 Motor1_Indirect Data 61 +1067 1 Motor1_Indirect Data 62 +1068 1 Motor1_Indirect Data 63 +1069 1 Motor1_Indirect Data 64 +1070 1 Motor1_Indirect Data 65 +1071 1 Motor1_Indirect Data 66 +1072 1 Motor1_Indirect Data 67 +1073 1 Motor1_Indirect Data 68 +1074 1 Motor1_Indirect Data 69 +1075 1 Motor1_Indirect Data 70 +1076 1 Motor1_Indirect Data 71 +1077 1 Motor1_Indirect Data 72 +1078 1 Motor1_Indirect Data 73 +1079 1 Motor1_Indirect Data 74 +1080 1 Motor1_Indirect Data 75 +1081 1 Motor1_Indirect Data 76 +1082 1 Motor1_Indirect Data 77 +1083 1 Motor1_Indirect Data 78 +1084 1 Motor1_Indirect Data 79 +1085 1 Motor1_Indirect Data 80 +1086 1 Motor1_Indirect Data 81 +1087 1 Motor1_Indirect Data 82 +1088 1 Motor1_Indirect Data 83 +1089 1 Motor1_Indirect Data 84 +1090 1 Motor1_Indirect Data 85 +1091 1 Motor1_Indirect Data 86 +1092 1 Motor1_Indirect Data 87 +1093 1 Motor1_Indirect Data 88 +1094 1 Motor1_Indirect Data 89 +1095 1 Motor1_Indirect Data 90 +1096 1 Motor1_Indirect Data 91 +1097 1 Motor1_Indirect Data 92 +1098 1 Motor1_Indirect Data 93 +1099 1 Motor1_Indirect Data 94 +1100 1 Motor1_Indirect Data 95 +1101 1 Motor1_Indirect Data 96 +1102 1 Motor1_Indirect Data 97 +1103 1 Motor1_Indirect Data 98 +1104 1 Motor1_Indirect Data 99 +1105 1 Motor1_Indirect Data 100 +1106 1 Motor1_Indirect Data 101 +1107 1 Motor1_Indirect Data 102 +1108 1 Motor1_Indirect Data 103 +1109 1 Motor1_Indirect Data 104 +1110 1 Motor1_Indirect Data 105 +1111 1 Motor1_Indirect Data 106 +1112 1 Motor1_Indirect Data 107 +1113 1 Motor1_Indirect Data 108 +1114 1 Motor1_Indirect Data 109 +1115 1 Motor1_Indirect Data 110 +1116 1 Motor1_Indirect Data 111 +1117 1 Motor1_Indirect Data 112 +1118 1 Motor1_Indirect Data 113 +1119 1 Motor1_Indirect Data 114 +1120 1 Motor1_Indirect Data 115 +1121 1 Motor1_Indirect Data 116 +1122 1 Motor1_Indirect Data 117 +1123 1 Motor1_Indirect Data 118 +1124 1 Motor1_Indirect Data 119 +1125 1 Motor1_Indirect Data 120 +1126 1 Motor1_Indirect Data 121 +1127 1 Motor1_Indirect Data 122 +1128 1 Motor1_Indirect Data 123 +1129 1 Motor1_Indirect Data 124 +1130 1 Motor1_Indirect Data 125 +1131 1 Motor1_Indirect Data 126 +1132 1 Motor1_Indirect Data 127 +1133 1 Motor1_Indirect Data 128 +1134 1024 Motor2_Control_Table +1134 2 Motor2_Model Number +1136 4 Motor2_Model Information +1140 1 Motor2_Firmware Version +1141 1 Motor2_ID +1142 1 Motor2_Bus Watchdog +1144 1 Motor2_Virtual ID +1145 1 Motor2_Protocol Type +1146 1 Motor2_Baud Rate (Bus) +1147 1 Motor2_Return Delay Time +1148 1 Motor2_Bit Rate (CAN) +1149 1 Motor2_Status Return Level +1150 1 Motor2_Registered Instruction +1151 1 Motor2_Dump Control +1152 2 Motor2_Dump Time +1154 2 Motor2_Dump Step Time +1156 2 Motor2_Dump Interval Time +1158 1 Motor2_Dump Item +1162 4 Motor2_Build Date +1166 1 Motor2_Drive Mode +1167 1 Motor2_Operating Mode +1168 1 Motor2_Startup Configuration +1174 4 Motor2_In-Position Threshold +1178 4 Motor2_Following Error Threshold +1182 4 Motor2_Moving Threshold +1186 4 Motor2_Homing Offset +1190 1 Motor2_Temperature Limit +1191 1 Motor2_Temperature Limit (Motor) +1194 2 Motor2_Max Voltage Limit +1196 2 Motor2_Min Voltage Limit +1198 2 Motor2_PWM Limit +1200 2 Motor2_Current Limit +1202 4 Motor2_Acceleration Limit +1206 4 Motor2_Velocity Limit +1210 4 Motor2_Max Position Limit +1214 4 Motor2_Min Position Limit +1273 1 Motor2_Controller State +1316 1 Motor2_Error Code +1317 1 Motor2_Error Code History 1 +1318 1 Motor2_Error Code History 2 +1319 1 Motor2_Error Code History 3 +1320 1 Motor2_Error Code History 4 +1321 1 Motor2_Error Code History 5 +1322 1 Motor2_Error Code History 6 +1323 1 Motor2_Error Code History 7 +1324 1 Motor2_Error Code History 8 +1325 1 Motor2_Error Code History 9 +1326 1 Motor2_Error Code History 10 +1327 1 Motor2_Error Code History 11 +1328 1 Motor2_Error Code History 12 +1329 1 Motor2_Error Code History 13 +1330 1 Motor2_Error Code History 14 +1331 1 Motor2_Error Code History 15 +1332 1 Motor2_Error Code History 16 +1390 2 Motor2_Indirect Address 1 +1392 2 Motor2_Indirect Address 2 +1394 2 Motor2_Indirect Address 3 +1396 2 Motor2_Indirect Address 4 +1398 2 Motor2_Indirect Address 5 +1400 2 Motor2_Indirect Address 6 +1402 2 Motor2_Indirect Address 7 +1404 2 Motor2_Indirect Address 8 +1406 2 Motor2_Indirect Address 9 +1408 2 Motor2_Indirect Address 10 +1410 2 Motor2_Indirect Address 11 +1412 2 Motor2_Indirect Address 12 +1414 2 Motor2_Indirect Address 13 +1416 2 Motor2_Indirect Address 14 +1418 2 Motor2_Indirect Address 15 +1420 2 Motor2_Indirect Address 16 +1422 2 Motor2_Indirect Address 17 +1424 2 Motor2_Indirect Address 18 +1426 2 Motor2_Indirect Address 19 +1428 2 Motor2_Indirect Address 20 +1430 2 Motor2_Indirect Address 21 +1432 2 Motor2_Indirect Address 22 +1434 2 Motor2_Indirect Address 23 +1436 2 Motor2_Indirect Address 24 +1438 2 Motor2_Indirect Address 25 +1440 2 Motor2_Indirect Address 26 +1442 2 Motor2_Indirect Address 27 +1444 2 Motor2_Indirect Address 28 +1446 2 Motor2_Indirect Address 29 +1448 2 Motor2_Indirect Address 30 +1450 2 Motor2_Indirect Address 31 +1452 2 Motor2_Indirect Address 32 +1454 2 Motor2_Indirect Address 33 +1456 2 Motor2_Indirect Address 34 +1458 2 Motor2_Indirect Address 35 +1460 2 Motor2_Indirect Address 36 +1462 2 Motor2_Indirect Address 37 +1464 2 Motor2_Indirect Address 38 +1466 2 Motor2_Indirect Address 39 +1468 2 Motor2_Indirect Address 40 +1470 2 Motor2_Indirect Address 41 +1472 2 Motor2_Indirect Address 42 +1474 2 Motor2_Indirect Address 43 +1476 2 Motor2_Indirect Address 44 +1478 2 Motor2_Indirect Address 45 +1480 2 Motor2_Indirect Address 46 +1482 2 Motor2_Indirect Address 47 +1484 2 Motor2_Indirect Address 48 +1486 2 Motor2_Indirect Address 49 +1488 2 Motor2_Indirect Address 50 +1490 2 Motor2_Indirect Address 51 +1492 2 Motor2_Indirect Address 52 +1494 2 Motor2_Indirect Address 53 +1496 2 Motor2_Indirect Address 54 +1498 2 Motor2_Indirect Address 55 +1500 2 Motor2_Indirect Address 56 +1502 2 Motor2_Indirect Address 57 +1504 2 Motor2_Indirect Address 58 +1506 2 Motor2_Indirect Address 59 +1508 2 Motor2_Indirect Address 60 +1510 2 Motor2_Indirect Address 61 +1512 2 Motor2_Indirect Address 62 +1514 2 Motor2_Indirect Address 63 +1516 2 Motor2_Indirect Address 64 +1518 2 Motor2_Indirect Address 65 +1520 2 Motor2_Indirect Address 66 +1522 2 Motor2_Indirect Address 67 +1524 2 Motor2_Indirect Address 68 +1526 2 Motor2_Indirect Address 69 +1528 2 Motor2_Indirect Address 70 +1530 2 Motor2_Indirect Address 71 +1532 2 Motor2_Indirect Address 72 +1534 2 Motor2_Indirect Address 73 +1536 2 Motor2_Indirect Address 74 +1538 2 Motor2_Indirect Address 75 +1540 2 Motor2_Indirect Address 76 +1542 2 Motor2_Indirect Address 77 +1544 2 Motor2_Indirect Address 78 +1546 2 Motor2_Indirect Address 79 +1548 2 Motor2_Indirect Address 80 +1550 2 Motor2_Indirect Address 81 +1552 2 Motor2_Indirect Address 82 +1554 2 Motor2_Indirect Address 83 +1556 2 Motor2_Indirect Address 84 +1558 2 Motor2_Indirect Address 85 +1560 2 Motor2_Indirect Address 86 +1562 2 Motor2_Indirect Address 87 +1564 2 Motor2_Indirect Address 88 +1566 2 Motor2_Indirect Address 89 +1568 2 Motor2_Indirect Address 90 +1570 2 Motor2_Indirect Address 91 +1572 2 Motor2_Indirect Address 92 +1574 2 Motor2_Indirect Address 93 +1576 2 Motor2_Indirect Address 94 +1578 2 Motor2_Indirect Address 95 +1580 2 Motor2_Indirect Address 96 +1582 2 Motor2_Indirect Address 97 +1584 2 Motor2_Indirect Address 98 +1586 2 Motor2_Indirect Address 99 +1588 2 Motor2_Indirect Address 100 +1590 2 Motor2_Indirect Address 101 +1592 2 Motor2_Indirect Address 102 +1594 2 Motor2_Indirect Address 103 +1596 2 Motor2_Indirect Address 104 +1598 2 Motor2_Indirect Address 105 +1600 2 Motor2_Indirect Address 106 +1602 2 Motor2_Indirect Address 107 +1604 2 Motor2_Indirect Address 108 +1606 2 Motor2_Indirect Address 109 +1608 2 Motor2_Indirect Address 110 +1610 2 Motor2_Indirect Address 111 +1612 2 Motor2_Indirect Address 112 +1614 2 Motor2_Indirect Address 113 +1616 2 Motor2_Indirect Address 114 +1618 2 Motor2_Indirect Address 115 +1620 2 Motor2_Indirect Address 116 +1622 2 Motor2_Indirect Address 117 +1624 2 Motor2_Indirect Address 118 +1626 2 Motor2_Indirect Address 119 +1628 2 Motor2_Indirect Address 120 +1630 2 Motor2_Indirect Address 121 +1632 2 Motor2_Indirect Address 122 +1634 2 Motor2_Indirect Address 123 +1636 2 Motor2_Indirect Address 124 +1638 2 Motor2_Indirect Address 125 +1640 2 Motor2_Indirect Address 126 +1642 2 Motor2_Indirect Address 127 +1644 2 Motor2_Indirect Address 128 +1646 1 Motor2_Torque Enable +1647 1 Motor2_LED +1648 1 Motor2_Gain Save +1686 4 Motor2_Current I Gain +1690 4 Motor2_Current P Gain +1694 4 Motor2_Velocity I Gain +1698 4 Motor2_Velocity P Gain +1702 4 Motor2_Velocity FF Gain +1706 4 Motor2_Position D Gain +1710 4 Motor2_Position I Gain +1714 4 Motor2_Position P Gain +1718 4 Motor2_Position FF Gain +1722 4 Motor2_Profile Acceleration +1726 4 Motor2_Profile Velocity +1730 4 Motor2_Profile Acceleration Time +1734 4 Motor2_Profile Time +1738 2 Motor2_PWM Offset +1740 2 Motor2_Current Offset +1742 4 Motor2_Velocity Offset +1746 2 Motor2_Goal PWM +1748 2 Motor2_Goal Current +1750 4 Motor2_Goal Velocity +1754 4 Motor2_Goal Position +1758 4 Motor2_Goal Position (H) +1763 1 Motor2_Moving Status +1764 2 Motor2_Realtime Tick +1766 2 Motor2_Present PWM +1768 2 Motor2_Present Current +1770 4 Motor2_Present Velocity +1774 4 Motor2_Present Position +1778 4 Motor2_Present Position (H) +1782 4 Motor2_Position Trajectory +1786 4 Motor2_Velocity Trajectory +1790 2 Motor2_Present Input Voltage +1792 1 Motor2_Present Temperature +1793 1 Motor2_Present Temperature (Motor) +1822 2 Motor2_Voltage A +1824 2 Motor2_Voltage B +1826 2 Motor2_Voltage C +1828 2 Motor2_Present Impact +1848 2 Motor2_Current A +1850 2 Motor2_Current B +1852 2 Motor2_Current C +1854 4 Motor2_Present Single Position +1858 4 Motor2_Present Multi Position +1862 2 Motor2_Elec. Angle +2025 1 Motor2_Backup Ready +2030 1 Motor2_Indirect Data 1 +2031 1 Motor2_Indirect Data 2 +2032 1 Motor2_Indirect Data 3 +2033 1 Motor2_Indirect Data 4 +2034 1 Motor2_Indirect Data 5 +2035 1 Motor2_Indirect Data 6 +2036 1 Motor2_Indirect Data 7 +2037 1 Motor2_Indirect Data 8 +2038 1 Motor2_Indirect Data 9 +2039 1 Motor2_Indirect Data 10 +2040 1 Motor2_Indirect Data 11 +2041 1 Motor2_Indirect Data 12 +2042 1 Motor2_Indirect Data 13 +2043 1 Motor2_Indirect Data 14 +2044 1 Motor2_Indirect Data 15 +2045 1 Motor2_Indirect Data 16 +2046 1 Motor2_Indirect Data 17 +2047 1 Motor2_Indirect Data 18 +2048 1 Motor2_Indirect Data 19 +2049 1 Motor2_Indirect Data 20 +2050 1 Motor2_Indirect Data 21 +2051 1 Motor2_Indirect Data 22 +2052 1 Motor2_Indirect Data 23 +2053 1 Motor2_Indirect Data 24 +2054 1 Motor2_Indirect Data 25 +2055 1 Motor2_Indirect Data 26 +2056 1 Motor2_Indirect Data 27 +2057 1 Motor2_Indirect Data 28 +2058 1 Motor2_Indirect Data 29 +2059 1 Motor2_Indirect Data 30 +2060 1 Motor2_Indirect Data 31 +2061 1 Motor2_Indirect Data 32 +2062 1 Motor2_Indirect Data 33 +2063 1 Motor2_Indirect Data 34 +2064 1 Motor2_Indirect Data 35 +2065 1 Motor2_Indirect Data 36 +2066 1 Motor2_Indirect Data 37 +2067 1 Motor2_Indirect Data 38 +2068 1 Motor2_Indirect Data 39 +2069 1 Motor2_Indirect Data 40 +2070 1 Motor2_Indirect Data 41 +2071 1 Motor2_Indirect Data 42 +2072 1 Motor2_Indirect Data 43 +2073 1 Motor2_Indirect Data 44 +2074 1 Motor2_Indirect Data 45 +2075 1 Motor2_Indirect Data 46 +2076 1 Motor2_Indirect Data 47 +2077 1 Motor2_Indirect Data 48 +2078 1 Motor2_Indirect Data 49 +2079 1 Motor2_Indirect Data 50 +2080 1 Motor2_Indirect Data 51 +2081 1 Motor2_Indirect Data 52 +2082 1 Motor2_Indirect Data 53 +2083 1 Motor2_Indirect Data 54 +2084 1 Motor2_Indirect Data 55 +2085 1 Motor2_Indirect Data 56 +2086 1 Motor2_Indirect Data 57 +2087 1 Motor2_Indirect Data 58 +2088 1 Motor2_Indirect Data 59 +2089 1 Motor2_Indirect Data 60 +2090 1 Motor2_Indirect Data 61 +2091 1 Motor2_Indirect Data 62 +2092 1 Motor2_Indirect Data 63 +2093 1 Motor2_Indirect Data 64 +2094 1 Motor2_Indirect Data 65 +2095 1 Motor2_Indirect Data 66 +2096 1 Motor2_Indirect Data 67 +2097 1 Motor2_Indirect Data 68 +2098 1 Motor2_Indirect Data 69 +2099 1 Motor2_Indirect Data 70 +2100 1 Motor2_Indirect Data 71 +2101 1 Motor2_Indirect Data 72 +2102 1 Motor2_Indirect Data 73 +2103 1 Motor2_Indirect Data 74 +2104 1 Motor2_Indirect Data 75 +2105 1 Motor2_Indirect Data 76 +2106 1 Motor2_Indirect Data 77 +2107 1 Motor2_Indirect Data 78 +2108 1 Motor2_Indirect Data 79 +2109 1 Motor2_Indirect Data 80 +2110 1 Motor2_Indirect Data 81 +2111 1 Motor2_Indirect Data 82 +2112 1 Motor2_Indirect Data 83 +2113 1 Motor2_Indirect Data 84 +2114 1 Motor2_Indirect Data 85 +2115 1 Motor2_Indirect Data 86 +2116 1 Motor2_Indirect Data 87 +2117 1 Motor2_Indirect Data 88 +2118 1 Motor2_Indirect Data 89 +2119 1 Motor2_Indirect Data 90 +2120 1 Motor2_Indirect Data 91 +2121 1 Motor2_Indirect Data 92 +2122 1 Motor2_Indirect Data 93 +2123 1 Motor2_Indirect Data 94 +2124 1 Motor2_Indirect Data 95 +2125 1 Motor2_Indirect Data 96 +2126 1 Motor2_Indirect Data 97 +2127 1 Motor2_Indirect Data 98 +2128 1 Motor2_Indirect Data 99 +2129 1 Motor2_Indirect Data 100 +2130 1 Motor2_Indirect Data 101 +2131 1 Motor2_Indirect Data 102 +2132 1 Motor2_Indirect Data 103 +2133 1 Motor2_Indirect Data 104 +2134 1 Motor2_Indirect Data 105 +2135 1 Motor2_Indirect Data 106 +2136 1 Motor2_Indirect Data 107 +2137 1 Motor2_Indirect Data 108 +2138 1 Motor2_Indirect Data 109 +2139 1 Motor2_Indirect Data 110 +2140 1 Motor2_Indirect Data 111 +2141 1 Motor2_Indirect Data 112 +2142 1 Motor2_Indirect Data 113 +2143 1 Motor2_Indirect Data 114 +2144 1 Motor2_Indirect Data 115 +2145 1 Motor2_Indirect Data 116 +2146 1 Motor2_Indirect Data 117 +2147 1 Motor2_Indirect Data 118 +2148 1 Motor2_Indirect Data 119 +2149 1 Motor2_Indirect Data 120 +2150 1 Motor2_Indirect Data 121 +2151 1 Motor2_Indirect Data 122 +2152 1 Motor2_Indirect Data 123 +2153 1 Motor2_Indirect Data 124 +2154 1 Motor2_Indirect Data 125 +2155 1 Motor2_Indirect Data 126 +2156 1 Motor2_Indirect Data 127 +2157 1 Motor2_Indirect Data 128 +2158 1024 Motor3_Control_Table +2158 2 Motor3_Model Number +2160 4 Motor3_Model Information +2164 1 Motor3_Firmware Version +2165 1 Motor3_ID +2166 1 Motor3_Bus Watchdog +2168 1 Motor3_Virtual ID +2169 1 Motor3_Protocol Type +2170 1 Motor3_Baud Rate (Bus) +2171 1 Motor3_Return Delay Time +2172 1 Motor3_Bit Rate (CAN) +2173 1 Motor3_Status Return Level +2174 1 Motor3_Registered Instruction +2175 1 Motor3_Dump Control +2176 2 Motor3_Dump Time +2178 2 Motor3_Dump Step Time +2180 2 Motor3_Dump Interval Time +2182 1 Motor3_Dump Item +2186 4 Motor3_Build Date +2190 1 Motor3_Drive Mode +2191 1 Motor3_Operating Mode +2192 1 Motor3_Startup Configuration +2198 4 Motor3_In-Position Threshold +2202 4 Motor3_Following Error Threshold +2206 4 Motor3_Moving Threshold +2210 4 Motor3_Homing Offset +2214 1 Motor3_Temperature Limit +2215 1 Motor3_Temperature Limit (Motor) +2218 2 Motor3_Max Voltage Limit +2220 2 Motor3_Min Voltage Limit +2222 2 Motor3_PWM Limit +2224 2 Motor3_Current Limit +2226 4 Motor3_Acceleration Limit +2230 4 Motor3_Velocity Limit +2234 4 Motor3_Max Position Limit +2238 4 Motor3_Min Position Limit +2297 1 Motor3_Controller State +2340 1 Motor3_Error Code +2341 1 Motor3_Error Code History 1 +2342 1 Motor3_Error Code History 2 +2343 1 Motor3_Error Code History 3 +2344 1 Motor3_Error Code History 4 +2345 1 Motor3_Error Code History 5 +2346 1 Motor3_Error Code History 6 +2347 1 Motor3_Error Code History 7 +2348 1 Motor3_Error Code History 8 +2349 1 Motor3_Error Code History 9 +2350 1 Motor3_Error Code History 10 +2351 1 Motor3_Error Code History 11 +2352 1 Motor3_Error Code History 12 +2353 1 Motor3_Error Code History 13 +2354 1 Motor3_Error Code History 14 +2355 1 Motor3_Error Code History 15 +2356 1 Motor3_Error Code History 16 +2414 2 Motor3_Indirect Address 1 +2416 2 Motor3_Indirect Address 2 +2418 2 Motor3_Indirect Address 3 +2420 2 Motor3_Indirect Address 4 +2422 2 Motor3_Indirect Address 5 +2424 2 Motor3_Indirect Address 6 +2426 2 Motor3_Indirect Address 7 +2428 2 Motor3_Indirect Address 8 +2430 2 Motor3_Indirect Address 9 +2432 2 Motor3_Indirect Address 10 +2434 2 Motor3_Indirect Address 11 +2436 2 Motor3_Indirect Address 12 +2438 2 Motor3_Indirect Address 13 +2440 2 Motor3_Indirect Address 14 +2442 2 Motor3_Indirect Address 15 +2444 2 Motor3_Indirect Address 16 +2446 2 Motor3_Indirect Address 17 +2448 2 Motor3_Indirect Address 18 +2450 2 Motor3_Indirect Address 19 +2452 2 Motor3_Indirect Address 20 +2454 2 Motor3_Indirect Address 21 +2456 2 Motor3_Indirect Address 22 +2458 2 Motor3_Indirect Address 23 +2460 2 Motor3_Indirect Address 24 +2462 2 Motor3_Indirect Address 25 +2464 2 Motor3_Indirect Address 26 +2466 2 Motor3_Indirect Address 27 +2468 2 Motor3_Indirect Address 28 +2470 2 Motor3_Indirect Address 29 +2472 2 Motor3_Indirect Address 30 +2474 2 Motor3_Indirect Address 31 +2476 2 Motor3_Indirect Address 32 +2478 2 Motor3_Indirect Address 33 +2480 2 Motor3_Indirect Address 34 +2482 2 Motor3_Indirect Address 35 +2484 2 Motor3_Indirect Address 36 +2486 2 Motor3_Indirect Address 37 +2488 2 Motor3_Indirect Address 38 +2490 2 Motor3_Indirect Address 39 +2492 2 Motor3_Indirect Address 40 +2494 2 Motor3_Indirect Address 41 +2496 2 Motor3_Indirect Address 42 +2498 2 Motor3_Indirect Address 43 +2500 2 Motor3_Indirect Address 44 +2502 2 Motor3_Indirect Address 45 +2504 2 Motor3_Indirect Address 46 +2506 2 Motor3_Indirect Address 47 +2508 2 Motor3_Indirect Address 48 +2510 2 Motor3_Indirect Address 49 +2512 2 Motor3_Indirect Address 50 +2514 2 Motor3_Indirect Address 51 +2516 2 Motor3_Indirect Address 52 +2518 2 Motor3_Indirect Address 53 +2520 2 Motor3_Indirect Address 54 +2522 2 Motor3_Indirect Address 55 +2524 2 Motor3_Indirect Address 56 +2526 2 Motor3_Indirect Address 57 +2528 2 Motor3_Indirect Address 58 +2530 2 Motor3_Indirect Address 59 +2532 2 Motor3_Indirect Address 60 +2534 2 Motor3_Indirect Address 61 +2536 2 Motor3_Indirect Address 62 +2538 2 Motor3_Indirect Address 63 +2540 2 Motor3_Indirect Address 64 +2542 2 Motor3_Indirect Address 65 +2544 2 Motor3_Indirect Address 66 +2546 2 Motor3_Indirect Address 67 +2548 2 Motor3_Indirect Address 68 +2550 2 Motor3_Indirect Address 69 +2552 2 Motor3_Indirect Address 70 +2554 2 Motor3_Indirect Address 71 +2556 2 Motor3_Indirect Address 72 +2558 2 Motor3_Indirect Address 73 +2560 2 Motor3_Indirect Address 74 +2562 2 Motor3_Indirect Address 75 +2564 2 Motor3_Indirect Address 76 +2566 2 Motor3_Indirect Address 77 +2568 2 Motor3_Indirect Address 78 +2570 2 Motor3_Indirect Address 79 +2572 2 Motor3_Indirect Address 80 +2574 2 Motor3_Indirect Address 81 +2576 2 Motor3_Indirect Address 82 +2578 2 Motor3_Indirect Address 83 +2580 2 Motor3_Indirect Address 84 +2582 2 Motor3_Indirect Address 85 +2584 2 Motor3_Indirect Address 86 +2586 2 Motor3_Indirect Address 87 +2588 2 Motor3_Indirect Address 88 +2590 2 Motor3_Indirect Address 89 +2592 2 Motor3_Indirect Address 90 +2594 2 Motor3_Indirect Address 91 +2596 2 Motor3_Indirect Address 92 +2598 2 Motor3_Indirect Address 93 +2600 2 Motor3_Indirect Address 94 +2602 2 Motor3_Indirect Address 95 +2604 2 Motor3_Indirect Address 96 +2606 2 Motor3_Indirect Address 97 +2608 2 Motor3_Indirect Address 98 +2610 2 Motor3_Indirect Address 99 +2612 2 Motor3_Indirect Address 100 +2614 2 Motor3_Indirect Address 101 +2616 2 Motor3_Indirect Address 102 +2618 2 Motor3_Indirect Address 103 +2620 2 Motor3_Indirect Address 104 +2622 2 Motor3_Indirect Address 105 +2624 2 Motor3_Indirect Address 106 +2626 2 Motor3_Indirect Address 107 +2628 2 Motor3_Indirect Address 108 +2630 2 Motor3_Indirect Address 109 +2632 2 Motor3_Indirect Address 110 +2634 2 Motor3_Indirect Address 111 +2636 2 Motor3_Indirect Address 112 +2638 2 Motor3_Indirect Address 113 +2640 2 Motor3_Indirect Address 114 +2642 2 Motor3_Indirect Address 115 +2644 2 Motor3_Indirect Address 116 +2646 2 Motor3_Indirect Address 117 +2648 2 Motor3_Indirect Address 118 +2650 2 Motor3_Indirect Address 119 +2652 2 Motor3_Indirect Address 120 +2654 2 Motor3_Indirect Address 121 +2656 2 Motor3_Indirect Address 122 +2658 2 Motor3_Indirect Address 123 +2660 2 Motor3_Indirect Address 124 +2662 2 Motor3_Indirect Address 125 +2664 2 Motor3_Indirect Address 126 +2666 2 Motor3_Indirect Address 127 +2668 2 Motor3_Indirect Address 128 +2670 1 Motor3_Torque Enable +2671 1 Motor3_LED +2672 1 Motor3_Gain Save +2710 4 Motor3_Current I Gain +2714 4 Motor3_Current P Gain +2718 4 Motor3_Velocity I Gain +2722 4 Motor3_Velocity P Gain +2726 4 Motor3_Velocity FF Gain +2730 4 Motor3_Position D Gain +2734 4 Motor3_Position I Gain +2738 4 Motor3_Position P Gain +2742 4 Motor3_Position FF Gain +2746 4 Motor3_Profile Acceleration +2750 4 Motor3_Profile Velocity +2754 4 Motor3_Profile Acceleration Time +2758 4 Motor3_Profile Time +2762 2 Motor3_PWM Offset +2764 2 Motor3_Current Offset +2766 4 Motor3_Velocity Offset +2770 2 Motor3_Goal PWM +2772 2 Motor3_Goal Current +2774 4 Motor3_Goal Velocity +2778 4 Motor3_Goal Position +2782 4 Motor3_Goal Position (H) +2787 1 Motor3_Moving Status +2788 2 Motor3_Realtime Tick +2790 2 Motor3_Present PWM +2792 2 Motor3_Present Current +2794 4 Motor3_Present Velocity +2798 4 Motor3_Present Position +2802 4 Motor3_Present Position (H) +2806 4 Motor3_Position Trajectory +2810 4 Motor3_Velocity Trajectory +2814 2 Motor3_Present Input Voltage +2816 1 Motor3_Present Temperature +2817 1 Motor3_Present Temperature (Motor) +2846 2 Motor3_Voltage A +2848 2 Motor3_Voltage B +2850 2 Motor3_Voltage C +2852 2 Motor3_Present Impact +2872 2 Motor3_Current A +2874 2 Motor3_Current B +2876 2 Motor3_Current C +2878 4 Motor3_Present Single Position +2882 4 Motor3_Present Multi Position +2886 2 Motor3_Elec. Angle +3049 1 Motor3_Backup Ready +3054 1 Motor3_Indirect Data 1 +3055 1 Motor3_Indirect Data 2 +3056 1 Motor3_Indirect Data 3 +3057 1 Motor3_Indirect Data 4 +3058 1 Motor3_Indirect Data 5 +3059 1 Motor3_Indirect Data 6 +3060 1 Motor3_Indirect Data 7 +3061 1 Motor3_Indirect Data 8 +3062 1 Motor3_Indirect Data 9 +3063 1 Motor3_Indirect Data 10 +3064 1 Motor3_Indirect Data 11 +3065 1 Motor3_Indirect Data 12 +3066 1 Motor3_Indirect Data 13 +3067 1 Motor3_Indirect Data 14 +3068 1 Motor3_Indirect Data 15 +3069 1 Motor3_Indirect Data 16 +3070 1 Motor3_Indirect Data 17 +3071 1 Motor3_Indirect Data 18 +3072 1 Motor3_Indirect Data 19 +3073 1 Motor3_Indirect Data 20 +3074 1 Motor3_Indirect Data 21 +3075 1 Motor3_Indirect Data 22 +3076 1 Motor3_Indirect Data 23 +3077 1 Motor3_Indirect Data 24 +3078 1 Motor3_Indirect Data 25 +3079 1 Motor3_Indirect Data 26 +3080 1 Motor3_Indirect Data 27 +3081 1 Motor3_Indirect Data 28 +3082 1 Motor3_Indirect Data 29 +3083 1 Motor3_Indirect Data 30 +3084 1 Motor3_Indirect Data 31 +3085 1 Motor3_Indirect Data 32 +3086 1 Motor3_Indirect Data 33 +3087 1 Motor3_Indirect Data 34 +3088 1 Motor3_Indirect Data 35 +3089 1 Motor3_Indirect Data 36 +3090 1 Motor3_Indirect Data 37 +3091 1 Motor3_Indirect Data 38 +3092 1 Motor3_Indirect Data 39 +3093 1 Motor3_Indirect Data 40 +3094 1 Motor3_Indirect Data 41 +3095 1 Motor3_Indirect Data 42 +3096 1 Motor3_Indirect Data 43 +3097 1 Motor3_Indirect Data 44 +3098 1 Motor3_Indirect Data 45 +3099 1 Motor3_Indirect Data 46 +3100 1 Motor3_Indirect Data 47 +3101 1 Motor3_Indirect Data 48 +3102 1 Motor3_Indirect Data 49 +3103 1 Motor3_Indirect Data 50 +3104 1 Motor3_Indirect Data 51 +3105 1 Motor3_Indirect Data 52 +3106 1 Motor3_Indirect Data 53 +3107 1 Motor3_Indirect Data 54 +3108 1 Motor3_Indirect Data 55 +3109 1 Motor3_Indirect Data 56 +3110 1 Motor3_Indirect Data 57 +3111 1 Motor3_Indirect Data 58 +3112 1 Motor3_Indirect Data 59 +3113 1 Motor3_Indirect Data 60 +3114 1 Motor3_Indirect Data 61 +3115 1 Motor3_Indirect Data 62 +3116 1 Motor3_Indirect Data 63 +3117 1 Motor3_Indirect Data 64 +3118 1 Motor3_Indirect Data 65 +3119 1 Motor3_Indirect Data 66 +3120 1 Motor3_Indirect Data 67 +3121 1 Motor3_Indirect Data 68 +3122 1 Motor3_Indirect Data 69 +3123 1 Motor3_Indirect Data 70 +3124 1 Motor3_Indirect Data 71 +3125 1 Motor3_Indirect Data 72 +3126 1 Motor3_Indirect Data 73 +3127 1 Motor3_Indirect Data 74 +3128 1 Motor3_Indirect Data 75 +3129 1 Motor3_Indirect Data 76 +3130 1 Motor3_Indirect Data 77 +3131 1 Motor3_Indirect Data 78 +3132 1 Motor3_Indirect Data 79 +3133 1 Motor3_Indirect Data 80 +3134 1 Motor3_Indirect Data 81 +3135 1 Motor3_Indirect Data 82 +3136 1 Motor3_Indirect Data 83 +3137 1 Motor3_Indirect Data 84 +3138 1 Motor3_Indirect Data 85 +3139 1 Motor3_Indirect Data 86 +3140 1 Motor3_Indirect Data 87 +3141 1 Motor3_Indirect Data 88 +3142 1 Motor3_Indirect Data 89 +3143 1 Motor3_Indirect Data 90 +3144 1 Motor3_Indirect Data 91 +3145 1 Motor3_Indirect Data 92 +3146 1 Motor3_Indirect Data 93 +3147 1 Motor3_Indirect Data 94 +3148 1 Motor3_Indirect Data 95 +3149 1 Motor3_Indirect Data 96 +3150 1 Motor3_Indirect Data 97 +3151 1 Motor3_Indirect Data 98 +3152 1 Motor3_Indirect Data 99 +3153 1 Motor3_Indirect Data 100 +3154 1 Motor3_Indirect Data 101 +3155 1 Motor3_Indirect Data 102 +3156 1 Motor3_Indirect Data 103 +3157 1 Motor3_Indirect Data 104 +3158 1 Motor3_Indirect Data 105 +3159 1 Motor3_Indirect Data 106 +3160 1 Motor3_Indirect Data 107 +3161 1 Motor3_Indirect Data 108 +3162 1 Motor3_Indirect Data 109 +3163 1 Motor3_Indirect Data 110 +3164 1 Motor3_Indirect Data 111 +3165 1 Motor3_Indirect Data 112 +3166 1 Motor3_Indirect Data 113 +3167 1 Motor3_Indirect Data 114 +3168 1 Motor3_Indirect Data 115 +3169 1 Motor3_Indirect Data 116 +3170 1 Motor3_Indirect Data 117 +3171 1 Motor3_Indirect Data 118 +3172 1 Motor3_Indirect Data 119 +3173 1 Motor3_Indirect Data 120 +3174 1 Motor3_Indirect Data 121 +3175 1 Motor3_Indirect Data 122 +3176 1 Motor3_Indirect Data 123 +3177 1 Motor3_Indirect Data 124 +3178 1 Motor3_Indirect Data 125 +3179 1 Motor3_Indirect Data 126 +3180 1 Motor3_Indirect Data 127 +3181 1 Motor3_Indirect Data 128 +3182 1024 Swerve_DXL1_Control_Table +3182 2 Swerve_DXL1_Model Number +3184 4 Swerve_DXL1_Model Information +3188 1 Swerve_DXL1_Firmware Version +3189 1 Swerve_DXL1_ID +3190 2 Swerve_DXL1_Bus Watchdog +3192 1 Swerve_DXL1_Secondary(Shadow) ID +3193 1 Swerve_DXL1_Protocol Type +3194 1 Swerve_DXL1_Baud Rate (Bus) +3195 1 Swerve_DXL1_Return Delay Time +3197 1 Swerve_DXL1_Status Return Level +3198 1 Swerve_DXL1_Registered Instruction +3214 1 Swerve_DXL1_Drive Mode +3215 1 Swerve_DXL1_Operating Mode +3216 1 Swerve_DXL1_Startup Configuration +3220 2 Swerve_DXL1_Position Limit Threshold +3222 4 Swerve_DXL1_In-Position Threshold +3226 4 Swerve_DXL1_Following Error Threshold +3230 4 Swerve_DXL1_Moving Threshold +3234 4 Swerve_DXL1_Homing Offset +3238 1 Swerve_DXL1_Inverter Temperature Limit +3239 1 Swerve_DXL1_Motor Temperature Limit +3242 2 Swerve_DXL1_Max Voltage Limit +3244 2 Swerve_DXL1_Min Voltage Limit +3246 2 Swerve_DXL1_PWM Limit +3248 2 Swerve_DXL1_Current Limit +3250 4 Swerve_DXL1_Acceleration Limit +3254 4 Swerve_DXL1_Velocity Limit +3258 4 Swerve_DXL1_Max Position Limit +3266 4 Swerve_DXL1_Min Position Limit +3278 4 Swerve_DXL1_Electronic GearRatio Numerator +3282 4 Swerve_DXL1_Electronic GearRatio Denominator +3286 2 Swerve_DXL1_Safe Stop Time +3288 2 Swerve_DXL1_Brake Delay +3290 2 Swerve_DXL1_Goal Update Delay +3292 1 Swerve_DXL1_Overexcitation Voltage +3293 1 Swerve_DXL1_Normal Excitation Voltage +3294 2 Swerve_DXL1_Overexcitation Time +3314 2 Swerve_DXL1_Present Velocity LPF Frequency +3316 2 Swerve_DXL1_Goal Current LPF Frequency +3318 2 Swerve_DXL1_Position FF LPF Time +3320 2 Swerve_DXL1_Velocity FF LPF Time +3334 1 Swerve_DXL1_Controller State +3335 1 Swerve_DXL1_Error Code +3336 1 Swerve_DXL1_Error Code History 1 +3337 1 Swerve_DXL1_Error Code History 2 +3338 1 Swerve_DXL1_Error Code History 3 +3339 1 Swerve_DXL1_Error Code History 4 +3340 1 Swerve_DXL1_Error Code History 5 +3341 1 Swerve_DXL1_Error Code History 6 +3342 1 Swerve_DXL1_Error Code History 7 +3343 1 Swerve_DXL1_Error Code History 8 +3344 1 Swerve_DXL1_Error Code History 9 +3345 1 Swerve_DXL1_Error Code History 10 +3346 1 Swerve_DXL1_Error Code History 11 +3347 1 Swerve_DXL1_Error Code History 12 +3348 1 Swerve_DXL1_Error Code History 13 +3349 1 Swerve_DXL1_Error Code History 14 +3350 1 Swerve_DXL1_Error Code History 15 +3351 1 Swerve_DXL1_Error Code History 16 +3352 1 Swerve_DXL1_Hybrid Saveve +3394 4 Swerve_DXL1_Velocity I Gain +3398 4 Swerve_DXL1_Velocity P Gain +3402 4 Swerve_DXL1_Velocity FF Gain +3406 4 Swerve_DXL1_Position D Gain +3410 4 Swerve_DXL1_Position I Gain +3414 4 Swerve_DXL1_Position P Gain +3418 4 Swerve_DXL1_Position FF Gain +3422 4 Swerve_DXL1_Profile Acceleration +3426 4 Swerve_DXL1_Profile Velocity +3430 4 Swerve_DXL1_Profile Acceleration Time +3434 4 Swerve_DXL1_Profile Time +3438 2 Swerve_DXL1_Indirect Address 1 +3440 2 Swerve_DXL1_Indirect Address 2 +3442 2 Swerve_DXL1_Indirect Address 3 +3444 2 Swerve_DXL1_Indirect Address 4 +3446 2 Swerve_DXL1_Indirect Address 5 +3448 2 Swerve_DXL1_Indirect Address 6 +3450 2 Swerve_DXL1_Indirect Address 7 +3452 2 Swerve_DXL1_Indirect Address 8 +3454 2 Swerve_DXL1_Indirect Address 9 +3456 2 Swerve_DXL1_Indirect Address 10 +3458 2 Swerve_DXL1_Indirect Address 11 +3460 2 Swerve_DXL1_Indirect Address 12 +3462 2 Swerve_DXL1_Indirect Address 13 +3464 2 Swerve_DXL1_Indirect Address 14 +3466 2 Swerve_DXL1_Indirect Address 15 +3468 2 Swerve_DXL1_Indirect Address 16 +3470 2 Swerve_DXL1_Indirect Address 17 +3472 2 Swerve_DXL1_Indirect Address 18 +3474 2 Swerve_DXL1_Indirect Address 19 +3476 2 Swerve_DXL1_Indirect Address 20 +3478 2 Swerve_DXL1_Indirect Address 21 +3480 2 Swerve_DXL1_Indirect Address 22 +3482 2 Swerve_DXL1_Indirect Address 23 +3484 2 Swerve_DXL1_Indirect Address 24 +3486 2 Swerve_DXL1_Indirect Address 25 +3488 2 Swerve_DXL1_Indirect Address 26 +3490 2 Swerve_DXL1_Indirect Address 27 +3492 2 Swerve_DXL1_Indirect Address 28 +3494 2 Swerve_DXL1_Indirect Address 29 +3496 2 Swerve_DXL1_Indirect Address 30 +3498 2 Swerve_DXL1_Indirect Address 31 +3500 2 Swerve_DXL1_Indirect Address 32 +3502 2 Swerve_DXL1_Indirect Address 33 +3504 2 Swerve_DXL1_Indirect Address 34 +3506 2 Swerve_DXL1_Indirect Address 35 +3508 2 Swerve_DXL1_Indirect Address 36 +3510 2 Swerve_DXL1_Indirect Address 37 +3512 2 Swerve_DXL1_Indirect Address 38 +3514 2 Swerve_DXL1_Indirect Address 39 +3516 2 Swerve_DXL1_Indirect Address 40 +3518 2 Swerve_DXL1_Indirect Address 41 +3520 2 Swerve_DXL1_Indirect Address 42 +3522 2 Swerve_DXL1_Indirect Address 43 +3524 2 Swerve_DXL1_Indirect Address 44 +3526 2 Swerve_DXL1_Indirect Address 45 +3528 2 Swerve_DXL1_Indirect Address 46 +3530 2 Swerve_DXL1_Indirect Address 47 +3532 2 Swerve_DXL1_Indirect Address 48 +3534 2 Swerve_DXL1_Indirect Address 49 +3536 2 Swerve_DXL1_Indirect Address 50 +3538 2 Swerve_DXL1_Indirect Address 51 +3540 2 Swerve_DXL1_Indirect Address 52 +3542 2 Swerve_DXL1_Indirect Address 53 +3544 2 Swerve_DXL1_Indirect Address 54 +3546 2 Swerve_DXL1_Indirect Address 55 +3548 2 Swerve_DXL1_Indirect Address 56 +3550 2 Swerve_DXL1_Indirect Address 57 +3552 2 Swerve_DXL1_Indirect Address 58 +3554 2 Swerve_DXL1_Indirect Address 59 +3556 2 Swerve_DXL1_Indirect Address 60 +3558 2 Swerve_DXL1_Indirect Address 61 +3560 2 Swerve_DXL1_Indirect Address 62 +3562 2 Swerve_DXL1_Indirect Address 63 +3564 2 Swerve_DXL1_Indirect Address 64 +3566 2 Swerve_DXL1_Indirect Address 65 +3568 2 Swerve_DXL1_Indirect Address 66 +3570 2 Swerve_DXL1_Indirect Address 67 +3572 2 Swerve_DXL1_Indirect Address 68 +3574 2 Swerve_DXL1_Indirect Address 69 +3576 2 Swerve_DXL1_Indirect Address 70 +3578 2 Swerve_DXL1_Indirect Address 71 +3580 2 Swerve_DXL1_Indirect Address 72 +3582 2 Swerve_DXL1_Indirect Address 73 +3584 2 Swerve_DXL1_Indirect Address 74 +3586 2 Swerve_DXL1_Indirect Address 75 +3588 2 Swerve_DXL1_Indirect Address 76 +3590 2 Swerve_DXL1_Indirect Address 77 +3592 2 Swerve_DXL1_Indirect Address 78 +3594 2 Swerve_DXL1_Indirect Address 79 +3596 2 Swerve_DXL1_Indirect Address 80 +3598 2 Swerve_DXL1_Indirect Address 81 +3600 2 Swerve_DXL1_Indirect Address 82 +3602 2 Swerve_DXL1_Indirect Address 83 +3604 2 Swerve_DXL1_Indirect Address 84 +3606 2 Swerve_DXL1_Indirect Address 85 +3608 2 Swerve_DXL1_Indirect Address 86 +3610 2 Swerve_DXL1_Indirect Address 87 +3612 2 Swerve_DXL1_Indirect Address 88 +3614 2 Swerve_DXL1_Indirect Address 89 +3616 2 Swerve_DXL1_Indirect Address 90 +3618 2 Swerve_DXL1_Indirect Address 91 +3620 2 Swerve_DXL1_Indirect Address 92 +3622 2 Swerve_DXL1_Indirect Address 93 +3624 2 Swerve_DXL1_Indirect Address 94 +3626 2 Swerve_DXL1_Indirect Address 95 +3628 2 Swerve_DXL1_Indirect Address 96 +3630 2 Swerve_DXL1_Indirect Address 97 +3632 2 Swerve_DXL1_Indirect Address 98 +3634 2 Swerve_DXL1_Indirect Address 99 +3636 2 Swerve_DXL1_Indirect Address 100 +3638 2 Swerve_DXL1_Indirect Address 101 +3640 2 Swerve_DXL1_Indirect Address 102 +3642 2 Swerve_DXL1_Indirect Address 103 +3644 2 Swerve_DXL1_Indirect Address 104 +3646 2 Swerve_DXL1_Indirect Address 105 +3648 2 Swerve_DXL1_Indirect Address 106 +3650 2 Swerve_DXL1_Indirect Address 107 +3652 2 Swerve_DXL1_Indirect Address 108 +3654 2 Swerve_DXL1_Indirect Address 109 +3656 2 Swerve_DXL1_Indirect Address 110 +3658 2 Swerve_DXL1_Indirect Address 111 +3660 2 Swerve_DXL1_Indirect Address 112 +3662 2 Swerve_DXL1_Indirect Address 113 +3664 2 Swerve_DXL1_Indirect Address 114 +3666 2 Swerve_DXL1_Indirect Address 115 +3668 2 Swerve_DXL1_Indirect Address 116 +3670 2 Swerve_DXL1_Indirect Address 117 +3672 2 Swerve_DXL1_Indirect Address 118 +3674 2 Swerve_DXL1_Indirect Address 119 +3676 2 Swerve_DXL1_Indirect Address 120 +3678 2 Swerve_DXL1_Indirect Address 121 +3680 2 Swerve_DXL1_Indirect Address 122 +3682 2 Swerve_DXL1_Indirect Address 123 +3684 2 Swerve_DXL1_Indirect Address 124 +3686 2 Swerve_DXL1_Indirect Address 125 +3688 2 Swerve_DXL1_Indirect Address 126 +3690 2 Swerve_DXL1_Indirect Address 127 +3692 2 Swerve_DXL1_Indirect Address 128 +3694 1 Swerve_DXL1_Torque Enable +3695 1 Swerve_DXL1_LED +3698 2 Swerve_DXL1_PWM Offset +3700 2 Swerve_DXL1_Current Offset +3702 4 Swerve_DXL1_Velocity Offset +3706 2 Swerve_DXL1_Goal PWM +3708 2 Swerve_DXL1_Goal Current +3710 4 Swerve_DXL1_Goal Velocity +3714 4 Swerve_DXL1_Goal Position +3723 1 Swerve_DXL1_Moving Status +3724 2 Swerve_DXL1_Realtime Tick +3726 2 Swerve_DXL1_Present PWM +3728 2 Swerve_DXL1_Present Current +3730 4 Swerve_DXL1_Present Velocity +3734 4 Swerve_DXL1_Present Position +3742 4 Swerve_DXL1_Position Trajectory +3746 4 Swerve_DXL1_Velocity Trajectory +3750 2 Swerve_DXL1_Present Input Voltage +3752 1 Swerve_DXL1_Present Inverter Temperature +3753 1 Swerve_DXL1_Present Motor Temperature +3816 1 Swerve_DXL1_Indirect Data 1 +3817 1 Swerve_DXL1_Indirect Data 2 +3818 1 Swerve_DXL1_Indirect Data 3 +3819 1 Swerve_DXL1_Indirect Data 4 +3820 1 Swerve_DXL1_Indirect Data 5 +3821 1 Swerve_DXL1_Indirect Data 6 +3822 1 Swerve_DXL1_Indirect Data 7 +3823 1 Swerve_DXL1_Indirect Data 8 +3824 1 Swerve_DXL1_Indirect Data 9 +3825 1 Swerve_DXL1_Indirect Data 10 +3826 1 Swerve_DXL1_Indirect Data 11 +3827 1 Swerve_DXL1_Indirect Data 12 +3828 1 Swerve_DXL1_Indirect Data 13 +3829 1 Swerve_DXL1_Indirect Data 14 +3830 1 Swerve_DXL1_Indirect Data 15 +3831 1 Swerve_DXL1_Indirect Data 16 +3832 1 Swerve_DXL1_Indirect Data 17 +3833 1 Swerve_DXL1_Indirect Data 18 +3834 1 Swerve_DXL1_Indirect Data 19 +3835 1 Swerve_DXL1_Indirect Data 20 +3836 1 Swerve_DXL1_Indirect Data 21 +3837 1 Swerve_DXL1_Indirect Data 22 +3838 1 Swerve_DXL1_Indirect Data 23 +3839 1 Swerve_DXL1_Indirect Data 24 +3840 1 Swerve_DXL1_Indirect Data 25 +3841 1 Swerve_DXL1_Indirect Data 26 +3842 1 Swerve_DXL1_Indirect Data 27 +3843 1 Swerve_DXL1_Indirect Data 28 +3844 1 Swerve_DXL1_Indirect Data 29 +3845 1 Swerve_DXL1_Indirect Data 30 +3846 1 Swerve_DXL1_Indirect Data 31 +3847 1 Swerve_DXL1_Indirect Data 32 +3848 1 Swerve_DXL1_Indirect Data 33 +3849 1 Swerve_DXL1_Indirect Data 34 +3850 1 Swerve_DXL1_Indirect Data 35 +3851 1 Swerve_DXL1_Indirect Data 36 +3852 1 Swerve_DXL1_Indirect Data 37 +3853 1 Swerve_DXL1_Indirect Data 38 +3854 1 Swerve_DXL1_Indirect Data 39 +3855 1 Swerve_DXL1_Indirect Data 40 +3856 1 Swerve_DXL1_Indirect Data 41 +3857 1 Swerve_DXL1_Indirect Data 42 +3858 1 Swerve_DXL1_Indirect Data 43 +3859 1 Swerve_DXL1_Indirect Data 44 +3860 1 Swerve_DXL1_Indirect Data 45 +3861 1 Swerve_DXL1_Indirect Data 46 +3862 1 Swerve_DXL1_Indirect Data 47 +3863 1 Swerve_DXL1_Indirect Data 48 +3864 1 Swerve_DXL1_Indirect Data 49 +3865 1 Swerve_DXL1_Indirect Data 50 +3866 1 Swerve_DXL1_Indirect Data 51 +3867 1 Swerve_DXL1_Indirect Data 52 +3868 1 Swerve_DXL1_Indirect Data 53 +3869 1 Swerve_DXL1_Indirect Data 54 +3870 1 Swerve_DXL1_Indirect Data 55 +3871 1 Swerve_DXL1_Indirect Data 56 +3872 1 Swerve_DXL1_Indirect Data 57 +3873 1 Swerve_DXL1_Indirect Data 58 +3874 1 Swerve_DXL1_Indirect Data 59 +3875 1 Swerve_DXL1_Indirect Data 60 +3876 1 Swerve_DXL1_Indirect Data 61 +3877 1 Swerve_DXL1_Indirect Data 62 +3878 1 Swerve_DXL1_Indirect Data 63 +3879 1 Swerve_DXL1_Indirect Data 64 +3880 1 Swerve_DXL1_Indirect Data 65 +3881 1 Swerve_DXL1_Indirect Data 66 +3882 1 Swerve_DXL1_Indirect Data 67 +3883 1 Swerve_DXL1_Indirect Data 68 +3884 1 Swerve_DXL1_Indirect Data 69 +3885 1 Swerve_DXL1_Indirect Data 70 +3886 1 Swerve_DXL1_Indirect Data 71 +3887 1 Swerve_DXL1_Indirect Data 72 +3888 1 Swerve_DXL1_Indirect Data 73 +3889 1 Swerve_DXL1_Indirect Data 74 +3890 1 Swerve_DXL1_Indirect Data 75 +3891 1 Swerve_DXL1_Indirect Data 76 +3892 1 Swerve_DXL1_Indirect Data 77 +3893 1 Swerve_DXL1_Indirect Data 78 +3894 1 Swerve_DXL1_Indirect Data 79 +3895 1 Swerve_DXL1_Indirect Data 80 +3896 1 Swerve_DXL1_Indirect Data 81 +3897 1 Swerve_DXL1_Indirect Data 82 +3898 1 Swerve_DXL1_Indirect Data 83 +3899 1 Swerve_DXL1_Indirect Data 84 +3900 1 Swerve_DXL1_Indirect Data 85 +3901 1 Swerve_DXL1_Indirect Data 86 +3902 1 Swerve_DXL1_Indirect Data 87 +3903 1 Swerve_DXL1_Indirect Data 88 +3904 1 Swerve_DXL1_Indirect Data 89 +3905 1 Swerve_DXL1_Indirect Data 90 +3906 1 Swerve_DXL1_Indirect Data 91 +3907 1 Swerve_DXL1_Indirect Data 92 +3908 1 Swerve_DXL1_Indirect Data 93 +3909 1 Swerve_DXL1_Indirect Data 94 +3910 1 Swerve_DXL1_Indirect Data 95 +3911 1 Swerve_DXL1_Indirect Data 96 +3912 1 Swerve_DXL1_Indirect Data 97 +3913 1 Swerve_DXL1_Indirect Data 98 +3914 1 Swerve_DXL1_Indirect Data 99 +3915 1 Swerve_DXL1_Indirect Data 100 +3916 1 Swerve_DXL1_Indirect Data 101 +3917 1 Swerve_DXL1_Indirect Data 102 +3918 1 Swerve_DXL1_Indirect Data 103 +3919 1 Swerve_DXL1_Indirect Data 104 +3920 1 Swerve_DXL1_Indirect Data 105 +3921 1 Swerve_DXL1_Indirect Data 106 +3922 1 Swerve_DXL1_Indirect Data 107 +3923 1 Swerve_DXL1_Indirect Data 108 +3924 1 Swerve_DXL1_Indirect Data 109 +3925 1 Swerve_DXL1_Indirect Data 110 +3926 1 Swerve_DXL1_Indirect Data 111 +3927 1 Swerve_DXL1_Indirect Data 112 +3928 1 Swerve_DXL1_Indirect Data 113 +3929 1 Swerve_DXL1_Indirect Data 114 +3930 1 Swerve_DXL1_Indirect Data 115 +3931 1 Swerve_DXL1_Indirect Data 116 +3932 1 Swerve_DXL1_Indirect Data 117 +3933 1 Swerve_DXL1_Indirect Data 118 +3934 1 Swerve_DXL1_Indirect Data 119 +3935 1 Swerve_DXL1_Indirect Data 120 +3936 1 Swerve_DXL1_Indirect Data 121 +3937 1 Swerve_DXL1_Indirect Data 122 +3938 1 Swerve_DXL1_Indirect Data 123 +3939 1 Swerve_DXL1_Indirect Data 124 +3940 1 Swerve_DXL1_Indirect Data 125 +3941 1 Swerve_DXL1_Indirect Data 126 +3942 1 Swerve_DXL1_Indirect Data 127 +3943 1 Swerve_DXL1_Indirect Data 128 +4101 1 Swerve_DXL1_Backup Ready +4206 1024 Swerve_DXL2_Control_Table +4206 2 Swerve_DXL2_Model Number +4208 4 Swerve_DXL2_Model Information +4212 1 Swerve_DXL2_Firmware Version +4213 1 Swerve_DXL2_ID +4214 2 Swerve_DXL2_Bus Watchdog +4216 1 Swerve_DXL2_Secondary(Shadow) ID +4217 1 Swerve_DXL2_Protocol Type +4218 1 Swerve_DXL2_Baud Rate (Bus) +4219 1 Swerve_DXL2_Return Delay Time +4221 1 Swerve_DXL2_Status Return Level +4222 1 Swerve_DXL2_Registered Instruction +4238 1 Swerve_DXL2_Drive Mode +4239 1 Swerve_DXL2_Operating Mode +4240 1 Swerve_DXL2_Startup Configuration +4244 2 Swerve_DXL2_Position Limit Threshold +4246 4 Swerve_DXL2_In-Position Threshold +4250 4 Swerve_DXL2_Following Error Threshold +4254 4 Swerve_DXL2_Moving Threshold +4258 4 Swerve_DXL2_Homing Offset +4262 1 Swerve_DXL2_Inverter Temperature Limit +4263 1 Swerve_DXL2_Motor Temperature Limit +4266 2 Swerve_DXL2_Max Voltage Limit +4268 2 Swerve_DXL2_Min Voltage Limit +4270 2 Swerve_DXL2_PWM Limit +4272 2 Swerve_DXL2_Current Limit +4274 4 Swerve_DXL2_Acceleration Limit +4278 4 Swerve_DXL2_Velocity Limit +4282 4 Swerve_DXL2_Max Position Limit +4290 4 Swerve_DXL2_Min Position Limit +4302 4 Swerve_DXL2_Electronic GearRatio Numerator +4306 4 Swerve_DXL2_Electronic GearRatio Denominator +4310 2 Swerve_DXL2_Safe Stop Time +4312 2 Swerve_DXL2_Brake Delay +4314 2 Swerve_DXL2_Goal Update Delay +4316 1 Swerve_DXL2_Overexcitation Voltage +4317 1 Swerve_DXL2_Normal Excitation Voltage +4318 2 Swerve_DXL2_Overexcitation Time +4338 2 Swerve_DXL2_Present Velocity LPF Frequency +4340 2 Swerve_DXL2_Goal Current LPF Frequency +4342 2 Swerve_DXL2_Position FF LPF Time +4344 2 Swerve_DXL2_Velocity FF LPF Time +4358 1 Swerve_DXL2_Controller State +4359 1 Swerve_DXL2_Error Code +4360 1 Swerve_DXL2_Error Code History 1 +4361 1 Swerve_DXL2_Error Code History 2 +4362 1 Swerve_DXL2_Error Code History 3 +4363 1 Swerve_DXL2_Error Code History 4 +4364 1 Swerve_DXL2_Error Code History 5 +4365 1 Swerve_DXL2_Error Code History 6 +4366 1 Swerve_DXL2_Error Code History 7 +4367 1 Swerve_DXL2_Error Code History 8 +4368 1 Swerve_DXL2_Error Code History 9 +4369 1 Swerve_DXL2_Error Code History 10 +4370 1 Swerve_DXL2_Error Code History 11 +4371 1 Swerve_DXL2_Error Code History 12 +4372 1 Swerve_DXL2_Error Code History 13 +4373 1 Swerve_DXL2_Error Code History 14 +4374 1 Swerve_DXL2_Error Code History 15 +4375 1 Swerve_DXL2_Error Code History 16 +4376 1 Swerve_DXL2_Hybrid Saveve +4418 4 Swerve_DXL2_Velocity I Gain +4422 4 Swerve_DXL2_Velocity P Gain +4426 4 Swerve_DXL2_Velocity FF Gain +4430 4 Swerve_DXL2_Position D Gain +4434 4 Swerve_DXL2_Position I Gain +4438 4 Swerve_DXL2_Position P Gain +4442 4 Swerve_DXL2_Position FF Gain +4446 4 Swerve_DXL2_Profile Acceleration +4450 4 Swerve_DXL2_Profile Velocity +4454 4 Swerve_DXL2_Profile Acceleration Time +4458 4 Swerve_DXL2_Profile Time +4462 2 Swerve_DXL2_Indirect Address 1 +4464 2 Swerve_DXL2_Indirect Address 2 +4466 2 Swerve_DXL2_Indirect Address 3 +4468 2 Swerve_DXL2_Indirect Address 4 +4470 2 Swerve_DXL2_Indirect Address 5 +4472 2 Swerve_DXL2_Indirect Address 6 +4474 2 Swerve_DXL2_Indirect Address 7 +4476 2 Swerve_DXL2_Indirect Address 8 +4478 2 Swerve_DXL2_Indirect Address 9 +4480 2 Swerve_DXL2_Indirect Address 10 +4482 2 Swerve_DXL2_Indirect Address 11 +4484 2 Swerve_DXL2_Indirect Address 12 +4486 2 Swerve_DXL2_Indirect Address 13 +4488 2 Swerve_DXL2_Indirect Address 14 +4490 2 Swerve_DXL2_Indirect Address 15 +4492 2 Swerve_DXL2_Indirect Address 16 +4494 2 Swerve_DXL2_Indirect Address 17 +4496 2 Swerve_DXL2_Indirect Address 18 +4498 2 Swerve_DXL2_Indirect Address 19 +4500 2 Swerve_DXL2_Indirect Address 20 +4502 2 Swerve_DXL2_Indirect Address 21 +4504 2 Swerve_DXL2_Indirect Address 22 +4506 2 Swerve_DXL2_Indirect Address 23 +4508 2 Swerve_DXL2_Indirect Address 24 +4510 2 Swerve_DXL2_Indirect Address 25 +4512 2 Swerve_DXL2_Indirect Address 26 +4514 2 Swerve_DXL2_Indirect Address 27 +4516 2 Swerve_DXL2_Indirect Address 28 +4518 2 Swerve_DXL2_Indirect Address 29 +4520 2 Swerve_DXL2_Indirect Address 30 +4522 2 Swerve_DXL2_Indirect Address 31 +4524 2 Swerve_DXL2_Indirect Address 32 +4526 2 Swerve_DXL2_Indirect Address 33 +4528 2 Swerve_DXL2_Indirect Address 34 +4530 2 Swerve_DXL2_Indirect Address 35 +4532 2 Swerve_DXL2_Indirect Address 36 +4534 2 Swerve_DXL2_Indirect Address 37 +4536 2 Swerve_DXL2_Indirect Address 38 +4538 2 Swerve_DXL2_Indirect Address 39 +4540 2 Swerve_DXL2_Indirect Address 40 +4542 2 Swerve_DXL2_Indirect Address 41 +4544 2 Swerve_DXL2_Indirect Address 42 +4546 2 Swerve_DXL2_Indirect Address 43 +4548 2 Swerve_DXL2_Indirect Address 44 +4550 2 Swerve_DXL2_Indirect Address 45 +4552 2 Swerve_DXL2_Indirect Address 46 +4554 2 Swerve_DXL2_Indirect Address 47 +4556 2 Swerve_DXL2_Indirect Address 48 +4558 2 Swerve_DXL2_Indirect Address 49 +4560 2 Swerve_DXL2_Indirect Address 50 +4562 2 Swerve_DXL2_Indirect Address 51 +4564 2 Swerve_DXL2_Indirect Address 52 +4566 2 Swerve_DXL2_Indirect Address 53 +4568 2 Swerve_DXL2_Indirect Address 54 +4570 2 Swerve_DXL2_Indirect Address 55 +4572 2 Swerve_DXL2_Indirect Address 56 +4574 2 Swerve_DXL2_Indirect Address 57 +4576 2 Swerve_DXL2_Indirect Address 58 +4578 2 Swerve_DXL2_Indirect Address 59 +4580 2 Swerve_DXL2_Indirect Address 60 +4582 2 Swerve_DXL2_Indirect Address 61 +4584 2 Swerve_DXL2_Indirect Address 62 +4586 2 Swerve_DXL2_Indirect Address 63 +4588 2 Swerve_DXL2_Indirect Address 64 +4590 2 Swerve_DXL2_Indirect Address 65 +4592 2 Swerve_DXL2_Indirect Address 66 +4594 2 Swerve_DXL2_Indirect Address 67 +4596 2 Swerve_DXL2_Indirect Address 68 +4598 2 Swerve_DXL2_Indirect Address 69 +4600 2 Swerve_DXL2_Indirect Address 70 +4602 2 Swerve_DXL2_Indirect Address 71 +4604 2 Swerve_DXL2_Indirect Address 72 +4606 2 Swerve_DXL2_Indirect Address 73 +4608 2 Swerve_DXL2_Indirect Address 74 +4610 2 Swerve_DXL2_Indirect Address 75 +4612 2 Swerve_DXL2_Indirect Address 76 +4614 2 Swerve_DXL2_Indirect Address 77 +4616 2 Swerve_DXL2_Indirect Address 78 +4618 2 Swerve_DXL2_Indirect Address 79 +4620 2 Swerve_DXL2_Indirect Address 80 +4622 2 Swerve_DXL2_Indirect Address 81 +4624 2 Swerve_DXL2_Indirect Address 82 +4626 2 Swerve_DXL2_Indirect Address 83 +4628 2 Swerve_DXL2_Indirect Address 84 +4630 2 Swerve_DXL2_Indirect Address 85 +4632 2 Swerve_DXL2_Indirect Address 86 +4634 2 Swerve_DXL2_Indirect Address 87 +4636 2 Swerve_DXL2_Indirect Address 88 +4638 2 Swerve_DXL2_Indirect Address 89 +4640 2 Swerve_DXL2_Indirect Address 90 +4642 2 Swerve_DXL2_Indirect Address 91 +4644 2 Swerve_DXL2_Indirect Address 92 +4646 2 Swerve_DXL2_Indirect Address 93 +4648 2 Swerve_DXL2_Indirect Address 94 +4650 2 Swerve_DXL2_Indirect Address 95 +4652 2 Swerve_DXL2_Indirect Address 96 +4654 2 Swerve_DXL2_Indirect Address 97 +4656 2 Swerve_DXL2_Indirect Address 98 +4658 2 Swerve_DXL2_Indirect Address 99 +4660 2 Swerve_DXL2_Indirect Address 100 +4662 2 Swerve_DXL2_Indirect Address 101 +4664 2 Swerve_DXL2_Indirect Address 102 +4666 2 Swerve_DXL2_Indirect Address 103 +4668 2 Swerve_DXL2_Indirect Address 104 +4670 2 Swerve_DXL2_Indirect Address 105 +4672 2 Swerve_DXL2_Indirect Address 106 +4674 2 Swerve_DXL2_Indirect Address 107 +4676 2 Swerve_DXL2_Indirect Address 108 +4678 2 Swerve_DXL2_Indirect Address 109 +4680 2 Swerve_DXL2_Indirect Address 110 +4682 2 Swerve_DXL2_Indirect Address 111 +4684 2 Swerve_DXL2_Indirect Address 112 +4686 2 Swerve_DXL2_Indirect Address 113 +4688 2 Swerve_DXL2_Indirect Address 114 +4690 2 Swerve_DXL2_Indirect Address 115 +4692 2 Swerve_DXL2_Indirect Address 116 +4694 2 Swerve_DXL2_Indirect Address 117 +4696 2 Swerve_DXL2_Indirect Address 118 +4698 2 Swerve_DXL2_Indirect Address 119 +4700 2 Swerve_DXL2_Indirect Address 120 +4702 2 Swerve_DXL2_Indirect Address 121 +4704 2 Swerve_DXL2_Indirect Address 122 +4706 2 Swerve_DXL2_Indirect Address 123 +4708 2 Swerve_DXL2_Indirect Address 124 +4710 2 Swerve_DXL2_Indirect Address 125 +4712 2 Swerve_DXL2_Indirect Address 126 +4714 2 Swerve_DXL2_Indirect Address 127 +4716 2 Swerve_DXL2_Indirect Address 128 +4718 1 Swerve_DXL2_Torque Enable +4719 1 Swerve_DXL2_LED +4722 2 Swerve_DXL2_PWM Offset +4724 2 Swerve_DXL2_Current Offset +4726 4 Swerve_DXL2_Velocity Offset +4730 2 Swerve_DXL2_Goal PWM +4732 2 Swerve_DXL2_Goal Current +4734 4 Swerve_DXL2_Goal Velocity +4738 4 Swerve_DXL2_Goal Position +4747 1 Swerve_DXL2_Moving Status +4748 2 Swerve_DXL2_Realtime Tick +4750 2 Swerve_DXL2_Present PWM +4752 2 Swerve_DXL2_Present Current +4754 4 Swerve_DXL2_Present Velocity +4758 4 Swerve_DXL2_Present Position +4766 4 Swerve_DXL2_Position Trajectory +4770 4 Swerve_DXL2_Velocity Trajectory +4774 2 Swerve_DXL2_Present Input Voltage +4776 1 Swerve_DXL2_Present Inverter Temperature +4777 1 Swerve_DXL2_Present Motor Temperature +4840 1 Swerve_DXL2_Indirect Data 1 +4841 1 Swerve_DXL2_Indirect Data 2 +4842 1 Swerve_DXL2_Indirect Data 3 +4843 1 Swerve_DXL2_Indirect Data 4 +4844 1 Swerve_DXL2_Indirect Data 5 +4845 1 Swerve_DXL2_Indirect Data 6 +4846 1 Swerve_DXL2_Indirect Data 7 +4847 1 Swerve_DXL2_Indirect Data 8 +4848 1 Swerve_DXL2_Indirect Data 9 +4849 1 Swerve_DXL2_Indirect Data 10 +4850 1 Swerve_DXL2_Indirect Data 11 +4851 1 Swerve_DXL2_Indirect Data 12 +4852 1 Swerve_DXL2_Indirect Data 13 +4853 1 Swerve_DXL2_Indirect Data 14 +4854 1 Swerve_DXL2_Indirect Data 15 +4855 1 Swerve_DXL2_Indirect Data 16 +4856 1 Swerve_DXL2_Indirect Data 17 +4857 1 Swerve_DXL2_Indirect Data 18 +4858 1 Swerve_DXL2_Indirect Data 19 +4859 1 Swerve_DXL2_Indirect Data 20 +4860 1 Swerve_DXL2_Indirect Data 21 +4861 1 Swerve_DXL2_Indirect Data 22 +4862 1 Swerve_DXL2_Indirect Data 23 +4863 1 Swerve_DXL2_Indirect Data 24 +4864 1 Swerve_DXL2_Indirect Data 25 +4865 1 Swerve_DXL2_Indirect Data 26 +4866 1 Swerve_DXL2_Indirect Data 27 +4867 1 Swerve_DXL2_Indirect Data 28 +4868 1 Swerve_DXL2_Indirect Data 29 +4869 1 Swerve_DXL2_Indirect Data 30 +4870 1 Swerve_DXL2_Indirect Data 31 +4871 1 Swerve_DXL2_Indirect Data 32 +4872 1 Swerve_DXL2_Indirect Data 33 +4873 1 Swerve_DXL2_Indirect Data 34 +4874 1 Swerve_DXL2_Indirect Data 35 +4875 1 Swerve_DXL2_Indirect Data 36 +4876 1 Swerve_DXL2_Indirect Data 37 +4877 1 Swerve_DXL2_Indirect Data 38 +4878 1 Swerve_DXL2_Indirect Data 39 +4879 1 Swerve_DXL2_Indirect Data 40 +4880 1 Swerve_DXL2_Indirect Data 41 +4881 1 Swerve_DXL2_Indirect Data 42 +4882 1 Swerve_DXL2_Indirect Data 43 +4883 1 Swerve_DXL2_Indirect Data 44 +4884 1 Swerve_DXL2_Indirect Data 45 +4885 1 Swerve_DXL2_Indirect Data 46 +4886 1 Swerve_DXL2_Indirect Data 47 +4887 1 Swerve_DXL2_Indirect Data 48 +4888 1 Swerve_DXL2_Indirect Data 49 +4889 1 Swerve_DXL2_Indirect Data 50 +4890 1 Swerve_DXL2_Indirect Data 51 +4891 1 Swerve_DXL2_Indirect Data 52 +4892 1 Swerve_DXL2_Indirect Data 53 +4893 1 Swerve_DXL2_Indirect Data 54 +4894 1 Swerve_DXL2_Indirect Data 55 +4895 1 Swerve_DXL2_Indirect Data 56 +4896 1 Swerve_DXL2_Indirect Data 57 +4897 1 Swerve_DXL2_Indirect Data 58 +4898 1 Swerve_DXL2_Indirect Data 59 +4899 1 Swerve_DXL2_Indirect Data 60 +4900 1 Swerve_DXL2_Indirect Data 61 +4901 1 Swerve_DXL2_Indirect Data 62 +4902 1 Swerve_DXL2_Indirect Data 63 +4903 1 Swerve_DXL2_Indirect Data 64 +4904 1 Swerve_DXL2_Indirect Data 65 +4905 1 Swerve_DXL2_Indirect Data 66 +4906 1 Swerve_DXL2_Indirect Data 67 +4907 1 Swerve_DXL2_Indirect Data 68 +4908 1 Swerve_DXL2_Indirect Data 69 +4909 1 Swerve_DXL2_Indirect Data 70 +4910 1 Swerve_DXL2_Indirect Data 71 +4911 1 Swerve_DXL2_Indirect Data 72 +4912 1 Swerve_DXL2_Indirect Data 73 +4913 1 Swerve_DXL2_Indirect Data 74 +4914 1 Swerve_DXL2_Indirect Data 75 +4915 1 Swerve_DXL2_Indirect Data 76 +4916 1 Swerve_DXL2_Indirect Data 77 +4917 1 Swerve_DXL2_Indirect Data 78 +4918 1 Swerve_DXL2_Indirect Data 79 +4919 1 Swerve_DXL2_Indirect Data 80 +4920 1 Swerve_DXL2_Indirect Data 81 +4921 1 Swerve_DXL2_Indirect Data 82 +4922 1 Swerve_DXL2_Indirect Data 83 +4923 1 Swerve_DXL2_Indirect Data 84 +4924 1 Swerve_DXL2_Indirect Data 85 +4925 1 Swerve_DXL2_Indirect Data 86 +4926 1 Swerve_DXL2_Indirect Data 87 +4927 1 Swerve_DXL2_Indirect Data 88 +4928 1 Swerve_DXL2_Indirect Data 89 +4929 1 Swerve_DXL2_Indirect Data 90 +4930 1 Swerve_DXL2_Indirect Data 91 +4931 1 Swerve_DXL2_Indirect Data 92 +4932 1 Swerve_DXL2_Indirect Data 93 +4933 1 Swerve_DXL2_Indirect Data 94 +4934 1 Swerve_DXL2_Indirect Data 95 +4935 1 Swerve_DXL2_Indirect Data 96 +4936 1 Swerve_DXL2_Indirect Data 97 +4937 1 Swerve_DXL2_Indirect Data 98 +4938 1 Swerve_DXL2_Indirect Data 99 +4939 1 Swerve_DXL2_Indirect Data 100 +4940 1 Swerve_DXL2_Indirect Data 101 +4941 1 Swerve_DXL2_Indirect Data 102 +4942 1 Swerve_DXL2_Indirect Data 103 +4943 1 Swerve_DXL2_Indirect Data 104 +4944 1 Swerve_DXL2_Indirect Data 105 +4945 1 Swerve_DXL2_Indirect Data 106 +4946 1 Swerve_DXL2_Indirect Data 107 +4947 1 Swerve_DXL2_Indirect Data 108 +4948 1 Swerve_DXL2_Indirect Data 109 +4949 1 Swerve_DXL2_Indirect Data 110 +4950 1 Swerve_DXL2_Indirect Data 111 +4951 1 Swerve_DXL2_Indirect Data 112 +4952 1 Swerve_DXL2_Indirect Data 113 +4953 1 Swerve_DXL2_Indirect Data 114 +4954 1 Swerve_DXL2_Indirect Data 115 +4955 1 Swerve_DXL2_Indirect Data 116 +4956 1 Swerve_DXL2_Indirect Data 117 +4957 1 Swerve_DXL2_Indirect Data 118 +4958 1 Swerve_DXL2_Indirect Data 119 +4959 1 Swerve_DXL2_Indirect Data 120 +4960 1 Swerve_DXL2_Indirect Data 121 +4961 1 Swerve_DXL2_Indirect Data 122 +4962 1 Swerve_DXL2_Indirect Data 123 +4963 1 Swerve_DXL2_Indirect Data 124 +4964 1 Swerve_DXL2_Indirect Data 125 +4965 1 Swerve_DXL2_Indirect Data 126 +4966 1 Swerve_DXL2_Indirect Data 127 +4967 1 Swerve_DXL2_Indirect Data 128 +5125 1 Swerve_DXL2_Backup Ready +5230 1024 Swerve_DXL3_Control_Table +5230 2 Swerve_DXL3_Model Number +5232 4 Swerve_DXL3_Model Information +5236 1 Swerve_DXL3_Firmware Version +5237 1 Swerve_DXL3_ID +5238 2 Swerve_DXL3_Bus Watchdog +5240 1 Swerve_DXL3_Secondary(Shadow) ID +5241 1 Swerve_DXL3_Protocol Type +5242 1 Swerve_DXL3_Baud Rate (Bus) +5243 1 Swerve_DXL3_Return Delay Time +5245 1 Swerve_DXL3_Status Return Level +5246 1 Swerve_DXL3_Registered Instruction +5262 1 Swerve_DXL3_Drive Mode +5263 1 Swerve_DXL3_Operating Mode +5264 1 Swerve_DXL3_Startup Configuration +5268 2 Swerve_DXL3_Position Limit Threshold +5270 4 Swerve_DXL3_In-Position Threshold +5274 4 Swerve_DXL3_Following Error Threshold +5278 4 Swerve_DXL3_Moving Threshold +5282 4 Swerve_DXL3_Homing Offset +5286 1 Swerve_DXL3_Inverter Temperature Limit +5287 1 Swerve_DXL3_Motor Temperature Limit +5290 2 Swerve_DXL3_Max Voltage Limit +5292 2 Swerve_DXL3_Min Voltage Limit +5294 2 Swerve_DXL3_PWM Limit +5296 2 Swerve_DXL3_Current Limit +5298 4 Swerve_DXL3_Acceleration Limit +5302 4 Swerve_DXL3_Velocity Limit +5306 4 Swerve_DXL3_Max Position Limit +5314 4 Swerve_DXL3_Min Position Limit +5326 4 Swerve_DXL3_Electronic GearRatio Numerator +5330 4 Swerve_DXL3_Electronic GearRatio Denominator +5334 2 Swerve_DXL3_Safe Stop Time +5336 2 Swerve_DXL3_Brake Delay +5338 2 Swerve_DXL3_Goal Update Delay +5340 1 Swerve_DXL3_Overexcitation Voltage +5341 1 Swerve_DXL3_Normal Excitation Voltage +5342 2 Swerve_DXL3_Overexcitation Time +5362 2 Swerve_DXL3_Present Velocity LPF Frequency +5364 2 Swerve_DXL3_Goal Current LPF Frequency +5366 2 Swerve_DXL3_Position FF LPF Time +5368 2 Swerve_DXL3_Velocity FF LPF Time +5382 1 Swerve_DXL3_Controller State +5383 1 Swerve_DXL3_Error Code +5384 1 Swerve_DXL3_Error Code History 1 +5385 1 Swerve_DXL3_Error Code History 2 +5386 1 Swerve_DXL3_Error Code History 3 +5387 1 Swerve_DXL3_Error Code History 4 +5388 1 Swerve_DXL3_Error Code History 5 +5389 1 Swerve_DXL3_Error Code History 6 +5390 1 Swerve_DXL3_Error Code History 7 +5391 1 Swerve_DXL3_Error Code History 8 +5392 1 Swerve_DXL3_Error Code History 9 +5393 1 Swerve_DXL3_Error Code History 10 +5394 1 Swerve_DXL3_Error Code History 11 +5395 1 Swerve_DXL3_Error Code History 12 +5396 1 Swerve_DXL3_Error Code History 13 +5397 1 Swerve_DXL3_Error Code History 14 +5398 1 Swerve_DXL3_Error Code History 15 +5399 1 Swerve_DXL3_Error Code History 16 +5400 1 Swerve_DXL3_Hybrid Saveve +5442 4 Swerve_DXL3_Velocity I Gain +5446 4 Swerve_DXL3_Velocity P Gain +5450 4 Swerve_DXL3_Velocity FF Gain +5454 4 Swerve_DXL3_Position D Gain +5458 4 Swerve_DXL3_Position I Gain +5462 4 Swerve_DXL3_Position P Gain +5466 4 Swerve_DXL3_Position FF Gain +5470 4 Swerve_DXL3_Profile Acceleration +5474 4 Swerve_DXL3_Profile Velocity +5478 4 Swerve_DXL3_Profile Acceleration Time +5482 4 Swerve_DXL3_Profile Time +5486 2 Swerve_DXL3_Indirect Address 1 +5488 2 Swerve_DXL3_Indirect Address 2 +5490 2 Swerve_DXL3_Indirect Address 3 +5492 2 Swerve_DXL3_Indirect Address 4 +5494 2 Swerve_DXL3_Indirect Address 5 +5496 2 Swerve_DXL3_Indirect Address 6 +5498 2 Swerve_DXL3_Indirect Address 7 +5500 2 Swerve_DXL3_Indirect Address 8 +5502 2 Swerve_DXL3_Indirect Address 9 +5504 2 Swerve_DXL3_Indirect Address 10 +5506 2 Swerve_DXL3_Indirect Address 11 +5508 2 Swerve_DXL3_Indirect Address 12 +5510 2 Swerve_DXL3_Indirect Address 13 +5512 2 Swerve_DXL3_Indirect Address 14 +5514 2 Swerve_DXL3_Indirect Address 15 +5516 2 Swerve_DXL3_Indirect Address 16 +5518 2 Swerve_DXL3_Indirect Address 17 +5520 2 Swerve_DXL3_Indirect Address 18 +5522 2 Swerve_DXL3_Indirect Address 19 +5524 2 Swerve_DXL3_Indirect Address 20 +5526 2 Swerve_DXL3_Indirect Address 21 +5528 2 Swerve_DXL3_Indirect Address 22 +5530 2 Swerve_DXL3_Indirect Address 23 +5532 2 Swerve_DXL3_Indirect Address 24 +5534 2 Swerve_DXL3_Indirect Address 25 +5536 2 Swerve_DXL3_Indirect Address 26 +5538 2 Swerve_DXL3_Indirect Address 27 +5540 2 Swerve_DXL3_Indirect Address 28 +5542 2 Swerve_DXL3_Indirect Address 29 +5544 2 Swerve_DXL3_Indirect Address 30 +5546 2 Swerve_DXL3_Indirect Address 31 +5548 2 Swerve_DXL3_Indirect Address 32 +5550 2 Swerve_DXL3_Indirect Address 33 +5552 2 Swerve_DXL3_Indirect Address 34 +5554 2 Swerve_DXL3_Indirect Address 35 +5556 2 Swerve_DXL3_Indirect Address 36 +5558 2 Swerve_DXL3_Indirect Address 37 +5560 2 Swerve_DXL3_Indirect Address 38 +5562 2 Swerve_DXL3_Indirect Address 39 +5564 2 Swerve_DXL3_Indirect Address 40 +5566 2 Swerve_DXL3_Indirect Address 41 +5568 2 Swerve_DXL3_Indirect Address 42 +5570 2 Swerve_DXL3_Indirect Address 43 +5572 2 Swerve_DXL3_Indirect Address 44 +5574 2 Swerve_DXL3_Indirect Address 45 +5576 2 Swerve_DXL3_Indirect Address 46 +5578 2 Swerve_DXL3_Indirect Address 47 +5580 2 Swerve_DXL3_Indirect Address 48 +5582 2 Swerve_DXL3_Indirect Address 49 +5584 2 Swerve_DXL3_Indirect Address 50 +5586 2 Swerve_DXL3_Indirect Address 51 +5588 2 Swerve_DXL3_Indirect Address 52 +5590 2 Swerve_DXL3_Indirect Address 53 +5592 2 Swerve_DXL3_Indirect Address 54 +5594 2 Swerve_DXL3_Indirect Address 55 +5596 2 Swerve_DXL3_Indirect Address 56 +5598 2 Swerve_DXL3_Indirect Address 57 +5600 2 Swerve_DXL3_Indirect Address 58 +5602 2 Swerve_DXL3_Indirect Address 59 +5604 2 Swerve_DXL3_Indirect Address 60 +5606 2 Swerve_DXL3_Indirect Address 61 +5608 2 Swerve_DXL3_Indirect Address 62 +5610 2 Swerve_DXL3_Indirect Address 63 +5612 2 Swerve_DXL3_Indirect Address 64 +5614 2 Swerve_DXL3_Indirect Address 65 +5616 2 Swerve_DXL3_Indirect Address 66 +5618 2 Swerve_DXL3_Indirect Address 67 +5620 2 Swerve_DXL3_Indirect Address 68 +5622 2 Swerve_DXL3_Indirect Address 69 +5624 2 Swerve_DXL3_Indirect Address 70 +5626 2 Swerve_DXL3_Indirect Address 71 +5628 2 Swerve_DXL3_Indirect Address 72 +5630 2 Swerve_DXL3_Indirect Address 73 +5632 2 Swerve_DXL3_Indirect Address 74 +5634 2 Swerve_DXL3_Indirect Address 75 +5636 2 Swerve_DXL3_Indirect Address 76 +5638 2 Swerve_DXL3_Indirect Address 77 +5640 2 Swerve_DXL3_Indirect Address 78 +5642 2 Swerve_DXL3_Indirect Address 79 +5644 2 Swerve_DXL3_Indirect Address 80 +5646 2 Swerve_DXL3_Indirect Address 81 +5648 2 Swerve_DXL3_Indirect Address 82 +5650 2 Swerve_DXL3_Indirect Address 83 +5652 2 Swerve_DXL3_Indirect Address 84 +5654 2 Swerve_DXL3_Indirect Address 85 +5656 2 Swerve_DXL3_Indirect Address 86 +5658 2 Swerve_DXL3_Indirect Address 87 +5660 2 Swerve_DXL3_Indirect Address 88 +5662 2 Swerve_DXL3_Indirect Address 89 +5664 2 Swerve_DXL3_Indirect Address 90 +5666 2 Swerve_DXL3_Indirect Address 91 +5668 2 Swerve_DXL3_Indirect Address 92 +5670 2 Swerve_DXL3_Indirect Address 93 +5672 2 Swerve_DXL3_Indirect Address 94 +5674 2 Swerve_DXL3_Indirect Address 95 +5676 2 Swerve_DXL3_Indirect Address 96 +5678 2 Swerve_DXL3_Indirect Address 97 +5680 2 Swerve_DXL3_Indirect Address 98 +5682 2 Swerve_DXL3_Indirect Address 99 +5684 2 Swerve_DXL3_Indirect Address 100 +5686 2 Swerve_DXL3_Indirect Address 101 +5688 2 Swerve_DXL3_Indirect Address 102 +5690 2 Swerve_DXL3_Indirect Address 103 +5692 2 Swerve_DXL3_Indirect Address 104 +5694 2 Swerve_DXL3_Indirect Address 105 +5696 2 Swerve_DXL3_Indirect Address 106 +5698 2 Swerve_DXL3_Indirect Address 107 +5700 2 Swerve_DXL3_Indirect Address 108 +5702 2 Swerve_DXL3_Indirect Address 109 +5704 2 Swerve_DXL3_Indirect Address 110 +5706 2 Swerve_DXL3_Indirect Address 111 +5708 2 Swerve_DXL3_Indirect Address 112 +5710 2 Swerve_DXL3_Indirect Address 113 +5712 2 Swerve_DXL3_Indirect Address 114 +5714 2 Swerve_DXL3_Indirect Address 115 +5716 2 Swerve_DXL3_Indirect Address 116 +5718 2 Swerve_DXL3_Indirect Address 117 +5720 2 Swerve_DXL3_Indirect Address 118 +5722 2 Swerve_DXL3_Indirect Address 119 +5724 2 Swerve_DXL3_Indirect Address 120 +5726 2 Swerve_DXL3_Indirect Address 121 +5728 2 Swerve_DXL3_Indirect Address 122 +5730 2 Swerve_DXL3_Indirect Address 123 +5732 2 Swerve_DXL3_Indirect Address 124 +5734 2 Swerve_DXL3_Indirect Address 125 +5736 2 Swerve_DXL3_Indirect Address 126 +5738 2 Swerve_DXL3_Indirect Address 127 +5740 2 Swerve_DXL3_Indirect Address 128 +5742 1 Swerve_DXL3_Torque Enable +5743 1 Swerve_DXL3_LED +5746 2 Swerve_DXL3_PWM Offset +5748 2 Swerve_DXL3_Current Offset +5750 4 Swerve_DXL3_Velocity Offset +5754 2 Swerve_DXL3_Goal PWM +5756 2 Swerve_DXL3_Goal Current +5758 4 Swerve_DXL3_Goal Velocity +5762 4 Swerve_DXL3_Goal Position +5771 1 Swerve_DXL3_Moving Status +5772 2 Swerve_DXL3_Realtime Tick +5774 2 Swerve_DXL3_Present PWM +5776 2 Swerve_DXL3_Present Current +5778 4 Swerve_DXL3_Present Velocity +5782 4 Swerve_DXL3_Present Position +5790 4 Swerve_DXL3_Position Trajectory +5794 4 Swerve_DXL3_Velocity Trajectory +5798 2 Swerve_DXL3_Present Input Voltage +5800 1 Swerve_DXL3_Present Inverter Temperature +5801 1 Swerve_DXL3_Present Motor Temperature +5864 1 Swerve_DXL3_Indirect Data 1 +5865 1 Swerve_DXL3_Indirect Data 2 +5866 1 Swerve_DXL3_Indirect Data 3 +5867 1 Swerve_DXL3_Indirect Data 4 +5868 1 Swerve_DXL3_Indirect Data 5 +5869 1 Swerve_DXL3_Indirect Data 6 +5870 1 Swerve_DXL3_Indirect Data 7 +5871 1 Swerve_DXL3_Indirect Data 8 +5872 1 Swerve_DXL3_Indirect Data 9 +5873 1 Swerve_DXL3_Indirect Data 10 +5874 1 Swerve_DXL3_Indirect Data 11 +5875 1 Swerve_DXL3_Indirect Data 12 +5876 1 Swerve_DXL3_Indirect Data 13 +5877 1 Swerve_DXL3_Indirect Data 14 +5878 1 Swerve_DXL3_Indirect Data 15 +5879 1 Swerve_DXL3_Indirect Data 16 +5880 1 Swerve_DXL3_Indirect Data 17 +5881 1 Swerve_DXL3_Indirect Data 18 +5882 1 Swerve_DXL3_Indirect Data 19 +5883 1 Swerve_DXL3_Indirect Data 20 +5884 1 Swerve_DXL3_Indirect Data 21 +5885 1 Swerve_DXL3_Indirect Data 22 +5886 1 Swerve_DXL3_Indirect Data 23 +5887 1 Swerve_DXL3_Indirect Data 24 +5888 1 Swerve_DXL3_Indirect Data 25 +5889 1 Swerve_DXL3_Indirect Data 26 +5890 1 Swerve_DXL3_Indirect Data 27 +5891 1 Swerve_DXL3_Indirect Data 28 +5892 1 Swerve_DXL3_Indirect Data 29 +5893 1 Swerve_DXL3_Indirect Data 30 +5894 1 Swerve_DXL3_Indirect Data 31 +5895 1 Swerve_DXL3_Indirect Data 32 +5896 1 Swerve_DXL3_Indirect Data 33 +5897 1 Swerve_DXL3_Indirect Data 34 +5898 1 Swerve_DXL3_Indirect Data 35 +5899 1 Swerve_DXL3_Indirect Data 36 +5900 1 Swerve_DXL3_Indirect Data 37 +5901 1 Swerve_DXL3_Indirect Data 38 +5902 1 Swerve_DXL3_Indirect Data 39 +5903 1 Swerve_DXL3_Indirect Data 40 +5904 1 Swerve_DXL3_Indirect Data 41 +5905 1 Swerve_DXL3_Indirect Data 42 +5906 1 Swerve_DXL3_Indirect Data 43 +5907 1 Swerve_DXL3_Indirect Data 44 +5908 1 Swerve_DXL3_Indirect Data 45 +5909 1 Swerve_DXL3_Indirect Data 46 +5910 1 Swerve_DXL3_Indirect Data 47 +5911 1 Swerve_DXL3_Indirect Data 48 +5912 1 Swerve_DXL3_Indirect Data 49 +5913 1 Swerve_DXL3_Indirect Data 50 +5914 1 Swerve_DXL3_Indirect Data 51 +5915 1 Swerve_DXL3_Indirect Data 52 +5916 1 Swerve_DXL3_Indirect Data 53 +5917 1 Swerve_DXL3_Indirect Data 54 +5918 1 Swerve_DXL3_Indirect Data 55 +5919 1 Swerve_DXL3_Indirect Data 56 +5920 1 Swerve_DXL3_Indirect Data 57 +5921 1 Swerve_DXL3_Indirect Data 58 +5922 1 Swerve_DXL3_Indirect Data 59 +5923 1 Swerve_DXL3_Indirect Data 60 +5924 1 Swerve_DXL3_Indirect Data 61 +5925 1 Swerve_DXL3_Indirect Data 62 +5926 1 Swerve_DXL3_Indirect Data 63 +5927 1 Swerve_DXL3_Indirect Data 64 +5928 1 Swerve_DXL3_Indirect Data 65 +5929 1 Swerve_DXL3_Indirect Data 66 +5930 1 Swerve_DXL3_Indirect Data 67 +5931 1 Swerve_DXL3_Indirect Data 68 +5932 1 Swerve_DXL3_Indirect Data 69 +5933 1 Swerve_DXL3_Indirect Data 70 +5934 1 Swerve_DXL3_Indirect Data 71 +5935 1 Swerve_DXL3_Indirect Data 72 +5936 1 Swerve_DXL3_Indirect Data 73 +5937 1 Swerve_DXL3_Indirect Data 74 +5938 1 Swerve_DXL3_Indirect Data 75 +5939 1 Swerve_DXL3_Indirect Data 76 +5940 1 Swerve_DXL3_Indirect Data 77 +5941 1 Swerve_DXL3_Indirect Data 78 +5942 1 Swerve_DXL3_Indirect Data 79 +5943 1 Swerve_DXL3_Indirect Data 80 +5944 1 Swerve_DXL3_Indirect Data 81 +5945 1 Swerve_DXL3_Indirect Data 82 +5946 1 Swerve_DXL3_Indirect Data 83 +5947 1 Swerve_DXL3_Indirect Data 84 +5948 1 Swerve_DXL3_Indirect Data 85 +5949 1 Swerve_DXL3_Indirect Data 86 +5950 1 Swerve_DXL3_Indirect Data 87 +5951 1 Swerve_DXL3_Indirect Data 88 +5952 1 Swerve_DXL3_Indirect Data 89 +5953 1 Swerve_DXL3_Indirect Data 90 +5954 1 Swerve_DXL3_Indirect Data 91 +5955 1 Swerve_DXL3_Indirect Data 92 +5956 1 Swerve_DXL3_Indirect Data 93 +5957 1 Swerve_DXL3_Indirect Data 94 +5958 1 Swerve_DXL3_Indirect Data 95 +5959 1 Swerve_DXL3_Indirect Data 96 +5960 1 Swerve_DXL3_Indirect Data 97 +5961 1 Swerve_DXL3_Indirect Data 98 +5962 1 Swerve_DXL3_Indirect Data 99 +5963 1 Swerve_DXL3_Indirect Data 100 +5964 1 Swerve_DXL3_Indirect Data 101 +5965 1 Swerve_DXL3_Indirect Data 102 +5966 1 Swerve_DXL3_Indirect Data 103 +5967 1 Swerve_DXL3_Indirect Data 104 +5968 1 Swerve_DXL3_Indirect Data 105 +5969 1 Swerve_DXL3_Indirect Data 106 +5970 1 Swerve_DXL3_Indirect Data 107 +5971 1 Swerve_DXL3_Indirect Data 108 +5972 1 Swerve_DXL3_Indirect Data 109 +5973 1 Swerve_DXL3_Indirect Data 110 +5974 1 Swerve_DXL3_Indirect Data 111 +5975 1 Swerve_DXL3_Indirect Data 112 +5976 1 Swerve_DXL3_Indirect Data 113 +5977 1 Swerve_DXL3_Indirect Data 114 +5978 1 Swerve_DXL3_Indirect Data 115 +5979 1 Swerve_DXL3_Indirect Data 116 +5980 1 Swerve_DXL3_Indirect Data 117 +5981 1 Swerve_DXL3_Indirect Data 118 +5982 1 Swerve_DXL3_Indirect Data 119 +5983 1 Swerve_DXL3_Indirect Data 120 +5984 1 Swerve_DXL3_Indirect Data 121 +5985 1 Swerve_DXL3_Indirect Data 122 +5986 1 Swerve_DXL3_Indirect Data 123 +5987 1 Swerve_DXL3_Indirect Data 124 +5988 1 Swerve_DXL3_Indirect Data 125 +5989 1 Swerve_DXL3_Indirect Data 126 +5990 1 Swerve_DXL3_Indirect Data 127 +5991 1 Swerve_DXL3_Indirect Data 128 +6149 1 Swerve_DXL3_Backup Ready +6500 1 Power_Supply_Method +6501 1 Charging_Method +6502 1 Battery_Percentage +6503 1 Charging_Status +6510 2 Voltage_Battery +6512 2 Voltage_Docking +6514 2 Voltage_Motor +6516 2 Voltage_14V +6518 2 Voltage_12V +6520 2 Voltage_5V +6522 2 Voltage_3.3V +6530 2 Current_Battery +6532 2 Current_Motor +6534 2 Current_Docking +6536 2 Current_Orin +6538 2 Current_12V +6540 2 Current_5V +6600 1 Error_List_Size +6604 4 Error_Main +6608 4 Error_List1 +6612 4 Error_List2 +6616 4 Error_List3 +6620 4 Error_List4 +6624 4 Error_List5 +6628 4 Error_List6 +6632 4 Error_List7 +6636 4 Error_List8 +6640 4 Error_List9 +6644 4 Error_List10 +6648 4 Error_List11 +6652 4 Error_List12 +6656 4 Error_List13 +6660 4 Error_List14 +6664 4 Error_List15 +6668 4 Error_List16 +6672 4 Error_List17 +6676 4 Error_List18 +6680 4 Error_List19 +6684 4 Error_List20 +6688 4 Error_List21 +6692 4 Error_List22 +6696 4 Error_List23 +6700 4 Error_List24 +7000 2 Indirect_Address_1 +7002 2 Indirect_Address_2 +7004 2 Indirect_Address_3 +7006 2 Indirect_Address_4 +7008 2 Indirect_Address_5 +7010 2 Indirect_Address_6 +7012 2 Indirect_Address_7 +7014 2 Indirect_Address_8 +7016 2 Indirect_Address_9 +7018 2 Indirect_Address_10 +7020 2 Indirect_Address_11 +7022 2 Indirect_Address_12 +7024 2 Indirect_Address_13 +7026 2 Indirect_Address_14 +7028 2 Indirect_Address_15 +7030 2 Indirect_Address_16 +7032 2 Indirect_Address_17 +7034 2 Indirect_Address_18 +7036 2 Indirect_Address_19 +7038 2 Indirect_Address_20 +7040 2 Indirect_Address_21 +7042 2 Indirect_Address_22 +7044 2 Indirect_Address_23 +7046 2 Indirect_Address_24 +7048 2 Indirect_Address_25 +7050 2 Indirect_Address_26 +7052 2 Indirect_Address_27 +7054 2 Indirect_Address_28 +7056 2 Indirect_Address_29 +7058 2 Indirect_Address_30 +7060 2 Indirect_Address_31 +7062 2 Indirect_Address_32 +7064 2 Indirect_Address_33 +7066 2 Indirect_Address_34 +7068 2 Indirect_Address_35 +7070 2 Indirect_Address_36 +7072 2 Indirect_Address_37 +7074 2 Indirect_Address_38 +7076 2 Indirect_Address_39 +7078 2 Indirect_Address_40 +7080 2 Indirect_Address_41 +7082 2 Indirect_Address_42 +7084 2 Indirect_Address_43 +7086 2 Indirect_Address_44 +7088 2 Indirect_Address_45 +7090 2 Indirect_Address_46 +7092 2 Indirect_Address_47 +7094 2 Indirect_Address_48 +7096 2 Indirect_Address_49 +7098 2 Indirect_Address_50 +7100 2 Indirect_Address_51 +7102 2 Indirect_Address_52 +7104 2 Indirect_Address_53 +7106 2 Indirect_Address_54 +7108 2 Indirect_Address_55 +7110 2 Indirect_Address_56 +7112 2 Indirect_Address_57 +7114 2 Indirect_Address_58 +7116 2 Indirect_Address_59 +7118 2 Indirect_Address_60 +7120 2 Indirect_Address_61 +7122 2 Indirect_Address_62 +7124 2 Indirect_Address_63 +7126 2 Indirect_Address_64 +7128 2 Indirect_Address_65 +7130 2 Indirect_Address_66 +7132 2 Indirect_Address_67 +7134 2 Indirect_Address_68 +7136 2 Indirect_Address_69 +7138 2 Indirect_Address_70 +7140 2 Indirect_Address_71 +7142 2 Indirect_Address_72 +7144 2 Indirect_Address_73 +7146 2 Indirect_Address_74 +7148 2 Indirect_Address_75 +7150 2 Indirect_Address_76 +7152 2 Indirect_Address_77 +7154 2 Indirect_Address_78 +7156 2 Indirect_Address_79 +7158 2 Indirect_Address_80 +7160 2 Indirect_Address_81 +7162 2 Indirect_Address_82 +7164 2 Indirect_Address_83 +7166 2 Indirect_Address_84 +7168 2 Indirect_Address_85 +7170 2 Indirect_Address_86 +7172 2 Indirect_Address_87 +7174 2 Indirect_Address_88 +7176 2 Indirect_Address_89 +7178 2 Indirect_Address_90 +7180 2 Indirect_Address_91 +7182 2 Indirect_Address_92 +7184 2 Indirect_Address_93 +7186 2 Indirect_Address_94 +7188 2 Indirect_Address_95 +7190 2 Indirect_Address_96 +7192 2 Indirect_Address_97 +7194 2 Indirect_Address_98 +7196 2 Indirect_Address_99 +7198 2 Indirect_Address_100 +7200 2 Indirect_Address_101 +7202 2 Indirect_Address_102 +7204 2 Indirect_Address_103 +7206 2 Indirect_Address_104 +7208 2 Indirect_Address_105 +7210 2 Indirect_Address_106 +7212 2 Indirect_Address_107 +7214 2 Indirect_Address_108 +7216 2 Indirect_Address_109 +7218 2 Indirect_Address_110 +7220 2 Indirect_Address_111 +7222 2 Indirect_Address_112 +7224 2 Indirect_Address_113 +7226 2 Indirect_Address_114 +7228 2 Indirect_Address_115 +7230 2 Indirect_Address_116 +7232 2 Indirect_Address_117 +7234 2 Indirect_Address_118 +7236 2 Indirect_Address_119 +7238 2 Indirect_Address_120 +7240 2 Indirect_Address_121 +7242 2 Indirect_Address_122 +7244 2 Indirect_Address_123 +7246 2 Indirect_Address_124 +7248 2 Indirect_Address_125 +7250 2 Indirect_Address_126 +7252 2 Indirect_Address_127 +7254 2 Indirect_Address_128 +7256 2 Indirect_Address_129 +7258 2 Indirect_Address_130 +7260 2 Indirect_Address_131 +7262 2 Indirect_Address_132 +7264 2 Indirect_Address_133 +7266 2 Indirect_Address_134 +7268 2 Indirect_Address_135 +7270 2 Indirect_Address_136 +7272 2 Indirect_Address_137 +7274 2 Indirect_Address_138 +7276 2 Indirect_Address_139 +7278 2 Indirect_Address_140 +7280 2 Indirect_Address_141 +7282 2 Indirect_Address_142 +7284 2 Indirect_Address_143 +7286 2 Indirect_Address_144 +7288 2 Indirect_Address_145 +7290 2 Indirect_Address_146 +7292 2 Indirect_Address_147 +7294 2 Indirect_Address_148 +7296 2 Indirect_Address_149 +7298 2 Indirect_Address_150 +7300 2 Indirect_Address_151 +7302 2 Indirect_Address_152 +7304 2 Indirect_Address_153 +7306 2 Indirect_Address_154 +7308 2 Indirect_Address_155 +7310 2 Indirect_Address_156 +7312 2 Indirect_Address_157 +7314 2 Indirect_Address_158 +7316 2 Indirect_Address_159 +7318 2 Indirect_Address_160 +7320 2 Indirect_Address_161 +7322 2 Indirect_Address_162 +7324 2 Indirect_Address_163 +7326 2 Indirect_Address_164 +7328 2 Indirect_Address_165 +7330 2 Indirect_Address_166 +7332 2 Indirect_Address_167 +7334 2 Indirect_Address_168 +7336 2 Indirect_Address_169 +7338 2 Indirect_Address_170 +7340 2 Indirect_Address_171 +7342 2 Indirect_Address_172 +7344 2 Indirect_Address_173 +7346 2 Indirect_Address_174 +7348 2 Indirect_Address_175 +7350 2 Indirect_Address_176 +7352 2 Indirect_Address_177 +7354 2 Indirect_Address_178 +7356 2 Indirect_Address_179 +7358 2 Indirect_Address_180 +7360 2 Indirect_Address_181 +7362 2 Indirect_Address_182 +7364 2 Indirect_Address_183 +7366 2 Indirect_Address_184 +7368 2 Indirect_Address_185 +7370 2 Indirect_Address_186 +7372 2 Indirect_Address_187 +7374 2 Indirect_Address_188 +7376 2 Indirect_Address_189 +7378 2 Indirect_Address_190 +7380 2 Indirect_Address_191 +7382 2 Indirect_Address_192 +7384 2 Indirect_Address_193 +7386 2 Indirect_Address_194 +7388 2 Indirect_Address_195 +7390 2 Indirect_Address_196 +7392 2 Indirect_Address_197 +7394 2 Indirect_Address_198 +7396 2 Indirect_Address_199 +7398 2 Indirect_Address_200 +7400 2 Indirect_Address_201 +7402 2 Indirect_Address_202 +7404 2 Indirect_Address_203 +7406 2 Indirect_Address_204 +7408 2 Indirect_Address_205 +7410 2 Indirect_Address_206 +7412 2 Indirect_Address_207 +7414 2 Indirect_Address_208 +7416 2 Indirect_Address_209 +7418 2 Indirect_Address_210 +7420 2 Indirect_Address_211 +7422 2 Indirect_Address_212 +7424 2 Indirect_Address_213 +7426 2 Indirect_Address_214 +7428 2 Indirect_Address_215 +7430 2 Indirect_Address_216 +7432 2 Indirect_Address_217 +7434 2 Indirect_Address_218 +7436 2 Indirect_Address_219 +7438 2 Indirect_Address_220 +7440 2 Indirect_Address_221 +7442 2 Indirect_Address_222 +7444 2 Indirect_Address_223 +7446 2 Indirect_Address_224 +7448 2 Indirect_Address_225 +7450 2 Indirect_Address_226 +7452 2 Indirect_Address_227 +7454 2 Indirect_Address_228 +7456 2 Indirect_Address_229 +7458 2 Indirect_Address_230 +7460 2 Indirect_Address_231 +7462 2 Indirect_Address_232 +7464 2 Indirect_Address_233 +7466 2 Indirect_Address_234 +7468 2 Indirect_Address_235 +7470 2 Indirect_Address_236 +7472 2 Indirect_Address_237 +7474 2 Indirect_Address_238 +7476 2 Indirect_Address_239 +7478 2 Indirect_Address_240 +7480 2 Indirect_Address_241 +7482 2 Indirect_Address_242 +7484 2 Indirect_Address_243 +7486 2 Indirect_Address_244 +7488 2 Indirect_Address_245 +7490 2 Indirect_Address_246 +7492 2 Indirect_Address_247 +7494 2 Indirect_Address_248 +7496 2 Indirect_Address_249 +7498 2 Indirect_Address_250 +7500 2 Indirect_Address_251 +7502 2 Indirect_Address_252 +7504 2 Indirect_Address_253 +7506 2 Indirect_Address_254 +7508 2 Indirect_Address_255 +7510 2 Indirect_Address_256 +7512 2 Indirect_Address_257 +7514 2 Indirect_Address_258 +7516 2 Indirect_Address_259 +7518 2 Indirect_Address_260 +7520 2 Indirect_Address_261 +7522 2 Indirect_Address_262 +7524 2 Indirect_Address_263 +7526 2 Indirect_Address_264 +7528 2 Indirect_Address_265 +7530 2 Indirect_Address_266 +7532 2 Indirect_Address_267 +7534 2 Indirect_Address_268 +7536 2 Indirect_Address_269 +7538 2 Indirect_Address_270 +7540 2 Indirect_Address_271 +7542 2 Indirect_Address_272 +7544 2 Indirect_Address_273 +7546 2 Indirect_Address_274 +7548 2 Indirect_Address_275 +7550 2 Indirect_Address_276 +7552 2 Indirect_Address_277 +7554 2 Indirect_Address_278 +7556 2 Indirect_Address_279 +7558 2 Indirect_Address_280 +7560 2 Indirect_Address_281 +7562 2 Indirect_Address_282 +7564 2 Indirect_Address_283 +7566 2 Indirect_Address_284 +7568 2 Indirect_Address_285 +7570 2 Indirect_Address_286 +7572 2 Indirect_Address_287 +7574 2 Indirect_Address_288 +7576 2 Indirect_Address_289 +7578 2 Indirect_Address_290 +7580 2 Indirect_Address_291 +7582 2 Indirect_Address_292 +7584 2 Indirect_Address_293 +7586 2 Indirect_Address_294 +7588 2 Indirect_Address_295 +7590 2 Indirect_Address_296 +7592 2 Indirect_Address_297 +7594 2 Indirect_Address_298 +7596 2 Indirect_Address_299 +7598 2 Indirect_Address_300 +7600 2 Indirect_Address_301 +7602 2 Indirect_Address_302 +7604 2 Indirect_Address_303 +7606 2 Indirect_Address_304 +7608 2 Indirect_Address_305 +7610 2 Indirect_Address_306 +7612 2 Indirect_Address_307 +7614 2 Indirect_Address_308 +7616 2 Indirect_Address_309 +7618 2 Indirect_Address_310 +7620 2 Indirect_Address_311 +7622 2 Indirect_Address_312 +7624 2 Indirect_Address_313 +7626 2 Indirect_Address_314 +7628 2 Indirect_Address_315 +7630 2 Indirect_Address_316 +7632 2 Indirect_Address_317 +7634 2 Indirect_Address_318 +7636 2 Indirect_Address_319 +7638 2 Indirect_Address_320 +7640 2 Indirect_Address_321 +7642 2 Indirect_Address_322 +7644 2 Indirect_Address_323 +7646 2 Indirect_Address_324 +7648 2 Indirect_Address_325 +7650 2 Indirect_Address_326 +7652 2 Indirect_Address_327 +7654 2 Indirect_Address_328 +7656 2 Indirect_Address_329 +7658 2 Indirect_Address_330 +7660 2 Indirect_Address_331 +7662 2 Indirect_Address_332 +7664 2 Indirect_Address_333 +7666 2 Indirect_Address_334 +7668 2 Indirect_Address_335 +7670 2 Indirect_Address_336 +7672 2 Indirect_Address_337 +7674 2 Indirect_Address_338 +7676 2 Indirect_Address_339 +7678 2 Indirect_Address_340 +7680 2 Indirect_Address_341 +7682 2 Indirect_Address_342 +7684 2 Indirect_Address_343 +7686 2 Indirect_Address_344 +7688 2 Indirect_Address_345 +7690 2 Indirect_Address_346 +7692 2 Indirect_Address_347 +7694 2 Indirect_Address_348 +7696 2 Indirect_Address_349 +7698 2 Indirect_Address_350 +7700 2 Indirect_Address_351 +7702 2 Indirect_Address_352 +7704 2 Indirect_Address_353 +7706 2 Indirect_Address_354 +7708 2 Indirect_Address_355 +7710 2 Indirect_Address_356 +7712 2 Indirect_Address_357 +7714 2 Indirect_Address_358 +7716 2 Indirect_Address_359 +7718 2 Indirect_Address_360 +7720 2 Indirect_Address_361 +7722 2 Indirect_Address_362 +7724 2 Indirect_Address_363 +7726 2 Indirect_Address_364 +7728 2 Indirect_Address_365 +7730 2 Indirect_Address_366 +7732 2 Indirect_Address_367 +7734 2 Indirect_Address_368 +7736 2 Indirect_Address_369 +7738 2 Indirect_Address_370 +7740 2 Indirect_Address_371 +7742 2 Indirect_Address_372 +7744 2 Indirect_Address_373 +7746 2 Indirect_Address_374 +7748 2 Indirect_Address_375 +7750 2 Indirect_Address_376 +7752 2 Indirect_Address_377 +7754 2 Indirect_Address_378 +7756 2 Indirect_Address_379 +7758 2 Indirect_Address_380 +7760 2 Indirect_Address_381 +7762 2 Indirect_Address_382 +7764 2 Indirect_Address_383 +7766 2 Indirect_Address_384 +7768 2 Indirect_Address_385 +7770 2 Indirect_Address_386 +7772 2 Indirect_Address_387 +7774 2 Indirect_Address_388 +7776 2 Indirect_Address_389 +7778 2 Indirect_Address_390 +7780 2 Indirect_Address_391 +7782 2 Indirect_Address_392 +7784 2 Indirect_Address_393 +7786 2 Indirect_Address_394 +7788 2 Indirect_Address_395 +7790 2 Indirect_Address_396 +7792 2 Indirect_Address_397 +7794 2 Indirect_Address_398 +7796 2 Indirect_Address_399 +7798 2 Indirect_Address_400 +7800 2 Indirect_Address_401 +7802 2 Indirect_Address_402 +7804 2 Indirect_Address_403 +7806 2 Indirect_Address_404 +7808 2 Indirect_Address_405 +7810 2 Indirect_Address_406 +7812 2 Indirect_Address_407 +7814 2 Indirect_Address_408 +7816 2 Indirect_Address_409 +7818 2 Indirect_Address_410 +7820 2 Indirect_Address_411 +7822 2 Indirect_Address_412 +7824 2 Indirect_Address_413 +7826 2 Indirect_Address_414 +7828 2 Indirect_Address_415 +7830 2 Indirect_Address_416 +7832 2 Indirect_Address_417 +7834 2 Indirect_Address_418 +7836 2 Indirect_Address_419 +7838 2 Indirect_Address_420 +7840 2 Indirect_Address_421 +7842 2 Indirect_Address_422 +7844 2 Indirect_Address_423 +7846 2 Indirect_Address_424 +7848 2 Indirect_Address_425 +7850 2 Indirect_Address_426 +7852 2 Indirect_Address_427 +7854 2 Indirect_Address_428 +7856 2 Indirect_Address_429 +7858 2 Indirect_Address_430 +7860 2 Indirect_Address_431 +7862 2 Indirect_Address_432 +7864 2 Indirect_Address_433 +7866 2 Indirect_Address_434 +7868 2 Indirect_Address_435 +7870 2 Indirect_Address_436 +7872 2 Indirect_Address_437 +7874 2 Indirect_Address_438 +7876 2 Indirect_Address_439 +7878 2 Indirect_Address_440 +7880 2 Indirect_Address_441 +7882 2 Indirect_Address_442 +7884 2 Indirect_Address_443 +7886 2 Indirect_Address_444 +7888 2 Indirect_Address_445 +7890 2 Indirect_Address_446 +7892 2 Indirect_Address_447 +7894 2 Indirect_Address_448 +7896 2 Indirect_Address_449 +7898 2 Indirect_Address_450 +7900 2 Indirect_Address_451 +7902 2 Indirect_Address_452 +7904 2 Indirect_Address_453 +7906 2 Indirect_Address_454 +7908 2 Indirect_Address_455 +7910 2 Indirect_Address_456 +7912 2 Indirect_Address_457 +7914 2 Indirect_Address_458 +7916 2 Indirect_Address_459 +7918 2 Indirect_Address_460 +7920 2 Indirect_Address_461 +7922 2 Indirect_Address_462 +7924 2 Indirect_Address_463 +7926 2 Indirect_Address_464 +7928 2 Indirect_Address_465 +7930 2 Indirect_Address_466 +7932 2 Indirect_Address_467 +7934 2 Indirect_Address_468 +7936 2 Indirect_Address_469 +7938 2 Indirect_Address_470 +7940 2 Indirect_Address_471 +7942 2 Indirect_Address_472 +7944 2 Indirect_Address_473 +7946 2 Indirect_Address_474 +7948 2 Indirect_Address_475 +7950 2 Indirect_Address_476 +7952 2 Indirect_Address_477 +7954 2 Indirect_Address_478 +7956 2 Indirect_Address_479 +7958 2 Indirect_Address_480 +7960 2 Indirect_Address_481 +7962 2 Indirect_Address_482 +7964 2 Indirect_Address_483 +7966 2 Indirect_Address_484 +7968 2 Indirect_Address_485 +7970 2 Indirect_Address_486 +7972 2 Indirect_Address_487 +7974 2 Indirect_Address_488 +7976 2 Indirect_Address_489 +7978 2 Indirect_Address_490 +7980 2 Indirect_Address_491 +7982 2 Indirect_Address_492 +7984 2 Indirect_Address_493 +7986 2 Indirect_Address_494 +7988 2 Indirect_Address_495 +7990 2 Indirect_Address_496 +7992 2 Indirect_Address_497 +7994 2 Indirect_Address_498 +7996 2 Indirect_Address_499 +7998 2 Indirect_Address_500 +8000 2 Indirect_Address_501 +8002 2 Indirect_Address_502 +8004 2 Indirect_Address_503 +8006 2 Indirect_Address_504 +8008 2 Indirect_Address_505 +8010 2 Indirect_Address_506 +8012 2 Indirect_Address_507 +8014 2 Indirect_Address_508 +8016 2 Indirect_Address_509 +8018 2 Indirect_Address_510 +8020 2 Indirect_Address_511 +8022 2 Indirect_Address_512 +10000 1 Indirect_Data_1 +10001 1 Indirect_Data_2 +10002 1 Indirect_Data_3 +10003 1 Indirect_Data_4 +10004 1 Indirect_Data_5 +10005 1 Indirect_Data_6 +10006 1 Indirect_Data_7 +10007 1 Indirect_Data_8 +10008 1 Indirect_Data_9 +10009 1 Indirect_Data_10 +10010 1 Indirect_Data_11 +10011 1 Indirect_Data_12 +10012 1 Indirect_Data_13 +10013 1 Indirect_Data_14 +10014 1 Indirect_Data_15 +10015 1 Indirect_Data_16 +10016 1 Indirect_Data_17 +10017 1 Indirect_Data_18 +10018 1 Indirect_Data_19 +10019 1 Indirect_Data_20 +10020 1 Indirect_Data_21 +10021 1 Indirect_Data_22 +10022 1 Indirect_Data_23 +10023 1 Indirect_Data_24 +10024 1 Indirect_Data_25 +10025 1 Indirect_Data_26 +10026 1 Indirect_Data_27 +10027 1 Indirect_Data_28 +10028 1 Indirect_Data_29 +10029 1 Indirect_Data_30 +10030 1 Indirect_Data_31 +10031 1 Indirect_Data_32 +10032 1 Indirect_Data_33 +10033 1 Indirect_Data_34 +10034 1 Indirect_Data_35 +10035 1 Indirect_Data_36 +10036 1 Indirect_Data_37 +10037 1 Indirect_Data_38 +10038 1 Indirect_Data_39 +10039 1 Indirect_Data_40 +10040 1 Indirect_Data_41 +10041 1 Indirect_Data_42 +10042 1 Indirect_Data_43 +10043 1 Indirect_Data_44 +10044 1 Indirect_Data_45 +10045 1 Indirect_Data_46 +10046 1 Indirect_Data_47 +10047 1 Indirect_Data_48 +10048 1 Indirect_Data_49 +10049 1 Indirect_Data_50 +10050 1 Indirect_Data_51 +10051 1 Indirect_Data_52 +10052 1 Indirect_Data_53 +10053 1 Indirect_Data_54 +10054 1 Indirect_Data_55 +10055 1 Indirect_Data_56 +10056 1 Indirect_Data_57 +10057 1 Indirect_Data_58 +10058 1 Indirect_Data_59 +10059 1 Indirect_Data_60 +10060 1 Indirect_Data_61 +10061 1 Indirect_Data_62 +10062 1 Indirect_Data_63 +10063 1 Indirect_Data_64 +10064 1 Indirect_Data_65 +10065 1 Indirect_Data_66 +10066 1 Indirect_Data_67 +10067 1 Indirect_Data_68 +10068 1 Indirect_Data_69 +10069 1 Indirect_Data_70 +10070 1 Indirect_Data_71 +10071 1 Indirect_Data_72 +10072 1 Indirect_Data_73 +10073 1 Indirect_Data_74 +10074 1 Indirect_Data_75 +10075 1 Indirect_Data_76 +10076 1 Indirect_Data_77 +10077 1 Indirect_Data_78 +10078 1 Indirect_Data_79 +10079 1 Indirect_Data_80 +10080 1 Indirect_Data_81 +10081 1 Indirect_Data_82 +10082 1 Indirect_Data_83 +10083 1 Indirect_Data_84 +10084 1 Indirect_Data_85 +10085 1 Indirect_Data_86 +10086 1 Indirect_Data_87 +10087 1 Indirect_Data_88 +10088 1 Indirect_Data_89 +10089 1 Indirect_Data_90 +10090 1 Indirect_Data_91 +10091 1 Indirect_Data_92 +10092 1 Indirect_Data_93 +10093 1 Indirect_Data_94 +10094 1 Indirect_Data_95 +10095 1 Indirect_Data_96 +10096 1 Indirect_Data_97 +10097 1 Indirect_Data_98 +10098 1 Indirect_Data_99 +10099 1 Indirect_Data_100 +10100 1 Indirect_Data_101 +10101 1 Indirect_Data_102 +10102 1 Indirect_Data_103 +10103 1 Indirect_Data_104 +10104 1 Indirect_Data_105 +10105 1 Indirect_Data_106 +10106 1 Indirect_Data_107 +10107 1 Indirect_Data_108 +10108 1 Indirect_Data_109 +10109 1 Indirect_Data_110 +10110 1 Indirect_Data_111 +10111 1 Indirect_Data_112 +10112 1 Indirect_Data_113 +10113 1 Indirect_Data_114 +10114 1 Indirect_Data_115 +10115 1 Indirect_Data_116 +10116 1 Indirect_Data_117 +10117 1 Indirect_Data_118 +10118 1 Indirect_Data_119 +10119 1 Indirect_Data_120 +10120 1 Indirect_Data_121 +10121 1 Indirect_Data_122 +10122 1 Indirect_Data_123 +10123 1 Indirect_Data_124 +10124 1 Indirect_Data_125 +10125 1 Indirect_Data_126 +10126 1 Indirect_Data_127 +10127 1 Indirect_Data_128 +10128 1 Indirect_Data_129 +10129 1 Indirect_Data_130 +10130 1 Indirect_Data_131 +10131 1 Indirect_Data_132 +10132 1 Indirect_Data_133 +10133 1 Indirect_Data_134 +10134 1 Indirect_Data_135 +10135 1 Indirect_Data_136 +10136 1 Indirect_Data_137 +10137 1 Indirect_Data_138 +10138 1 Indirect_Data_139 +10139 1 Indirect_Data_140 +10140 1 Indirect_Data_141 +10141 1 Indirect_Data_142 +10142 1 Indirect_Data_143 +10143 1 Indirect_Data_144 +10144 1 Indirect_Data_145 +10145 1 Indirect_Data_146 +10146 1 Indirect_Data_147 +10147 1 Indirect_Data_148 +10148 1 Indirect_Data_149 +10149 1 Indirect_Data_150 +10150 1 Indirect_Data_151 +10151 1 Indirect_Data_152 +10152 1 Indirect_Data_153 +10153 1 Indirect_Data_154 +10154 1 Indirect_Data_155 +10155 1 Indirect_Data_156 +10156 1 Indirect_Data_157 +10157 1 Indirect_Data_158 +10158 1 Indirect_Data_159 +10159 1 Indirect_Data_160 +10160 1 Indirect_Data_161 +10161 1 Indirect_Data_162 +10162 1 Indirect_Data_163 +10163 1 Indirect_Data_164 +10164 1 Indirect_Data_165 +10165 1 Indirect_Data_166 +10166 1 Indirect_Data_167 +10167 1 Indirect_Data_168 +10168 1 Indirect_Data_169 +10169 1 Indirect_Data_170 +10170 1 Indirect_Data_171 +10171 1 Indirect_Data_172 +10172 1 Indirect_Data_173 +10173 1 Indirect_Data_174 +10174 1 Indirect_Data_175 +10175 1 Indirect_Data_176 +10176 1 Indirect_Data_177 +10177 1 Indirect_Data_178 +10178 1 Indirect_Data_179 +10179 1 Indirect_Data_180 +10180 1 Indirect_Data_181 +10181 1 Indirect_Data_182 +10182 1 Indirect_Data_183 +10183 1 Indirect_Data_184 +10184 1 Indirect_Data_185 +10185 1 Indirect_Data_186 +10186 1 Indirect_Data_187 +10187 1 Indirect_Data_188 +10188 1 Indirect_Data_189 +10189 1 Indirect_Data_190 +10190 1 Indirect_Data_191 +10191 1 Indirect_Data_192 +10192 1 Indirect_Data_193 +10193 1 Indirect_Data_194 +10194 1 Indirect_Data_195 +10195 1 Indirect_Data_196 +10196 1 Indirect_Data_197 +10197 1 Indirect_Data_198 +10198 1 Indirect_Data_199 +10199 1 Indirect_Data_200 +10200 1 Indirect_Data_201 +10201 1 Indirect_Data_202 +10202 1 Indirect_Data_203 +10203 1 Indirect_Data_204 +10204 1 Indirect_Data_205 +10205 1 Indirect_Data_206 +10206 1 Indirect_Data_207 +10207 1 Indirect_Data_208 +10208 1 Indirect_Data_209 +10209 1 Indirect_Data_210 +10210 1 Indirect_Data_211 +10211 1 Indirect_Data_212 +10212 1 Indirect_Data_213 +10213 1 Indirect_Data_214 +10214 1 Indirect_Data_215 +10215 1 Indirect_Data_216 +10216 1 Indirect_Data_217 +10217 1 Indirect_Data_218 +10218 1 Indirect_Data_219 +10219 1 Indirect_Data_220 +10220 1 Indirect_Data_221 +10221 1 Indirect_Data_222 +10222 1 Indirect_Data_223 +10223 1 Indirect_Data_224 +10224 1 Indirect_Data_225 +10225 1 Indirect_Data_226 +10226 1 Indirect_Data_227 +10227 1 Indirect_Data_228 +10228 1 Indirect_Data_229 +10229 1 Indirect_Data_230 +10230 1 Indirect_Data_231 +10231 1 Indirect_Data_232 +10232 1 Indirect_Data_233 +10233 1 Indirect_Data_234 +10234 1 Indirect_Data_235 +10235 1 Indirect_Data_236 +10236 1 Indirect_Data_237 +10237 1 Indirect_Data_238 +10238 1 Indirect_Data_239 +10239 1 Indirect_Data_240 +10240 1 Indirect_Data_241 +10241 1 Indirect_Data_242 +10242 1 Indirect_Data_243 +10243 1 Indirect_Data_244 +10244 1 Indirect_Data_245 +10245 1 Indirect_Data_246 +10246 1 Indirect_Data_247 +10247 1 Indirect_Data_248 +10248 1 Indirect_Data_249 +10249 1 Indirect_Data_250 +10250 1 Indirect_Data_251 +10251 1 Indirect_Data_252 +10252 1 Indirect_Data_253 +10253 1 Indirect_Data_254 +10254 1 Indirect_Data_255 +10255 1 Indirect_Data_256 +10256 1 Indirect_Data_257 +10257 1 Indirect_Data_258 +10258 1 Indirect_Data_259 +10259 1 Indirect_Data_260 +10260 1 Indirect_Data_261 +10261 1 Indirect_Data_262 +10262 1 Indirect_Data_263 +10263 1 Indirect_Data_264 +10264 1 Indirect_Data_265 +10265 1 Indirect_Data_266 +10266 1 Indirect_Data_267 +10267 1 Indirect_Data_268 +10268 1 Indirect_Data_269 +10269 1 Indirect_Data_270 +10270 1 Indirect_Data_271 +10271 1 Indirect_Data_272 +10272 1 Indirect_Data_273 +10273 1 Indirect_Data_274 +10274 1 Indirect_Data_275 +10275 1 Indirect_Data_276 +10276 1 Indirect_Data_277 +10277 1 Indirect_Data_278 +10278 1 Indirect_Data_279 +10279 1 Indirect_Data_280 +10280 1 Indirect_Data_281 +10281 1 Indirect_Data_282 +10282 1 Indirect_Data_283 +10283 1 Indirect_Data_284 +10284 1 Indirect_Data_285 +10285 1 Indirect_Data_286 +10286 1 Indirect_Data_287 +10287 1 Indirect_Data_288 +10288 1 Indirect_Data_289 +10289 1 Indirect_Data_290 +10290 1 Indirect_Data_291 +10291 1 Indirect_Data_292 +10292 1 Indirect_Data_293 +10293 1 Indirect_Data_294 +10294 1 Indirect_Data_295 +10295 1 Indirect_Data_296 +10296 1 Indirect_Data_297 +10297 1 Indirect_Data_298 +10298 1 Indirect_Data_299 +10299 1 Indirect_Data_300 +10300 1 Indirect_Data_301 +10301 1 Indirect_Data_302 +10302 1 Indirect_Data_303 +10303 1 Indirect_Data_304 +10304 1 Indirect_Data_305 +10305 1 Indirect_Data_306 +10306 1 Indirect_Data_307 +10307 1 Indirect_Data_308 +10308 1 Indirect_Data_309 +10309 1 Indirect_Data_310 +10310 1 Indirect_Data_311 +10311 1 Indirect_Data_312 +10312 1 Indirect_Data_313 +10313 1 Indirect_Data_314 +10314 1 Indirect_Data_315 +10315 1 Indirect_Data_316 +10316 1 Indirect_Data_317 +10317 1 Indirect_Data_318 +10318 1 Indirect_Data_319 +10319 1 Indirect_Data_320 +10320 1 Indirect_Data_321 +10321 1 Indirect_Data_322 +10322 1 Indirect_Data_323 +10323 1 Indirect_Data_324 +10324 1 Indirect_Data_325 +10325 1 Indirect_Data_326 +10326 1 Indirect_Data_327 +10327 1 Indirect_Data_328 +10328 1 Indirect_Data_329 +10329 1 Indirect_Data_330 +10330 1 Indirect_Data_331 +10331 1 Indirect_Data_332 +10332 1 Indirect_Data_333 +10333 1 Indirect_Data_334 +10334 1 Indirect_Data_335 +10335 1 Indirect_Data_336 +10336 1 Indirect_Data_337 +10337 1 Indirect_Data_338 +10338 1 Indirect_Data_339 +10339 1 Indirect_Data_340 +10340 1 Indirect_Data_341 +10341 1 Indirect_Data_342 +10342 1 Indirect_Data_343 +10343 1 Indirect_Data_344 +10344 1 Indirect_Data_345 +10345 1 Indirect_Data_346 +10346 1 Indirect_Data_347 +10347 1 Indirect_Data_348 +10348 1 Indirect_Data_349 +10349 1 Indirect_Data_350 +10350 1 Indirect_Data_351 +10351 1 Indirect_Data_352 +10352 1 Indirect_Data_353 +10353 1 Indirect_Data_354 +10354 1 Indirect_Data_355 +10355 1 Indirect_Data_356 +10356 1 Indirect_Data_357 +10357 1 Indirect_Data_358 +10358 1 Indirect_Data_359 +10359 1 Indirect_Data_360 +10360 1 Indirect_Data_361 +10361 1 Indirect_Data_362 +10362 1 Indirect_Data_363 +10363 1 Indirect_Data_364 +10364 1 Indirect_Data_365 +10365 1 Indirect_Data_366 +10366 1 Indirect_Data_367 +10367 1 Indirect_Data_368 +10368 1 Indirect_Data_369 +10369 1 Indirect_Data_370 +10370 1 Indirect_Data_371 +10371 1 Indirect_Data_372 +10372 1 Indirect_Data_373 +10373 1 Indirect_Data_374 +10374 1 Indirect_Data_375 +10375 1 Indirect_Data_376 +10376 1 Indirect_Data_377 +10377 1 Indirect_Data_378 +10378 1 Indirect_Data_379 +10379 1 Indirect_Data_380 +10380 1 Indirect_Data_381 +10381 1 Indirect_Data_382 +10382 1 Indirect_Data_383 +10383 1 Indirect_Data_384 +10384 1 Indirect_Data_385 +10385 1 Indirect_Data_386 +10386 1 Indirect_Data_387 +10387 1 Indirect_Data_388 +10388 1 Indirect_Data_389 +10389 1 Indirect_Data_390 +10390 1 Indirect_Data_391 +10391 1 Indirect_Data_392 +10392 1 Indirect_Data_393 +10393 1 Indirect_Data_394 +10394 1 Indirect_Data_395 +10395 1 Indirect_Data_396 +10396 1 Indirect_Data_397 +10397 1 Indirect_Data_398 +10398 1 Indirect_Data_399 +10399 1 Indirect_Data_400 +10400 1 Indirect_Data_401 +10401 1 Indirect_Data_402 +10402 1 Indirect_Data_403 +10403 1 Indirect_Data_404 +10404 1 Indirect_Data_405 +10405 1 Indirect_Data_406 +10406 1 Indirect_Data_407 +10407 1 Indirect_Data_408 +10408 1 Indirect_Data_409 +10409 1 Indirect_Data_410 +10410 1 Indirect_Data_411 +10411 1 Indirect_Data_412 +10412 1 Indirect_Data_413 +10413 1 Indirect_Data_414 +10414 1 Indirect_Data_415 +10415 1 Indirect_Data_416 +10416 1 Indirect_Data_417 +10417 1 Indirect_Data_418 +10418 1 Indirect_Data_419 +10419 1 Indirect_Data_420 +10420 1 Indirect_Data_421 +10421 1 Indirect_Data_422 +10422 1 Indirect_Data_423 +10423 1 Indirect_Data_424 +10424 1 Indirect_Data_425 +10425 1 Indirect_Data_426 +10426 1 Indirect_Data_427 +10427 1 Indirect_Data_428 +10428 1 Indirect_Data_429 +10429 1 Indirect_Data_430 +10430 1 Indirect_Data_431 +10431 1 Indirect_Data_432 +10432 1 Indirect_Data_433 +10433 1 Indirect_Data_434 +10434 1 Indirect_Data_435 +10435 1 Indirect_Data_436 +10436 1 Indirect_Data_437 +10437 1 Indirect_Data_438 +10438 1 Indirect_Data_439 +10439 1 Indirect_Data_440 +10440 1 Indirect_Data_441 +10441 1 Indirect_Data_442 +10442 1 Indirect_Data_443 +10443 1 Indirect_Data_444 +10444 1 Indirect_Data_445 +10445 1 Indirect_Data_446 +10446 1 Indirect_Data_447 +10447 1 Indirect_Data_448 +10448 1 Indirect_Data_449 +10449 1 Indirect_Data_450 +10450 1 Indirect_Data_451 +10451 1 Indirect_Data_452 +10452 1 Indirect_Data_453 +10453 1 Indirect_Data_454 +10454 1 Indirect_Data_455 +10455 1 Indirect_Data_456 +10456 1 Indirect_Data_457 +10457 1 Indirect_Data_458 +10458 1 Indirect_Data_459 +10459 1 Indirect_Data_460 +10460 1 Indirect_Data_461 +10461 1 Indirect_Data_462 +10462 1 Indirect_Data_463 +10463 1 Indirect_Data_464 +10464 1 Indirect_Data_465 +10465 1 Indirect_Data_466 +10466 1 Indirect_Data_467 +10467 1 Indirect_Data_468 +10468 1 Indirect_Data_469 +10469 1 Indirect_Data_470 +10470 1 Indirect_Data_471 +10471 1 Indirect_Data_472 +10472 1 Indirect_Data_473 +10473 1 Indirect_Data_474 +10474 1 Indirect_Data_475 +10475 1 Indirect_Data_476 +10476 1 Indirect_Data_477 +10477 1 Indirect_Data_478 +10478 1 Indirect_Data_479 +10479 1 Indirect_Data_480 +10480 1 Indirect_Data_481 +10481 1 Indirect_Data_482 +10482 1 Indirect_Data_483 +10483 1 Indirect_Data_484 +10484 1 Indirect_Data_485 +10485 1 Indirect_Data_486 +10486 1 Indirect_Data_487 +10487 1 Indirect_Data_488 +10488 1 Indirect_Data_489 +10489 1 Indirect_Data_490 +10490 1 Indirect_Data_491 +10491 1 Indirect_Data_492 +10492 1 Indirect_Data_493 +10493 1 Indirect_Data_494 +10494 1 Indirect_Data_495 +10495 1 Indirect_Data_496 +10496 1 Indirect_Data_497 +10497 1 Indirect_Data_498 +10498 1 Indirect_Data_499 +10499 1 Indirect_Data_500 +10500 1 Indirect_Data_501 +10501 1 Indirect_Data_502 +10502 1 Indirect_Data_503 +10503 1 Indirect_Data_504 +10504 1 Indirect_Data_505 +10505 1 Indirect_Data_506 +10506 1 Indirect_Data_507 +10507 1 Indirect_Data_508 +10508 1 Indirect_Data_509 +10509 1 Indirect_Data_510 +10510 1 Indirect_Data_511 +10511 1 Indirect_Data_512 +7000 2 Indirect Address Write +10000 1 Indirect Data Write +7510 2 Indirect Address Read +10255 1 Indirect Data Read diff --git a/param/dxl_model/ffw_sg2_drive_1.model b/param/dxl_model/ffw_sg2_drive_1.model new file mode 100644 index 0000000..1d84d3d --- /dev/null +++ b/param/dxl_model/ffw_sg2_drive_1.model @@ -0,0 +1,382 @@ +[type info] +name value +value_of_zero_radian_position 0 +value_of_max_radian_position 262144 +value_of_min_radian_position -262144 +min_radian -3.14159265 +max_radian 3.14159265 +velocity_unit 0.01 + +[control table] +Address Size Data Name +110 2 Model Number +112 4 Model Information +116 1 Firmware Version +117 1 ID +118 1 Bus Watchdog +120 1 Virtual ID +121 1 Protocol Type +122 1 Baud Rate (Bus) +123 1 Return Delay Time +124 1 Bit Rate (CAN) +125 1 Status Return Level +126 1 Registered Instruction +127 1 Dump Control +128 2 Dump Time +130 2 Dump Step Time +132 2 Dump Interval Time +134 1 Dump Item +138 4 Build Date +142 1 Drive Mode +143 1 Operating Mode +144 1 Startup Configuration +150 4 In-Position Threshold +154 4 Following Error Threshold +158 4 Moving Threshold +162 4 Homing Offset +166 1 Temperature Limit +167 1 Temperature Limit (Motor) +170 2 Max Voltage Limit +172 2 Min Voltage Limit +174 2 PWM Limit +176 2 Current Limit +178 4 Acceleration Limit +182 4 Velocity Limit +186 4 Max Position Limit +190 4 Min Position Limit +206 4 GearRatio Num +210 4 GearRatio Den +214 2 Safe Stop Time +216 2 Brake Delay +218 2 Goal Update Delay +220 1 Overexcitation Voltage +221 1 Normal Excitation Voltage +222 2 Overexcitation Time +224 2 Goal Current BSF Freqency +226 2 Goal Current BSF Bandwith. +228 2 Goal Current BSF Depth. +230 2 Present Velocity LPF Freqency +232 2 Goal Current LPF Freqency +234 2 Position FF LPF Freqency +236 2 Velocity FF LPF Freqency +249 1 Controller State +292 1 Error Code +293 1 Error Code History 1 +294 1 Error Code History 2 +295 1 Error Code History 3 +296 1 Error Code History 4 +297 1 Error Code History 5 +298 1 Error Code History 6 +299 1 Error Code History 7 +300 1 Error Code History 8 +301 1 Error Code History 9 +302 1 Error Code History 10 +303 1 Error Code History 11 +304 1 Error Code History 12 +305 1 Error Code History 13 +306 1 Error Code History 14 +307 1 Error Code History 15 +308 1 Error Code History 16 +366 2 Indirect Address 1 +368 2 Indirect Address 2 +370 2 Indirect Address 3 +372 2 Indirect Address 4 +374 2 Indirect Address 5 +376 2 Indirect Address 6 +378 2 Indirect Address 7 +380 2 Indirect Address 8 +382 2 Indirect Address 9 +384 2 Indirect Address 10 +386 2 Indirect Address 11 +388 2 Indirect Address 12 +390 2 Indirect Address 13 +392 2 Indirect Address 14 +394 2 Indirect Address 15 +396 2 Indirect Address 16 +398 2 Indirect Address 17 +400 2 Indirect Address 18 +402 2 Indirect Address 19 +404 2 Indirect Address 20 +406 2 Indirect Address 21 +408 2 Indirect Address 22 +410 2 Indirect Address 23 +412 2 Indirect Address 24 +414 2 Indirect Address 25 +416 2 Indirect Address 26 +418 2 Indirect Address 27 +420 2 Indirect Address 28 +422 2 Indirect Address 29 +424 2 Indirect Address 30 +426 2 Indirect Address 31 +428 2 Indirect Address 32 +430 2 Indirect Address 33 +432 2 Indirect Address 34 +434 2 Indirect Address 35 +436 2 Indirect Address 36 +438 2 Indirect Address 37 +440 2 Indirect Address 38 +442 2 Indirect Address 39 +444 2 Indirect Address 40 +446 2 Indirect Address 41 +448 2 Indirect Address 42 +450 2 Indirect Address 43 +452 2 Indirect Address 44 +454 2 Indirect Address 45 +456 2 Indirect Address 46 +458 2 Indirect Address 47 +460 2 Indirect Address 48 +462 2 Indirect Address 49 +464 2 Indirect Address 50 +466 2 Indirect Address 51 +468 2 Indirect Address 52 +470 2 Indirect Address 53 +472 2 Indirect Address 54 +474 2 Indirect Address 55 +476 2 Indirect Address 56 +478 2 Indirect Address 57 +480 2 Indirect Address 58 +482 2 Indirect Address 59 +484 2 Indirect Address 60 +486 2 Indirect Address 61 +488 2 Indirect Address 62 +490 2 Indirect Address 63 +492 2 Indirect Address 64 +494 2 Indirect Address 65 +496 2 Indirect Address 66 +498 2 Indirect Address 67 +500 2 Indirect Address 68 +502 2 Indirect Address 69 +504 2 Indirect Address 70 +506 2 Indirect Address 71 +508 2 Indirect Address 72 +510 2 Indirect Address 73 +512 2 Indirect Address 74 +514 2 Indirect Address 75 +516 2 Indirect Address 76 +518 2 Indirect Address 77 +520 2 Indirect Address 78 +522 2 Indirect Address 79 +524 2 Indirect Address 80 +526 2 Indirect Address 81 +528 2 Indirect Address 82 +530 2 Indirect Address 83 +532 2 Indirect Address 84 +534 2 Indirect Address 85 +536 2 Indirect Address 86 +538 2 Indirect Address 87 +540 2 Indirect Address 88 +542 2 Indirect Address 89 +544 2 Indirect Address 90 +546 2 Indirect Address 91 +548 2 Indirect Address 92 +550 2 Indirect Address 93 +552 2 Indirect Address 94 +554 2 Indirect Address 95 +556 2 Indirect Address 96 +558 2 Indirect Address 97 +560 2 Indirect Address 98 +562 2 Indirect Address 99 +564 2 Indirect Address 100 +566 2 Indirect Address 101 +568 2 Indirect Address 102 +570 2 Indirect Address 103 +572 2 Indirect Address 104 +574 2 Indirect Address 105 +576 2 Indirect Address 106 +578 2 Indirect Address 107 +580 2 Indirect Address 108 +582 2 Indirect Address 109 +584 2 Indirect Address 110 +586 2 Indirect Address 111 +588 2 Indirect Address 112 +590 2 Indirect Address 113 +592 2 Indirect Address 114 +594 2 Indirect Address 115 +596 2 Indirect Address 116 +598 2 Indirect Address 117 +600 2 Indirect Address 118 +602 2 Indirect Address 119 +604 2 Indirect Address 120 +606 2 Indirect Address 121 +608 2 Indirect Address 122 +610 2 Indirect Address 123 +612 2 Indirect Address 124 +614 2 Indirect Address 125 +616 2 Indirect Address 126 +618 2 Indirect Address 127 +620 2 Indirect Address 128 +622 1 Torque Enable +623 1 LED +624 1 Gain Save +662 4 Current I Gain +666 4 Current P Gain +670 4 Velocity I Gain +674 4 Velocity P Gain +678 4 Velocity FF Gain +682 4 Position D Gain +686 4 Position I Gain +690 4 Position P Gain +694 4 Position FF Gain +698 4 Profile Acceleration +702 4 Profile Velocity +706 4 Profile Acceleration Time +710 4 Profile Time +714 2 PWM Offset +716 2 Current Offset +718 4 Velocity Offset +722 2 Goal PWM +724 2 Goal Current +726 4 Goal Velocity +730 4 Goal Position +734 4 Goal Position (H) +739 1 Moving Status +740 2 Realtime Tick +742 2 Present PWM +744 2 Present Current +746 4 Present Velocity +750 4 Present Position +754 4 Present Position (H) +758 4 Position Trajectory +762 4 Velocity Trajectory +766 2 Present Input Voltage +768 1 Present Temperature +769 1 Present Temperature (Motor) +798 2 Voltage A +800 2 Voltage B +802 2 Voltage C +804 2 Present Impact +824 2 Current A +826 2 Current B +828 2 Current C +830 4 Present Single Position +834 4 Present Multi Position +838 2 Elec. Angle +1001 1 Backup Ready +1006 1 Indirect Data 1 +1007 1 Indirect Data 2 +1008 1 Indirect Data 3 +1009 1 Indirect Data 4 +1010 1 Indirect Data 5 +1011 1 Indirect Data 6 +1012 1 Indirect Data 7 +1013 1 Indirect Data 8 +1014 1 Indirect Data 9 +1015 1 Indirect Data 10 +1016 1 Indirect Data 11 +1017 1 Indirect Data 12 +1018 1 Indirect Data 13 +1019 1 Indirect Data 14 +1020 1 Indirect Data 15 +1021 1 Indirect Data 16 +1022 1 Indirect Data 17 +1023 1 Indirect Data 18 +1024 1 Indirect Data 19 +1025 1 Indirect Data 20 +1026 1 Indirect Data 21 +1027 1 Indirect Data 22 +1028 1 Indirect Data 23 +1029 1 Indirect Data 24 +1030 1 Indirect Data 25 +1031 1 Indirect Data 26 +1032 1 Indirect Data 27 +1033 1 Indirect Data 28 +1034 1 Indirect Data 29 +1035 1 Indirect Data 30 +1036 1 Indirect Data 31 +1037 1 Indirect Data 32 +1038 1 Indirect Data 33 +1039 1 Indirect Data 34 +1040 1 Indirect Data 35 +1041 1 Indirect Data 36 +1042 1 Indirect Data 37 +1043 1 Indirect Data 38 +1044 1 Indirect Data 39 +1045 1 Indirect Data 40 +1046 1 Indirect Data 41 +1047 1 Indirect Data 42 +1048 1 Indirect Data 43 +1049 1 Indirect Data 44 +1050 1 Indirect Data 45 +1051 1 Indirect Data 46 +1052 1 Indirect Data 47 +1053 1 Indirect Data 48 +1054 1 Indirect Data 49 +1055 1 Indirect Data 50 +1056 1 Indirect Data 51 +1057 1 Indirect Data 52 +1058 1 Indirect Data 53 +1059 1 Indirect Data 54 +1060 1 Indirect Data 55 +1061 1 Indirect Data 56 +1062 1 Indirect Data 57 +1063 1 Indirect Data 58 +1064 1 Indirect Data 59 +1065 1 Indirect Data 60 +1066 1 Indirect Data 61 +1067 1 Indirect Data 62 +1068 1 Indirect Data 63 +1069 1 Indirect Data 64 +1070 1 Indirect Data 65 +1071 1 Indirect Data 66 +1072 1 Indirect Data 67 +1073 1 Indirect Data 68 +1074 1 Indirect Data 69 +1075 1 Indirect Data 70 +1076 1 Indirect Data 71 +1077 1 Indirect Data 72 +1078 1 Indirect Data 73 +1079 1 Indirect Data 74 +1080 1 Indirect Data 75 +1081 1 Indirect Data 76 +1082 1 Indirect Data 77 +1083 1 Indirect Data 78 +1084 1 Indirect Data 79 +1085 1 Indirect Data 80 +1086 1 Indirect Data 81 +1087 1 Indirect Data 82 +1088 1 Indirect Data 83 +1089 1 Indirect Data 84 +1090 1 Indirect Data 85 +1091 1 Indirect Data 86 +1092 1 Indirect Data 87 +1093 1 Indirect Data 88 +1094 1 Indirect Data 89 +1095 1 Indirect Data 90 +1096 1 Indirect Data 91 +1097 1 Indirect Data 92 +1098 1 Indirect Data 93 +1099 1 Indirect Data 94 +1100 1 Indirect Data 95 +1101 1 Indirect Data 96 +1102 1 Indirect Data 97 +1103 1 Indirect Data 98 +1104 1 Indirect Data 99 +1105 1 Indirect Data 100 +1106 1 Indirect Data 101 +1107 1 Indirect Data 102 +1108 1 Indirect Data 103 +1109 1 Indirect Data 104 +1110 1 Indirect Data 105 +1111 1 Indirect Data 106 +1112 1 Indirect Data 107 +1113 1 Indirect Data 108 +1114 1 Indirect Data 109 +1115 1 Indirect Data 110 +1116 1 Indirect Data 111 +1117 1 Indirect Data 112 +1118 1 Indirect Data 113 +1119 1 Indirect Data 114 +1120 1 Indirect Data 115 +1121 1 Indirect Data 116 +1122 1 Indirect Data 117 +1123 1 Indirect Data 118 +1124 1 Indirect Data 119 +1125 1 Indirect Data 120 +1126 1 Indirect Data 121 +1127 1 Indirect Data 122 +1128 1 Indirect Data 123 +1129 1 Indirect Data 124 +1130 1 Indirect Data 125 +1131 1 Indirect Data 126 +1132 1 Indirect Data 127 +1133 1 Indirect Data 128 diff --git a/param/dxl_model/ffw_sg2_drive_2.model b/param/dxl_model/ffw_sg2_drive_2.model new file mode 100644 index 0000000..dedb967 --- /dev/null +++ b/param/dxl_model/ffw_sg2_drive_2.model @@ -0,0 +1,382 @@ +[type info] +name value +value_of_zero_radian_position 0 +value_of_max_radian_position 262144 +value_of_min_radian_position -262144 +min_radian -3.14159265 +max_radian 3.14159265 +velocity_unit 0.01 + +[control table] +Address Size Data Name +1134 2 Model Number +1136 4 Model Information +1140 1 Firmware Version +1141 1 ID +1142 1 Bus Watchdog +1144 1 Virtual ID +1145 1 Protocol Type +1146 1 Baud Rate (Bus) +1147 1 Return Delay Time +1148 1 Bit Rate (CAN) +1149 1 Status Return Level +1150 1 Registered Instruction +1151 1 Dump Control +1152 2 Dump Time +1154 2 Dump Step Time +1156 2 Dump Interval Time +1158 1 Dump Item +1162 4 Build Date +1166 1 Drive Mode +1167 1 Operating Mode +1168 1 Startup Configuration +1174 4 In-Position Threshold +1178 4 Following Error Threshold +1182 4 Moving Threshold +1186 4 Homing Offset +1190 1 Temperature Limit +1191 1 Temperature Limit (Motor) +1194 2 Max Voltage Limit +1196 2 Min Voltage Limit +1198 2 PWM Limit +1200 2 Current Limit +1202 4 Acceleration Limit +1206 4 Velocity Limit +1210 4 Max Position Limit +1214 4 Min Position Limit +1230 4 GearRatio Num +1234 4 GearRatio Den +1238 2 Safe Stop Time +1240 2 Brake Delay +1242 2 Goal Update Delay +1244 1 Overexcitation Voltage +1245 1 Normal Excitation Voltage +1246 2 Overexcitation Time +1248 2 Goal Current BSF Freqency +1250 2 Goal Current BSF Bandwith. +1252 2 Goal Current BSF Depth. +1254 2 Present Velocity LPF Freqency +1256 2 Goal Current LPF Freqency +1258 2 Position FF LPF Freqency +1260 2 Velocity FF LPF Freqency +1273 1 Controller State +1316 1 Error Code +1317 1 Error Code History 1 +1318 1 Error Code History 2 +1319 1 Error Code History 3 +1320 1 Error Code History 4 +1321 1 Error Code History 5 +1322 1 Error Code History 6 +1323 1 Error Code History 7 +1324 1 Error Code History 8 +1325 1 Error Code History 9 +1326 1 Error Code History 10 +1327 1 Error Code History 11 +1328 1 Error Code History 12 +1329 1 Error Code History 13 +1330 1 Error Code History 14 +1331 1 Error Code History 15 +1332 1 Error Code History 16 +1390 2 Indirect Address 1 +1392 2 Indirect Address 2 +1394 2 Indirect Address 3 +1396 2 Indirect Address 4 +1398 2 Indirect Address 5 +1400 2 Indirect Address 6 +1402 2 Indirect Address 7 +1404 2 Indirect Address 8 +1406 2 Indirect Address 9 +1408 2 Indirect Address 10 +1410 2 Indirect Address 11 +1412 2 Indirect Address 12 +1414 2 Indirect Address 13 +1416 2 Indirect Address 14 +1418 2 Indirect Address 15 +1420 2 Indirect Address 16 +1422 2 Indirect Address 17 +1424 2 Indirect Address 18 +1426 2 Indirect Address 19 +1428 2 Indirect Address 20 +1430 2 Indirect Address 21 +1432 2 Indirect Address 22 +1434 2 Indirect Address 23 +1436 2 Indirect Address 24 +1438 2 Indirect Address 25 +1440 2 Indirect Address 26 +1442 2 Indirect Address 27 +1444 2 Indirect Address 28 +1446 2 Indirect Address 29 +1448 2 Indirect Address 30 +1450 2 Indirect Address 31 +1452 2 Indirect Address 32 +1454 2 Indirect Address 33 +1456 2 Indirect Address 34 +1458 2 Indirect Address 35 +1460 2 Indirect Address 36 +1462 2 Indirect Address 37 +1464 2 Indirect Address 38 +1466 2 Indirect Address 39 +1468 2 Indirect Address 40 +1470 2 Indirect Address 41 +1472 2 Indirect Address 42 +1474 2 Indirect Address 43 +1476 2 Indirect Address 44 +1478 2 Indirect Address 45 +1480 2 Indirect Address 46 +1482 2 Indirect Address 47 +1484 2 Indirect Address 48 +1486 2 Indirect Address 49 +1488 2 Indirect Address 50 +1490 2 Indirect Address 51 +1492 2 Indirect Address 52 +1494 2 Indirect Address 53 +1496 2 Indirect Address 54 +1498 2 Indirect Address 55 +1500 2 Indirect Address 56 +1502 2 Indirect Address 57 +1504 2 Indirect Address 58 +1506 2 Indirect Address 59 +1508 2 Indirect Address 60 +1510 2 Indirect Address 61 +1512 2 Indirect Address 62 +1514 2 Indirect Address 63 +1516 2 Indirect Address 64 +1518 2 Indirect Address 65 +1520 2 Indirect Address 66 +1522 2 Indirect Address 67 +1524 2 Indirect Address 68 +1526 2 Indirect Address 69 +1528 2 Indirect Address 70 +1530 2 Indirect Address 71 +1532 2 Indirect Address 72 +1534 2 Indirect Address 73 +1536 2 Indirect Address 74 +1538 2 Indirect Address 75 +1540 2 Indirect Address 76 +1542 2 Indirect Address 77 +1544 2 Indirect Address 78 +1546 2 Indirect Address 79 +1548 2 Indirect Address 80 +1550 2 Indirect Address 81 +1552 2 Indirect Address 82 +1554 2 Indirect Address 83 +1556 2 Indirect Address 84 +1558 2 Indirect Address 85 +1560 2 Indirect Address 86 +1562 2 Indirect Address 87 +1564 2 Indirect Address 88 +1566 2 Indirect Address 89 +1568 2 Indirect Address 90 +1570 2 Indirect Address 91 +1572 2 Indirect Address 92 +1574 2 Indirect Address 93 +1576 2 Indirect Address 94 +1578 2 Indirect Address 95 +1580 2 Indirect Address 96 +1582 2 Indirect Address 97 +1584 2 Indirect Address 98 +1586 2 Indirect Address 99 +1588 2 Indirect Address 100 +1590 2 Indirect Address 101 +1592 2 Indirect Address 102 +1594 2 Indirect Address 103 +1596 2 Indirect Address 104 +1598 2 Indirect Address 105 +1600 2 Indirect Address 106 +1602 2 Indirect Address 107 +1604 2 Indirect Address 108 +1606 2 Indirect Address 109 +1608 2 Indirect Address 110 +1610 2 Indirect Address 111 +1612 2 Indirect Address 112 +1614 2 Indirect Address 113 +1616 2 Indirect Address 114 +1618 2 Indirect Address 115 +1620 2 Indirect Address 116 +1622 2 Indirect Address 117 +1624 2 Indirect Address 118 +1626 2 Indirect Address 119 +1628 2 Indirect Address 120 +1630 2 Indirect Address 121 +1632 2 Indirect Address 122 +1634 2 Indirect Address 123 +1636 2 Indirect Address 124 +1638 2 Indirect Address 125 +1640 2 Indirect Address 126 +1642 2 Indirect Address 127 +1644 2 Indirect Address 128 +1646 1 Torque Enable +1647 1 LED +1648 1 Gain Save +1686 4 Current I Gain +1690 4 Current P Gain +1694 4 Velocity I Gain +1698 4 Velocity P Gain +1702 4 Velocity FF Gain +1706 4 Position D Gain +1710 4 Position I Gain +1714 4 Position P Gain +1718 4 Position FF Gain +1722 4 Profile Acceleration +1726 4 Profile Velocity +1730 4 Profile Acceleration Time +1734 4 Profile Time +1738 2 PWM Offset +1740 2 Current Offset +1742 4 Velocity Offset +1746 2 Goal PWM +1748 2 Goal Current +1750 4 Goal Velocity +1754 4 Goal Position +1758 4 Goal Position (H) +1763 1 Moving Status +1764 2 Realtime Tick +1766 2 Present PWM +1768 2 Present Current +1770 4 Present Velocity +1774 4 Present Position +1778 4 Present Position (H) +1782 4 Position Trajectory +1786 4 Velocity Trajectory +1790 2 Present Input Voltage +1792 1 Present Temperature +1793 1 Present Temperature (Motor) +1822 2 Voltage A +1824 2 Voltage B +1826 2 Voltage C +1828 2 Present Impact +1848 2 Current A +1850 2 Current B +1852 2 Current C +1854 4 Present Single Position +1858 4 Present Multi Position +1862 2 Elec. Angle +2025 1 Backup Ready +2030 1 Indirect Data 1 +2031 1 Indirect Data 2 +2032 1 Indirect Data 3 +2033 1 Indirect Data 4 +2034 1 Indirect Data 5 +2035 1 Indirect Data 6 +2036 1 Indirect Data 7 +2037 1 Indirect Data 8 +2038 1 Indirect Data 9 +2039 1 Indirect Data 10 +2040 1 Indirect Data 11 +2041 1 Indirect Data 12 +2042 1 Indirect Data 13 +2043 1 Indirect Data 14 +2044 1 Indirect Data 15 +2045 1 Indirect Data 16 +2046 1 Indirect Data 17 +2047 1 Indirect Data 18 +2048 1 Indirect Data 19 +2049 1 Indirect Data 20 +2050 1 Indirect Data 21 +2051 1 Indirect Data 22 +2052 1 Indirect Data 23 +2053 1 Indirect Data 24 +2054 1 Indirect Data 25 +2055 1 Indirect Data 26 +2056 1 Indirect Data 27 +2057 1 Indirect Data 28 +2058 1 Indirect Data 29 +2059 1 Indirect Data 30 +2060 1 Indirect Data 31 +2061 1 Indirect Data 32 +2062 1 Indirect Data 33 +2063 1 Indirect Data 34 +2064 1 Indirect Data 35 +2065 1 Indirect Data 36 +2066 1 Indirect Data 37 +2067 1 Indirect Data 38 +2068 1 Indirect Data 39 +2069 1 Indirect Data 40 +2070 1 Indirect Data 41 +2071 1 Indirect Data 42 +2072 1 Indirect Data 43 +2073 1 Indirect Data 44 +2074 1 Indirect Data 45 +2075 1 Indirect Data 46 +2076 1 Indirect Data 47 +2077 1 Indirect Data 48 +2078 1 Indirect Data 49 +2079 1 Indirect Data 50 +2080 1 Indirect Data 51 +2081 1 Indirect Data 52 +2082 1 Indirect Data 53 +2083 1 Indirect Data 54 +2084 1 Indirect Data 55 +2085 1 Indirect Data 56 +2086 1 Indirect Data 57 +2087 1 Indirect Data 58 +2088 1 Indirect Data 59 +2089 1 Indirect Data 60 +2090 1 Indirect Data 61 +2091 1 Indirect Data 62 +2092 1 Indirect Data 63 +2093 1 Indirect Data 64 +2094 1 Indirect Data 65 +2095 1 Indirect Data 66 +2096 1 Indirect Data 67 +2097 1 Indirect Data 68 +2098 1 Indirect Data 69 +2099 1 Indirect Data 70 +2100 1 Indirect Data 71 +2101 1 Indirect Data 72 +2102 1 Indirect Data 73 +2103 1 Indirect Data 74 +2104 1 Indirect Data 75 +2105 1 Indirect Data 76 +2106 1 Indirect Data 77 +2107 1 Indirect Data 78 +2108 1 Indirect Data 79 +2109 1 Indirect Data 80 +2110 1 Indirect Data 81 +2111 1 Indirect Data 82 +2112 1 Indirect Data 83 +2113 1 Indirect Data 84 +2114 1 Indirect Data 85 +2115 1 Indirect Data 86 +2116 1 Indirect Data 87 +2117 1 Indirect Data 88 +2118 1 Indirect Data 89 +2119 1 Indirect Data 90 +2120 1 Indirect Data 91 +2121 1 Indirect Data 92 +2122 1 Indirect Data 93 +2123 1 Indirect Data 94 +2124 1 Indirect Data 95 +2125 1 Indirect Data 96 +2126 1 Indirect Data 97 +2127 1 Indirect Data 98 +2128 1 Indirect Data 99 +2129 1 Indirect Data 100 +2130 1 Indirect Data 101 +2131 1 Indirect Data 102 +2132 1 Indirect Data 103 +2133 1 Indirect Data 104 +2134 1 Indirect Data 105 +2135 1 Indirect Data 106 +2136 1 Indirect Data 107 +2137 1 Indirect Data 108 +2138 1 Indirect Data 109 +2139 1 Indirect Data 110 +2140 1 Indirect Data 111 +2141 1 Indirect Data 112 +2142 1 Indirect Data 113 +2143 1 Indirect Data 114 +2144 1 Indirect Data 115 +2145 1 Indirect Data 116 +2146 1 Indirect Data 117 +2147 1 Indirect Data 118 +2148 1 Indirect Data 119 +2149 1 Indirect Data 120 +2150 1 Indirect Data 121 +2151 1 Indirect Data 122 +2152 1 Indirect Data 123 +2153 1 Indirect Data 124 +2154 1 Indirect Data 125 +2155 1 Indirect Data 126 +2156 1 Indirect Data 127 +2157 1 Indirect Data 128 diff --git a/param/dxl_model/ffw_sg2_drive_3.model b/param/dxl_model/ffw_sg2_drive_3.model new file mode 100644 index 0000000..28c276e --- /dev/null +++ b/param/dxl_model/ffw_sg2_drive_3.model @@ -0,0 +1,382 @@ +[type info] +name value +value_of_zero_radian_position 0 +value_of_max_radian_position 262144 +value_of_min_radian_position -262144 +min_radian -3.14159265 +max_radian 3.14159265 +velocity_unit 0.01 + +[control table] +Address Size Data Name +2158 2 Model Number +2160 4 Model Information +2164 1 Firmware Version +2165 1 ID +2166 1 Bus Watchdog +2168 1 Virtual ID +2169 1 Protocol Type +2170 1 Baud Rate (Bus) +2171 1 Return Delay Time +2172 1 Bit Rate (CAN) +2173 1 Status Return Level +2174 1 Registered Instruction +2175 1 Dump Control +2176 2 Dump Time +2178 2 Dump Step Time +2180 2 Dump Interval Time +2182 1 Dump Item +2186 4 Build Date +2190 1 Drive Mode +2191 1 Operating Mode +2192 1 Startup Configuration +2198 4 In-Position Threshold +2202 4 Following Error Threshold +2206 4 Moving Threshold +2210 4 Homing Offset +2214 1 Temperature Limit +2215 1 Temperature Limit (Motor) +2218 2 Max Voltage Limit +2220 2 Min Voltage Limit +2222 2 PWM Limit +2224 2 Current Limit +2226 4 Acceleration Limit +2230 4 Velocity Limit +2234 4 Max Position Limit +2238 4 Min Position Limit +2254 4 GearRatio Num +2258 4 GearRatio Den +2262 2 Safe Stop Time +2264 2 Brake Delay +2266 2 Goal Update Delay +2268 1 Overexcitation Voltage +2269 1 Normal Excitation Voltage +2270 2 Overexcitation Time +2272 2 Goal Current BSF Freqency +2274 2 Goal Current BSF Bandwith. +2276 2 Goal Current BSF Depth. +2278 2 Present Velocity LPF Freqency +2280 2 Goal Current LPF Freqency +2282 2 Position FF LPF Freqency +2284 2 Velocity FF LPF Freqency +2297 1 Controller State +2340 1 Error Code +2341 1 Error Code History 1 +2342 1 Error Code History 2 +2343 1 Error Code History 3 +2344 1 Error Code History 4 +2345 1 Error Code History 5 +2346 1 Error Code History 6 +2347 1 Error Code History 7 +2348 1 Error Code History 8 +2349 1 Error Code History 9 +2350 1 Error Code History 10 +2351 1 Error Code History 11 +2352 1 Error Code History 12 +2353 1 Error Code History 13 +2354 1 Error Code History 14 +2355 1 Error Code History 15 +2356 1 Error Code History 16 +2414 2 Indirect Address 1 +2416 2 Indirect Address 2 +2418 2 Indirect Address 3 +2420 2 Indirect Address 4 +2422 2 Indirect Address 5 +2424 2 Indirect Address 6 +2426 2 Indirect Address 7 +2428 2 Indirect Address 8 +2430 2 Indirect Address 9 +2432 2 Indirect Address 10 +2434 2 Indirect Address 11 +2436 2 Indirect Address 12 +2438 2 Indirect Address 13 +2440 2 Indirect Address 14 +2442 2 Indirect Address 15 +2444 2 Indirect Address 16 +2446 2 Indirect Address 17 +2448 2 Indirect Address 18 +2450 2 Indirect Address 19 +2452 2 Indirect Address 20 +2454 2 Indirect Address 21 +2456 2 Indirect Address 22 +2458 2 Indirect Address 23 +2460 2 Indirect Address 24 +2462 2 Indirect Address 25 +2464 2 Indirect Address 26 +2466 2 Indirect Address 27 +2468 2 Indirect Address 28 +2470 2 Indirect Address 29 +2472 2 Indirect Address 30 +2474 2 Indirect Address 31 +2476 2 Indirect Address 32 +2478 2 Indirect Address 33 +2480 2 Indirect Address 34 +2482 2 Indirect Address 35 +2484 2 Indirect Address 36 +2486 2 Indirect Address 37 +2488 2 Indirect Address 38 +2490 2 Indirect Address 39 +2492 2 Indirect Address 40 +2494 2 Indirect Address 41 +2496 2 Indirect Address 42 +2498 2 Indirect Address 43 +2500 2 Indirect Address 44 +2502 2 Indirect Address 45 +2504 2 Indirect Address 46 +2506 2 Indirect Address 47 +2508 2 Indirect Address 48 +2510 2 Indirect Address 49 +2512 2 Indirect Address 50 +2514 2 Indirect Address 51 +2516 2 Indirect Address 52 +2518 2 Indirect Address 53 +2520 2 Indirect Address 54 +2522 2 Indirect Address 55 +2524 2 Indirect Address 56 +2526 2 Indirect Address 57 +2528 2 Indirect Address 58 +2530 2 Indirect Address 59 +2532 2 Indirect Address 60 +2534 2 Indirect Address 61 +2536 2 Indirect Address 62 +2538 2 Indirect Address 63 +2540 2 Indirect Address 64 +2542 2 Indirect Address 65 +2544 2 Indirect Address 66 +2546 2 Indirect Address 67 +2548 2 Indirect Address 68 +2550 2 Indirect Address 69 +2552 2 Indirect Address 70 +2554 2 Indirect Address 71 +2556 2 Indirect Address 72 +2558 2 Indirect Address 73 +2560 2 Indirect Address 74 +2562 2 Indirect Address 75 +2564 2 Indirect Address 76 +2566 2 Indirect Address 77 +2568 2 Indirect Address 78 +2570 2 Indirect Address 79 +2572 2 Indirect Address 80 +2574 2 Indirect Address 81 +2576 2 Indirect Address 82 +2578 2 Indirect Address 83 +2580 2 Indirect Address 84 +2582 2 Indirect Address 85 +2584 2 Indirect Address 86 +2586 2 Indirect Address 87 +2588 2 Indirect Address 88 +2590 2 Indirect Address 89 +2592 2 Indirect Address 90 +2594 2 Indirect Address 91 +2596 2 Indirect Address 92 +2598 2 Indirect Address 93 +2600 2 Indirect Address 94 +2602 2 Indirect Address 95 +2604 2 Indirect Address 96 +2606 2 Indirect Address 97 +2608 2 Indirect Address 98 +2610 2 Indirect Address 99 +2612 2 Indirect Address 100 +2614 2 Indirect Address 101 +2616 2 Indirect Address 102 +2618 2 Indirect Address 103 +2620 2 Indirect Address 104 +2622 2 Indirect Address 105 +2624 2 Indirect Address 106 +2626 2 Indirect Address 107 +2628 2 Indirect Address 108 +2630 2 Indirect Address 109 +2632 2 Indirect Address 110 +2634 2 Indirect Address 111 +2636 2 Indirect Address 112 +2638 2 Indirect Address 113 +2640 2 Indirect Address 114 +2642 2 Indirect Address 115 +2644 2 Indirect Address 116 +2646 2 Indirect Address 117 +2648 2 Indirect Address 118 +2650 2 Indirect Address 119 +2652 2 Indirect Address 120 +2654 2 Indirect Address 121 +2656 2 Indirect Address 122 +2658 2 Indirect Address 123 +2660 2 Indirect Address 124 +2662 2 Indirect Address 125 +2664 2 Indirect Address 126 +2666 2 Indirect Address 127 +2668 2 Indirect Address 128 +2670 1 Torque Enable +2671 1 LED +2672 1 Gain Save +2710 4 Current I Gain +2714 4 Current P Gain +2718 4 Velocity I Gain +2722 4 Velocity P Gain +2726 4 Velocity FF Gain +2730 4 Position D Gain +2734 4 Position I Gain +2738 4 Position P Gain +2742 4 Position FF Gain +2746 4 Profile Acceleration +2750 4 Profile Velocity +2754 4 Profile Acceleration Time +2758 4 Profile Time +2762 2 PWM Offset +2764 2 Current Offset +2766 4 Velocity Offset +2770 2 Goal PWM +2772 2 Goal Current +2774 4 Goal Velocity +2778 4 Goal Position +2782 4 Goal Position (H) +2787 1 Moving Status +2788 2 Realtime Tick +2790 2 Present PWM +2792 2 Present Current +2794 4 Present Velocity +2798 4 Present Position +2802 4 Present Position (H) +2806 4 Position Trajectory +2810 4 Velocity Trajectory +2814 2 Present Input Voltage +2816 1 Present Temperature +2817 1 Present Temperature (Motor) +2846 2 Voltage A +2848 2 Voltage B +2850 2 Voltage C +2852 2 Present Impact +2872 2 Current A +2874 2 Current B +2876 2 Current C +2878 4 Present Single Position +2882 4 Present Multi Position +2886 2 Elec. Angle +3049 1 Backup Ready +3054 1 Indirect Data 1 +3055 1 Indirect Data 2 +3056 1 Indirect Data 3 +3057 1 Indirect Data 4 +3058 1 Indirect Data 5 +3059 1 Indirect Data 6 +3060 1 Indirect Data 7 +3061 1 Indirect Data 8 +3062 1 Indirect Data 9 +3063 1 Indirect Data 10 +3064 1 Indirect Data 11 +3065 1 Indirect Data 12 +3066 1 Indirect Data 13 +3067 1 Indirect Data 14 +3068 1 Indirect Data 15 +3069 1 Indirect Data 16 +3070 1 Indirect Data 17 +3071 1 Indirect Data 18 +3072 1 Indirect Data 19 +3073 1 Indirect Data 20 +3074 1 Indirect Data 21 +3075 1 Indirect Data 22 +3076 1 Indirect Data 23 +3077 1 Indirect Data 24 +3078 1 Indirect Data 25 +3079 1 Indirect Data 26 +3080 1 Indirect Data 27 +3081 1 Indirect Data 28 +3082 1 Indirect Data 29 +3083 1 Indirect Data 30 +3084 1 Indirect Data 31 +3085 1 Indirect Data 32 +3086 1 Indirect Data 33 +3087 1 Indirect Data 34 +3088 1 Indirect Data 35 +3089 1 Indirect Data 36 +3090 1 Indirect Data 37 +3091 1 Indirect Data 38 +3092 1 Indirect Data 39 +3093 1 Indirect Data 40 +3094 1 Indirect Data 41 +3095 1 Indirect Data 42 +3096 1 Indirect Data 43 +3097 1 Indirect Data 44 +3098 1 Indirect Data 45 +3099 1 Indirect Data 46 +3100 1 Indirect Data 47 +3101 1 Indirect Data 48 +3102 1 Indirect Data 49 +3103 1 Indirect Data 50 +3104 1 Indirect Data 51 +3105 1 Indirect Data 52 +3106 1 Indirect Data 53 +3107 1 Indirect Data 54 +3108 1 Indirect Data 55 +3109 1 Indirect Data 56 +3110 1 Indirect Data 57 +3111 1 Indirect Data 58 +3112 1 Indirect Data 59 +3113 1 Indirect Data 60 +3114 1 Indirect Data 61 +3115 1 Indirect Data 62 +3116 1 Indirect Data 63 +3117 1 Indirect Data 64 +3118 1 Indirect Data 65 +3119 1 Indirect Data 66 +3120 1 Indirect Data 67 +3121 1 Indirect Data 68 +3122 1 Indirect Data 69 +3123 1 Indirect Data 70 +3124 1 Indirect Data 71 +3125 1 Indirect Data 72 +3126 1 Indirect Data 73 +3127 1 Indirect Data 74 +3128 1 Indirect Data 75 +3129 1 Indirect Data 76 +3130 1 Indirect Data 77 +3131 1 Indirect Data 78 +3132 1 Indirect Data 79 +3133 1 Indirect Data 80 +3134 1 Indirect Data 81 +3135 1 Indirect Data 82 +3136 1 Indirect Data 83 +3137 1 Indirect Data 84 +3138 1 Indirect Data 85 +3139 1 Indirect Data 86 +3140 1 Indirect Data 87 +3141 1 Indirect Data 88 +3142 1 Indirect Data 89 +3143 1 Indirect Data 90 +3144 1 Indirect Data 91 +3145 1 Indirect Data 92 +3146 1 Indirect Data 93 +3147 1 Indirect Data 94 +3148 1 Indirect Data 95 +3149 1 Indirect Data 96 +3150 1 Indirect Data 97 +3151 1 Indirect Data 98 +3152 1 Indirect Data 99 +3153 1 Indirect Data 100 +3154 1 Indirect Data 101 +3155 1 Indirect Data 102 +3156 1 Indirect Data 103 +3157 1 Indirect Data 104 +3158 1 Indirect Data 105 +3159 1 Indirect Data 106 +3160 1 Indirect Data 107 +3161 1 Indirect Data 108 +3162 1 Indirect Data 109 +3163 1 Indirect Data 110 +3164 1 Indirect Data 111 +3165 1 Indirect Data 112 +3166 1 Indirect Data 113 +3167 1 Indirect Data 114 +3168 1 Indirect Data 115 +3169 1 Indirect Data 116 +3170 1 Indirect Data 117 +3171 1 Indirect Data 118 +3172 1 Indirect Data 119 +3173 1 Indirect Data 120 +3174 1 Indirect Data 121 +3175 1 Indirect Data 122 +3176 1 Indirect Data 123 +3177 1 Indirect Data 124 +3178 1 Indirect Data 125 +3179 1 Indirect Data 126 +3180 1 Indirect Data 127 +3181 1 Indirect Data 128 diff --git a/param/dxl_model/ffw_sg2_steer_1.model b/param/dxl_model/ffw_sg2_steer_1.model new file mode 100644 index 0000000..c05793c --- /dev/null +++ b/param/dxl_model/ffw_sg2_steer_1.model @@ -0,0 +1,359 @@ +[type info] +name value +value_of_zero_radian_position 0 +value_of_max_radian_position 262144 +value_of_min_radian_position -262144 +min_radian -3.14159265 +max_radian 3.14159265 +velocity_unit 0.01 + +[control table] +Address Size Data Name +3182 2 Model Number +3184 4 Model Information +3188 1 Firmware Version +3189 1 ID +3190 2 Bus Watchdog +3192 1 Secondary(Shadow) ID +3193 1 Protocol Type +3194 1 Baud Rate (Bus) +3195 1 Return Delay Time +3197 1 Status Return Level +3198 1 Registered Instruction +3214 1 Drive Mode +3215 1 Operating Mode +3216 1 Startup Configuration +3220 2 Position Limit Threshold +3222 4 In-Position Threshold +3226 4 Following Error Threshold +3230 4 Moving Threshold +3234 4 Homing Offset +3238 1 Inverter Temperature Limit +3239 1 Motor Temperature Limit +3242 2 Max Voltage Limit +3244 2 Min Voltage Limit +3246 2 PWM Limit +3248 2 Current Limit +3250 4 Acceleration Limit +3254 4 Velocity Limit +3258 4 Max Position Limit +3266 4 Min Position Limit +3278 4 Electronic GearRatio Numerator +3282 4 Electronic GearRatio Denominator +3286 2 Safe Stop Time +3288 2 Brake Delay +3290 2 Goal Update Delay +3292 1 Overexcitation Voltage +3293 1 Normal Excitation Voltage +3294 2 Overexcitation Time +3314 2 Present Velocity LPF Frequency +3316 2 Goal Current LPF Frequency +3318 2 Position FF LPF Time +3320 2 Velocity FF LPF Time +3334 1 Controller State +3335 1 Error Code +3336 1 Error Code History 1 +3337 1 Error Code History 2 +3338 1 Error Code History 3 +3339 1 Error Code History 4 +3340 1 Error Code History 5 +3341 1 Error Code History 6 +3342 1 Error Code History 7 +3343 1 Error Code History 8 +3344 1 Error Code History 9 +3345 1 Error Code History 10 +3346 1 Error Code History 11 +3347 1 Error Code History 12 +3348 1 Error Code History 13 +3349 1 Error Code History 14 +3350 1 Error Code History 15 +3351 1 Error Code History 16 +3352 1 Hybrid Saveve +3394 4 Velocity I Gain +3398 4 Velocity P Gain +3402 4 Velocity FF Gain +3406 4 Position D Gain +3410 4 Position I Gain +3414 4 Position P Gain +3418 4 Position FF Gain +3422 4 Profile Acceleration +3426 4 Profile Velocity +3430 4 Profile Acceleration Time +3434 4 Profile Time +3438 2 Indirect Address 1 +3440 2 Indirect Address 2 +3442 2 Indirect Address 3 +3444 2 Indirect Address 4 +3446 2 Indirect Address 5 +3448 2 Indirect Address 6 +3450 2 Indirect Address 7 +3452 2 Indirect Address 8 +3454 2 Indirect Address 9 +3456 2 Indirect Address 10 +3458 2 Indirect Address 11 +3460 2 Indirect Address 12 +3462 2 Indirect Address 13 +3464 2 Indirect Address 14 +3466 2 Indirect Address 15 +3468 2 Indirect Address 16 +3470 2 Indirect Address 17 +3472 2 Indirect Address 18 +3474 2 Indirect Address 19 +3476 2 Indirect Address 20 +3478 2 Indirect Address 21 +3480 2 Indirect Address 22 +3482 2 Indirect Address 23 +3484 2 Indirect Address 24 +3486 2 Indirect Address 25 +3488 2 Indirect Address 26 +3490 2 Indirect Address 27 +3492 2 Indirect Address 28 +3494 2 Indirect Address 29 +3496 2 Indirect Address 30 +3498 2 Indirect Address 31 +3500 2 Indirect Address 32 +3502 2 Indirect Address 33 +3504 2 Indirect Address 34 +3506 2 Indirect Address 35 +3508 2 Indirect Address 36 +3510 2 Indirect Address 37 +3512 2 Indirect Address 38 +3514 2 Indirect Address 39 +3516 2 Indirect Address 40 +3518 2 Indirect Address 41 +3520 2 Indirect Address 42 +3522 2 Indirect Address 43 +3524 2 Indirect Address 44 +3526 2 Indirect Address 45 +3528 2 Indirect Address 46 +3530 2 Indirect Address 47 +3532 2 Indirect Address 48 +3534 2 Indirect Address 49 +3536 2 Indirect Address 50 +3538 2 Indirect Address 51 +3540 2 Indirect Address 52 +3542 2 Indirect Address 53 +3544 2 Indirect Address 54 +3546 2 Indirect Address 55 +3548 2 Indirect Address 56 +3550 2 Indirect Address 57 +3552 2 Indirect Address 58 +3554 2 Indirect Address 59 +3556 2 Indirect Address 60 +3558 2 Indirect Address 61 +3560 2 Indirect Address 62 +3562 2 Indirect Address 63 +3564 2 Indirect Address 64 +3566 2 Indirect Address 65 +3568 2 Indirect Address 66 +3570 2 Indirect Address 67 +3572 2 Indirect Address 68 +3574 2 Indirect Address 69 +3576 2 Indirect Address 70 +3578 2 Indirect Address 71 +3580 2 Indirect Address 72 +3582 2 Indirect Address 73 +3584 2 Indirect Address 74 +3586 2 Indirect Address 75 +3588 2 Indirect Address 76 +3590 2 Indirect Address 77 +3592 2 Indirect Address 78 +3594 2 Indirect Address 79 +3596 2 Indirect Address 80 +3598 2 Indirect Address 81 +3600 2 Indirect Address 82 +3602 2 Indirect Address 83 +3604 2 Indirect Address 84 +3606 2 Indirect Address 85 +3608 2 Indirect Address 86 +3610 2 Indirect Address 87 +3612 2 Indirect Address 88 +3614 2 Indirect Address 89 +3616 2 Indirect Address 90 +3618 2 Indirect Address 91 +3620 2 Indirect Address 92 +3622 2 Indirect Address 93 +3624 2 Indirect Address 94 +3626 2 Indirect Address 95 +3628 2 Indirect Address 96 +3630 2 Indirect Address 97 +3632 2 Indirect Address 98 +3634 2 Indirect Address 99 +3636 2 Indirect Address 100 +3638 2 Indirect Address 101 +3640 2 Indirect Address 102 +3642 2 Indirect Address 103 +3644 2 Indirect Address 104 +3646 2 Indirect Address 105 +3648 2 Indirect Address 106 +3650 2 Indirect Address 107 +3652 2 Indirect Address 108 +3654 2 Indirect Address 109 +3656 2 Indirect Address 110 +3658 2 Indirect Address 111 +3660 2 Indirect Address 112 +3662 2 Indirect Address 113 +3664 2 Indirect Address 114 +3666 2 Indirect Address 115 +3668 2 Indirect Address 116 +3670 2 Indirect Address 117 +3672 2 Indirect Address 118 +3674 2 Indirect Address 119 +3676 2 Indirect Address 120 +3678 2 Indirect Address 121 +3680 2 Indirect Address 122 +3682 2 Indirect Address 123 +3684 2 Indirect Address 124 +3686 2 Indirect Address 125 +3688 2 Indirect Address 126 +3690 2 Indirect Address 127 +3692 2 Indirect Address 128 +3694 1 Torque Enable +3695 1 LED +3698 2 PWM Offset +3700 2 Current Offset +3702 4 Velocity Offset +3706 2 Goal PWM +3708 2 Goal Current +3710 4 Goal Velocity +3714 4 Goal Position +3723 1 Moving Status +3724 2 Realtime Tick +3726 2 Present PWM +3728 2 Present Current +3730 4 Present Velocity +3734 4 Present Position +3742 4 Position Trajectory +3746 4 Velocity Trajectory +3750 2 Present Input Voltage +3752 1 Present Inverter Temperature +3753 1 Present Motor Temperature +3816 1 Indirect Data 1 +3817 1 Indirect Data 2 +3818 1 Indirect Data 3 +3819 1 Indirect Data 4 +3820 1 Indirect Data 5 +3821 1 Indirect Data 6 +3822 1 Indirect Data 7 +3823 1 Indirect Data 8 +3824 1 Indirect Data 9 +3825 1 Indirect Data 10 +3826 1 Indirect Data 11 +3827 1 Indirect Data 12 +3828 1 Indirect Data 13 +3829 1 Indirect Data 14 +3830 1 Indirect Data 15 +3831 1 Indirect Data 16 +3832 1 Indirect Data 17 +3833 1 Indirect Data 18 +3834 1 Indirect Data 19 +3835 1 Indirect Data 20 +3836 1 Indirect Data 21 +3837 1 Indirect Data 22 +3838 1 Indirect Data 23 +3839 1 Indirect Data 24 +3840 1 Indirect Data 25 +3841 1 Indirect Data 26 +3842 1 Indirect Data 27 +3843 1 Indirect Data 28 +3844 1 Indirect Data 29 +3845 1 Indirect Data 30 +3846 1 Indirect Data 31 +3847 1 Indirect Data 32 +3848 1 Indirect Data 33 +3849 1 Indirect Data 34 +3850 1 Indirect Data 35 +3851 1 Indirect Data 36 +3852 1 Indirect Data 37 +3853 1 Indirect Data 38 +3854 1 Indirect Data 39 +3855 1 Indirect Data 40 +3856 1 Indirect Data 41 +3857 1 Indirect Data 42 +3858 1 Indirect Data 43 +3859 1 Indirect Data 44 +3860 1 Indirect Data 45 +3861 1 Indirect Data 46 +3862 1 Indirect Data 47 +3863 1 Indirect Data 48 +3864 1 Indirect Data 49 +3865 1 Indirect Data 50 +3866 1 Indirect Data 51 +3867 1 Indirect Data 52 +3868 1 Indirect Data 53 +3869 1 Indirect Data 54 +3870 1 Indirect Data 55 +3871 1 Indirect Data 56 +3872 1 Indirect Data 57 +3873 1 Indirect Data 58 +3874 1 Indirect Data 59 +3875 1 Indirect Data 60 +3876 1 Indirect Data 61 +3877 1 Indirect Data 62 +3878 1 Indirect Data 63 +3879 1 Indirect Data 64 +3880 1 Indirect Data 65 +3881 1 Indirect Data 66 +3882 1 Indirect Data 67 +3883 1 Indirect Data 68 +3884 1 Indirect Data 69 +3885 1 Indirect Data 70 +3886 1 Indirect Data 71 +3887 1 Indirect Data 72 +3888 1 Indirect Data 73 +3889 1 Indirect Data 74 +3890 1 Indirect Data 75 +3891 1 Indirect Data 76 +3892 1 Indirect Data 77 +3893 1 Indirect Data 78 +3894 1 Indirect Data 79 +3895 1 Indirect Data 80 +3896 1 Indirect Data 81 +3897 1 Indirect Data 82 +3898 1 Indirect Data 83 +3899 1 Indirect Data 84 +3900 1 Indirect Data 85 +3901 1 Indirect Data 86 +3902 1 Indirect Data 87 +3903 1 Indirect Data 88 +3904 1 Indirect Data 89 +3905 1 Indirect Data 90 +3906 1 Indirect Data 91 +3907 1 Indirect Data 92 +3908 1 Indirect Data 93 +3909 1 Indirect Data 94 +3910 1 Indirect Data 95 +3911 1 Indirect Data 96 +3912 1 Indirect Data 97 +3913 1 Indirect Data 98 +3914 1 Indirect Data 99 +3915 1 Indirect Data 100 +3916 1 Indirect Data 101 +3917 1 Indirect Data 102 +3918 1 Indirect Data 103 +3919 1 Indirect Data 104 +3920 1 Indirect Data 105 +3921 1 Indirect Data 106 +3922 1 Indirect Data 107 +3923 1 Indirect Data 108 +3924 1 Indirect Data 109 +3925 1 Indirect Data 110 +3926 1 Indirect Data 111 +3927 1 Indirect Data 112 +3928 1 Indirect Data 113 +3929 1 Indirect Data 114 +3930 1 Indirect Data 115 +3931 1 Indirect Data 116 +3932 1 Indirect Data 117 +3933 1 Indirect Data 118 +3934 1 Indirect Data 119 +3935 1 Indirect Data 120 +3936 1 Indirect Data 121 +3937 1 Indirect Data 122 +3938 1 Indirect Data 123 +3939 1 Indirect Data 124 +3940 1 Indirect Data 125 +3941 1 Indirect Data 126 +3942 1 Indirect Data 127 +3943 1 Indirect Data 128 +4101 1 Backup Ready diff --git a/param/dxl_model/ffw_sg2_steer_2.model b/param/dxl_model/ffw_sg2_steer_2.model new file mode 100644 index 0000000..f591c13 --- /dev/null +++ b/param/dxl_model/ffw_sg2_steer_2.model @@ -0,0 +1,359 @@ +[type info] +name value +value_of_zero_radian_position 0 +value_of_max_radian_position 262144 +value_of_min_radian_position -262144 +min_radian -3.14159265 +max_radian 3.14159265 +velocity_unit 0.01 + +[control table] +Address Size Data Name +4206 2 Model Number +4208 4 Model Information +4212 1 Firmware Version +4213 1 ID +4214 2 Bus Watchdog +4216 1 Secondary(Shadow) ID +4217 1 Protocol Type +4218 1 Baud Rate (Bus) +4219 1 Return Delay Time +4221 1 Status Return Level +4222 1 Registered Instruction +4238 1 Drive Mode +4239 1 Operating Mode +4240 1 Startup Configuration +4244 2 Position Limit Threshold +4246 4 In-Position Threshold +4250 4 Following Error Threshold +4254 4 Moving Threshold +4258 4 Homing Offset +4262 1 Inverter Temperature Limit +4263 1 Motor Temperature Limit +4266 2 Max Voltage Limit +4268 2 Min Voltage Limit +4270 2 PWM Limit +4272 2 Current Limit +4274 4 Acceleration Limit +4278 4 Velocity Limit +4282 4 Max Position Limit +4290 4 Min Position Limit +4302 4 Electronic GearRatio Numerator +4306 4 Electronic GearRatio Denominator +4310 2 Safe Stop Time +4312 2 Brake Delay +4314 2 Goal Update Delay +4316 1 Overexcitation Voltage +4317 1 Normal Excitation Voltage +4318 2 Overexcitation Time +4338 2 Present Velocity LPF Frequency +4340 2 Goal Current LPF Frequency +4342 2 Position FF LPF Time +4344 2 Velocity FF LPF Time +4358 1 Controller State +4359 1 Error Code +4360 1 Error Code History 1 +4361 1 Error Code History 2 +4362 1 Error Code History 3 +4363 1 Error Code History 4 +4364 1 Error Code History 5 +4365 1 Error Code History 6 +4366 1 Error Code History 7 +4367 1 Error Code History 8 +4368 1 Error Code History 9 +4369 1 Error Code History 10 +4370 1 Error Code History 11 +4371 1 Error Code History 12 +4372 1 Error Code History 13 +4373 1 Error Code History 14 +4374 1 Error Code History 15 +4375 1 Error Code History 16 +4376 1 Hybrid Saveve +4418 4 Velocity I Gain +4422 4 Velocity P Gain +4426 4 Velocity FF Gain +4430 4 Position D Gain +4434 4 Position I Gain +4438 4 Position P Gain +4442 4 Position FF Gain +4446 4 Profile Acceleration +4450 4 Profile Velocity +4454 4 Profile Acceleration Time +4458 4 Profile Time +4462 2 Indirect Address 1 +4464 2 Indirect Address 2 +4466 2 Indirect Address 3 +4468 2 Indirect Address 4 +4470 2 Indirect Address 5 +4472 2 Indirect Address 6 +4474 2 Indirect Address 7 +4476 2 Indirect Address 8 +4478 2 Indirect Address 9 +4480 2 Indirect Address 10 +4482 2 Indirect Address 11 +4484 2 Indirect Address 12 +4486 2 Indirect Address 13 +4488 2 Indirect Address 14 +4490 2 Indirect Address 15 +4492 2 Indirect Address 16 +4494 2 Indirect Address 17 +4496 2 Indirect Address 18 +4498 2 Indirect Address 19 +4500 2 Indirect Address 20 +4502 2 Indirect Address 21 +4504 2 Indirect Address 22 +4506 2 Indirect Address 23 +4508 2 Indirect Address 24 +4510 2 Indirect Address 25 +4512 2 Indirect Address 26 +4514 2 Indirect Address 27 +4516 2 Indirect Address 28 +4518 2 Indirect Address 29 +4520 2 Indirect Address 30 +4522 2 Indirect Address 31 +4524 2 Indirect Address 32 +4526 2 Indirect Address 33 +4528 2 Indirect Address 34 +4530 2 Indirect Address 35 +4532 2 Indirect Address 36 +4534 2 Indirect Address 37 +4536 2 Indirect Address 38 +4538 2 Indirect Address 39 +4540 2 Indirect Address 40 +4542 2 Indirect Address 41 +4544 2 Indirect Address 42 +4546 2 Indirect Address 43 +4548 2 Indirect Address 44 +4550 2 Indirect Address 45 +4552 2 Indirect Address 46 +4554 2 Indirect Address 47 +4556 2 Indirect Address 48 +4558 2 Indirect Address 49 +4560 2 Indirect Address 50 +4562 2 Indirect Address 51 +4564 2 Indirect Address 52 +4566 2 Indirect Address 53 +4568 2 Indirect Address 54 +4570 2 Indirect Address 55 +4572 2 Indirect Address 56 +4574 2 Indirect Address 57 +4576 2 Indirect Address 58 +4578 2 Indirect Address 59 +4580 2 Indirect Address 60 +4582 2 Indirect Address 61 +4584 2 Indirect Address 62 +4586 2 Indirect Address 63 +4588 2 Indirect Address 64 +4590 2 Indirect Address 65 +4592 2 Indirect Address 66 +4594 2 Indirect Address 67 +4596 2 Indirect Address 68 +4598 2 Indirect Address 69 +4600 2 Indirect Address 70 +4602 2 Indirect Address 71 +4604 2 Indirect Address 72 +4606 2 Indirect Address 73 +4608 2 Indirect Address 74 +4610 2 Indirect Address 75 +4612 2 Indirect Address 76 +4614 2 Indirect Address 77 +4616 2 Indirect Address 78 +4618 2 Indirect Address 79 +4620 2 Indirect Address 80 +4622 2 Indirect Address 81 +4624 2 Indirect Address 82 +4626 2 Indirect Address 83 +4628 2 Indirect Address 84 +4630 2 Indirect Address 85 +4632 2 Indirect Address 86 +4634 2 Indirect Address 87 +4636 2 Indirect Address 88 +4638 2 Indirect Address 89 +4640 2 Indirect Address 90 +4642 2 Indirect Address 91 +4644 2 Indirect Address 92 +4646 2 Indirect Address 93 +4648 2 Indirect Address 94 +4650 2 Indirect Address 95 +4652 2 Indirect Address 96 +4654 2 Indirect Address 97 +4656 2 Indirect Address 98 +4658 2 Indirect Address 99 +4660 2 Indirect Address 100 +4662 2 Indirect Address 101 +4664 2 Indirect Address 102 +4666 2 Indirect Address 103 +4668 2 Indirect Address 104 +4670 2 Indirect Address 105 +4672 2 Indirect Address 106 +4674 2 Indirect Address 107 +4676 2 Indirect Address 108 +4678 2 Indirect Address 109 +4680 2 Indirect Address 110 +4682 2 Indirect Address 111 +4684 2 Indirect Address 112 +4686 2 Indirect Address 113 +4688 2 Indirect Address 114 +4690 2 Indirect Address 115 +4692 2 Indirect Address 116 +4694 2 Indirect Address 117 +4696 2 Indirect Address 118 +4698 2 Indirect Address 119 +4700 2 Indirect Address 120 +4702 2 Indirect Address 121 +4704 2 Indirect Address 122 +4706 2 Indirect Address 123 +4708 2 Indirect Address 124 +4710 2 Indirect Address 125 +4712 2 Indirect Address 126 +4714 2 Indirect Address 127 +4716 2 Indirect Address 128 +4718 1 Torque Enable +4719 1 LED +4722 2 PWM Offset +4724 2 Current Offset +4726 4 Velocity Offset +4730 2 Goal PWM +4732 2 Goal Current +4734 4 Goal Velocity +4738 4 Goal Position +4747 1 Moving Status +4748 2 Realtime Tick +4750 2 Present PWM +4752 2 Present Current +4754 4 Present Velocity +4758 4 Present Position +4766 4 Position Trajectory +4770 4 Velocity Trajectory +4774 2 Present Input Voltage +4776 1 Present Inverter Temperature +4777 1 Present Motor Temperature +4840 1 Indirect Data 1 +4841 1 Indirect Data 2 +4842 1 Indirect Data 3 +4843 1 Indirect Data 4 +4844 1 Indirect Data 5 +4845 1 Indirect Data 6 +4846 1 Indirect Data 7 +4847 1 Indirect Data 8 +4848 1 Indirect Data 9 +4849 1 Indirect Data 10 +4850 1 Indirect Data 11 +4851 1 Indirect Data 12 +4852 1 Indirect Data 13 +4853 1 Indirect Data 14 +4854 1 Indirect Data 15 +4855 1 Indirect Data 16 +4856 1 Indirect Data 17 +4857 1 Indirect Data 18 +4858 1 Indirect Data 19 +4859 1 Indirect Data 20 +4860 1 Indirect Data 21 +4861 1 Indirect Data 22 +4862 1 Indirect Data 23 +4863 1 Indirect Data 24 +4864 1 Indirect Data 25 +4865 1 Indirect Data 26 +4866 1 Indirect Data 27 +4867 1 Indirect Data 28 +4868 1 Indirect Data 29 +4869 1 Indirect Data 30 +4870 1 Indirect Data 31 +4871 1 Indirect Data 32 +4872 1 Indirect Data 33 +4873 1 Indirect Data 34 +4874 1 Indirect Data 35 +4875 1 Indirect Data 36 +4876 1 Indirect Data 37 +4877 1 Indirect Data 38 +4878 1 Indirect Data 39 +4879 1 Indirect Data 40 +4880 1 Indirect Data 41 +4881 1 Indirect Data 42 +4882 1 Indirect Data 43 +4883 1 Indirect Data 44 +4884 1 Indirect Data 45 +4885 1 Indirect Data 46 +4886 1 Indirect Data 47 +4887 1 Indirect Data 48 +4888 1 Indirect Data 49 +4889 1 Indirect Data 50 +4890 1 Indirect Data 51 +4891 1 Indirect Data 52 +4892 1 Indirect Data 53 +4893 1 Indirect Data 54 +4894 1 Indirect Data 55 +4895 1 Indirect Data 56 +4896 1 Indirect Data 57 +4897 1 Indirect Data 58 +4898 1 Indirect Data 59 +4899 1 Indirect Data 60 +4900 1 Indirect Data 61 +4901 1 Indirect Data 62 +4902 1 Indirect Data 63 +4903 1 Indirect Data 64 +4904 1 Indirect Data 65 +4905 1 Indirect Data 66 +4906 1 Indirect Data 67 +4907 1 Indirect Data 68 +4908 1 Indirect Data 69 +4909 1 Indirect Data 70 +4910 1 Indirect Data 71 +4911 1 Indirect Data 72 +4912 1 Indirect Data 73 +4913 1 Indirect Data 74 +4914 1 Indirect Data 75 +4915 1 Indirect Data 76 +4916 1 Indirect Data 77 +4917 1 Indirect Data 78 +4918 1 Indirect Data 79 +4919 1 Indirect Data 80 +4920 1 Indirect Data 81 +4921 1 Indirect Data 82 +4922 1 Indirect Data 83 +4923 1 Indirect Data 84 +4924 1 Indirect Data 85 +4925 1 Indirect Data 86 +4926 1 Indirect Data 87 +4927 1 Indirect Data 88 +4928 1 Indirect Data 89 +4929 1 Indirect Data 90 +4930 1 Indirect Data 91 +4931 1 Indirect Data 92 +4932 1 Indirect Data 93 +4933 1 Indirect Data 94 +4934 1 Indirect Data 95 +4935 1 Indirect Data 96 +4936 1 Indirect Data 97 +4937 1 Indirect Data 98 +4938 1 Indirect Data 99 +4939 1 Indirect Data 100 +4940 1 Indirect Data 101 +4941 1 Indirect Data 102 +4942 1 Indirect Data 103 +4943 1 Indirect Data 104 +4944 1 Indirect Data 105 +4945 1 Indirect Data 106 +4946 1 Indirect Data 107 +4947 1 Indirect Data 108 +4948 1 Indirect Data 109 +4949 1 Indirect Data 110 +4950 1 Indirect Data 111 +4951 1 Indirect Data 112 +4952 1 Indirect Data 113 +4953 1 Indirect Data 114 +4954 1 Indirect Data 115 +4955 1 Indirect Data 116 +4956 1 Indirect Data 117 +4957 1 Indirect Data 118 +4958 1 Indirect Data 119 +4959 1 Indirect Data 120 +4960 1 Indirect Data 121 +4961 1 Indirect Data 122 +4962 1 Indirect Data 123 +4963 1 Indirect Data 124 +4964 1 Indirect Data 125 +4965 1 Indirect Data 126 +4966 1 Indirect Data 127 +4967 1 Indirect Data 128 +5125 1 Backup Ready diff --git a/param/dxl_model/ffw_sg2_steer_3.model b/param/dxl_model/ffw_sg2_steer_3.model new file mode 100644 index 0000000..b0c8e8c --- /dev/null +++ b/param/dxl_model/ffw_sg2_steer_3.model @@ -0,0 +1,359 @@ +[type info] +name value +value_of_zero_radian_position 0 +value_of_max_radian_position 262144 +value_of_min_radian_position -262144 +min_radian -3.14159265 +max_radian 3.14159265 +velocity_unit 0.01 + +[control table] +Address Size Data Name +5230 2 Model Number +5232 4 Model Information +5236 1 Firmware Version +5237 1 ID +5238 2 Bus Watchdog +5240 1 Secondary(Shadow) ID +5241 1 Protocol Type +5242 1 Baud Rate (Bus) +5243 1 Return Delay Time +5245 1 Status Return Level +5246 1 Registered Instruction +5262 1 Drive Mode +5263 1 Operating Mode +5264 1 Startup Configuration +5268 2 Position Limit Threshold +5270 4 In-Position Threshold +5274 4 Following Error Threshold +5278 4 Moving Threshold +5282 4 Homing Offset +5286 1 Inverter Temperature Limit +5287 1 Motor Temperature Limit +5290 2 Max Voltage Limit +5292 2 Min Voltage Limit +5294 2 PWM Limit +5296 2 Current Limit +5298 4 Acceleration Limit +5302 4 Velocity Limit +5306 4 Max Position Limit +5314 4 Min Position Limit +5326 4 Electronic GearRatio Numerator +5330 4 Electronic GearRatio Denominator +5334 2 Safe Stop Time +5336 2 Brake Delay +5338 2 Goal Update Delay +5340 1 Overexcitation Voltage +5341 1 Normal Excitation Voltage +5342 2 Overexcitation Time +5362 2 Present Velocity LPF Frequency +5364 2 Goal Current LPF Frequency +5366 2 Position FF LPF Time +5368 2 Velocity FF LPF Time +5382 1 Controller State +5383 1 Error Code +5384 1 Error Code History 1 +5385 1 Error Code History 2 +5386 1 Error Code History 3 +5387 1 Error Code History 4 +5388 1 Error Code History 5 +5389 1 Error Code History 6 +5390 1 Error Code History 7 +5391 1 Error Code History 8 +5392 1 Error Code History 9 +5393 1 Error Code History 10 +5394 1 Error Code History 11 +5395 1 Error Code History 12 +5396 1 Error Code History 13 +5397 1 Error Code History 14 +5398 1 Error Code History 15 +5399 1 Error Code History 16 +5400 1 Hybrid Saveve +5442 4 Velocity I Gain +5446 4 Velocity P Gain +5450 4 Velocity FF Gain +5454 4 Position D Gain +5458 4 Position I Gain +5462 4 Position P Gain +5466 4 Position FF Gain +5470 4 Profile Acceleration +5474 4 Profile Velocity +5478 4 Profile Acceleration Time +5482 4 Profile Time +5486 2 Indirect Address 1 +5488 2 Indirect Address 2 +5490 2 Indirect Address 3 +5492 2 Indirect Address 4 +5494 2 Indirect Address 5 +5496 2 Indirect Address 6 +5498 2 Indirect Address 7 +5500 2 Indirect Address 8 +5502 2 Indirect Address 9 +5504 2 Indirect Address 10 +5506 2 Indirect Address 11 +5508 2 Indirect Address 12 +5510 2 Indirect Address 13 +5512 2 Indirect Address 14 +5514 2 Indirect Address 15 +5516 2 Indirect Address 16 +5518 2 Indirect Address 17 +5520 2 Indirect Address 18 +5522 2 Indirect Address 19 +5524 2 Indirect Address 20 +5526 2 Indirect Address 21 +5528 2 Indirect Address 22 +5530 2 Indirect Address 23 +5532 2 Indirect Address 24 +5534 2 Indirect Address 25 +5536 2 Indirect Address 26 +5538 2 Indirect Address 27 +5540 2 Indirect Address 28 +5542 2 Indirect Address 29 +5544 2 Indirect Address 30 +5546 2 Indirect Address 31 +5548 2 Indirect Address 32 +5550 2 Indirect Address 33 +5552 2 Indirect Address 34 +5554 2 Indirect Address 35 +5556 2 Indirect Address 36 +5558 2 Indirect Address 37 +5560 2 Indirect Address 38 +5562 2 Indirect Address 39 +5564 2 Indirect Address 40 +5566 2 Indirect Address 41 +5568 2 Indirect Address 42 +5570 2 Indirect Address 43 +5572 2 Indirect Address 44 +5574 2 Indirect Address 45 +5576 2 Indirect Address 46 +5578 2 Indirect Address 47 +5580 2 Indirect Address 48 +5582 2 Indirect Address 49 +5584 2 Indirect Address 50 +5586 2 Indirect Address 51 +5588 2 Indirect Address 52 +5590 2 Indirect Address 53 +5592 2 Indirect Address 54 +5594 2 Indirect Address 55 +5596 2 Indirect Address 56 +5598 2 Indirect Address 57 +5600 2 Indirect Address 58 +5602 2 Indirect Address 59 +5604 2 Indirect Address 60 +5606 2 Indirect Address 61 +5608 2 Indirect Address 62 +5610 2 Indirect Address 63 +5612 2 Indirect Address 64 +5614 2 Indirect Address 65 +5616 2 Indirect Address 66 +5618 2 Indirect Address 67 +5620 2 Indirect Address 68 +5622 2 Indirect Address 69 +5624 2 Indirect Address 70 +5626 2 Indirect Address 71 +5628 2 Indirect Address 72 +5630 2 Indirect Address 73 +5632 2 Indirect Address 74 +5634 2 Indirect Address 75 +5636 2 Indirect Address 76 +5638 2 Indirect Address 77 +5640 2 Indirect Address 78 +5642 2 Indirect Address 79 +5644 2 Indirect Address 80 +5646 2 Indirect Address 81 +5648 2 Indirect Address 82 +5650 2 Indirect Address 83 +5652 2 Indirect Address 84 +5654 2 Indirect Address 85 +5656 2 Indirect Address 86 +5658 2 Indirect Address 87 +5660 2 Indirect Address 88 +5662 2 Indirect Address 89 +5664 2 Indirect Address 90 +5666 2 Indirect Address 91 +5668 2 Indirect Address 92 +5670 2 Indirect Address 93 +5672 2 Indirect Address 94 +5674 2 Indirect Address 95 +5676 2 Indirect Address 96 +5678 2 Indirect Address 97 +5680 2 Indirect Address 98 +5682 2 Indirect Address 99 +5684 2 Indirect Address 100 +5686 2 Indirect Address 101 +5688 2 Indirect Address 102 +5690 2 Indirect Address 103 +5692 2 Indirect Address 104 +5694 2 Indirect Address 105 +5696 2 Indirect Address 106 +5698 2 Indirect Address 107 +5700 2 Indirect Address 108 +5702 2 Indirect Address 109 +5704 2 Indirect Address 110 +5706 2 Indirect Address 111 +5708 2 Indirect Address 112 +5710 2 Indirect Address 113 +5712 2 Indirect Address 114 +5714 2 Indirect Address 115 +5716 2 Indirect Address 116 +5718 2 Indirect Address 117 +5720 2 Indirect Address 118 +5722 2 Indirect Address 119 +5724 2 Indirect Address 120 +5726 2 Indirect Address 121 +5728 2 Indirect Address 122 +5730 2 Indirect Address 123 +5732 2 Indirect Address 124 +5734 2 Indirect Address 125 +5736 2 Indirect Address 126 +5738 2 Indirect Address 127 +5740 2 Indirect Address 128 +5742 1 Torque Enable +5743 1 LED +5746 2 PWM Offset +5748 2 Current Offset +5750 4 Velocity Offset +5754 2 Goal PWM +5756 2 Goal Current +5758 4 Goal Velocity +5762 4 Goal Position +5771 1 Moving Status +5772 2 Realtime Tick +5774 2 Present PWM +5776 2 Present Current +5778 4 Present Velocity +5782 4 Present Position +5790 4 Position Trajectory +5794 4 Velocity Trajectory +5798 2 Present Input Voltage +5800 1 Present Inverter Temperature +5801 1 Present Motor Temperature +5864 1 Indirect Data 1 +5865 1 Indirect Data 2 +5866 1 Indirect Data 3 +5867 1 Indirect Data 4 +5868 1 Indirect Data 5 +5869 1 Indirect Data 6 +5870 1 Indirect Data 7 +5871 1 Indirect Data 8 +5872 1 Indirect Data 9 +5873 1 Indirect Data 10 +5874 1 Indirect Data 11 +5875 1 Indirect Data 12 +5876 1 Indirect Data 13 +5877 1 Indirect Data 14 +5878 1 Indirect Data 15 +5879 1 Indirect Data 16 +5880 1 Indirect Data 17 +5881 1 Indirect Data 18 +5882 1 Indirect Data 19 +5883 1 Indirect Data 20 +5884 1 Indirect Data 21 +5885 1 Indirect Data 22 +5886 1 Indirect Data 23 +5887 1 Indirect Data 24 +5888 1 Indirect Data 25 +5889 1 Indirect Data 26 +5890 1 Indirect Data 27 +5891 1 Indirect Data 28 +5892 1 Indirect Data 29 +5893 1 Indirect Data 30 +5894 1 Indirect Data 31 +5895 1 Indirect Data 32 +5896 1 Indirect Data 33 +5897 1 Indirect Data 34 +5898 1 Indirect Data 35 +5899 1 Indirect Data 36 +5900 1 Indirect Data 37 +5901 1 Indirect Data 38 +5902 1 Indirect Data 39 +5903 1 Indirect Data 40 +5904 1 Indirect Data 41 +5905 1 Indirect Data 42 +5906 1 Indirect Data 43 +5907 1 Indirect Data 44 +5908 1 Indirect Data 45 +5909 1 Indirect Data 46 +5910 1 Indirect Data 47 +5911 1 Indirect Data 48 +5912 1 Indirect Data 49 +5913 1 Indirect Data 50 +5914 1 Indirect Data 51 +5915 1 Indirect Data 52 +5916 1 Indirect Data 53 +5917 1 Indirect Data 54 +5918 1 Indirect Data 55 +5919 1 Indirect Data 56 +5920 1 Indirect Data 57 +5921 1 Indirect Data 58 +5922 1 Indirect Data 59 +5923 1 Indirect Data 60 +5924 1 Indirect Data 61 +5925 1 Indirect Data 62 +5926 1 Indirect Data 63 +5927 1 Indirect Data 64 +5928 1 Indirect Data 65 +5929 1 Indirect Data 66 +5930 1 Indirect Data 67 +5931 1 Indirect Data 68 +5932 1 Indirect Data 69 +5933 1 Indirect Data 70 +5934 1 Indirect Data 71 +5935 1 Indirect Data 72 +5936 1 Indirect Data 73 +5937 1 Indirect Data 74 +5938 1 Indirect Data 75 +5939 1 Indirect Data 76 +5940 1 Indirect Data 77 +5941 1 Indirect Data 78 +5942 1 Indirect Data 79 +5943 1 Indirect Data 80 +5944 1 Indirect Data 81 +5945 1 Indirect Data 82 +5946 1 Indirect Data 83 +5947 1 Indirect Data 84 +5948 1 Indirect Data 85 +5949 1 Indirect Data 86 +5950 1 Indirect Data 87 +5951 1 Indirect Data 88 +5952 1 Indirect Data 89 +5953 1 Indirect Data 90 +5954 1 Indirect Data 91 +5955 1 Indirect Data 92 +5956 1 Indirect Data 93 +5957 1 Indirect Data 94 +5958 1 Indirect Data 95 +5959 1 Indirect Data 96 +5960 1 Indirect Data 97 +5961 1 Indirect Data 98 +5962 1 Indirect Data 99 +5963 1 Indirect Data 100 +5964 1 Indirect Data 101 +5965 1 Indirect Data 102 +5966 1 Indirect Data 103 +5967 1 Indirect Data 104 +5968 1 Indirect Data 105 +5969 1 Indirect Data 106 +5970 1 Indirect Data 107 +5971 1 Indirect Data 108 +5972 1 Indirect Data 109 +5973 1 Indirect Data 110 +5974 1 Indirect Data 111 +5975 1 Indirect Data 112 +5976 1 Indirect Data 113 +5977 1 Indirect Data 114 +5978 1 Indirect Data 115 +5979 1 Indirect Data 116 +5980 1 Indirect Data 117 +5981 1 Indirect Data 118 +5982 1 Indirect Data 119 +5983 1 Indirect Data 120 +5984 1 Indirect Data 121 +5985 1 Indirect Data 122 +5986 1 Indirect Data 123 +5987 1 Indirect Data 124 +5988 1 Indirect Data 125 +5989 1 Indirect Data 126 +5990 1 Indirect Data 127 +5991 1 Indirect Data 128 +6149 1 Backup Ready diff --git a/param/dxl_model/omy_end.model b/param/dxl_model/omy_end.model index 31487a4..ad05924 100644 --- a/param/dxl_model/omy_end.model +++ b/param/dxl_model/omy_end.model @@ -5,6 +5,7 @@ value_of_max_radian_position 740 value_of_min_radian_position -740 min_radian -1.099 max_radian 1.099 +velocity_unit 0.114 [control table] Address Size Data Name diff --git a/param/dxl_model/ph42_020_s300.model b/param/dxl_model/ph42_020_s300.model index e9dd55c..d21a411 100644 --- a/param/dxl_model/ph42_020_s300.model +++ b/param/dxl_model/ph42_020_s300.model @@ -5,6 +5,7 @@ value_of_max_radian_position 303750 value_of_min_radian_position -303750 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.01 [control table] Address Size Data Name diff --git a/param/dxl_model/rh_p12_rn.model b/param/dxl_model/rh_p12_rn.model index afb407d..9396d01 100644 --- a/param/dxl_model/rh_p12_rn.model +++ b/param/dxl_model/rh_p12_rn.model @@ -5,6 +5,7 @@ value_of_max_radian_position 740 value_of_min_radian_position -740 min_radian -1.099 max_radian 1.099 +velocity_unit 0.114 [control table] Address Size Data Name diff --git a/param/dxl_model/xc330_m181.model b/param/dxl_model/xc330_m181.model index cb4a784..7fa8ab5 100644 --- a/param/dxl_model/xc330_m181.model +++ b/param/dxl_model/xc330_m181.model @@ -5,6 +5,7 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.229 [control table] Address Size Data Name diff --git a/param/dxl_model/xc330_m288.model b/param/dxl_model/xc330_m288.model index cb4a784..7fa8ab5 100644 --- a/param/dxl_model/xc330_m288.model +++ b/param/dxl_model/xc330_m288.model @@ -5,6 +5,7 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.229 [control table] Address Size Data Name diff --git a/param/dxl_model/xc330_t181.model b/param/dxl_model/xc330_t181.model index 21105f7..63da0b3 100644 --- a/param/dxl_model/xc330_t181.model +++ b/param/dxl_model/xc330_t181.model @@ -6,6 +6,7 @@ value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 torque_constant 0.0006709470296015791 +velocity_unit 0.229 [control table] Address Size Data Name diff --git a/param/dxl_model/xc330_t288.model b/param/dxl_model/xc330_t288.model index 68d3bbb..1e0dd24 100644 --- a/param/dxl_model/xc330_t288.model +++ b/param/dxl_model/xc330_t288.model @@ -6,6 +6,7 @@ value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 torque_constant 0.0009674796739867748 +velocity_unit 0.229 [control table] Address Size Data Name diff --git a/param/dxl_model/xc430_w150.model b/param/dxl_model/xc430_w150.model index 59e7d1f..6a1f335 100644 --- a/param/dxl_model/xc430_w150.model +++ b/param/dxl_model/xc430_w150.model @@ -5,6 +5,7 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.229 [control table] Address Size Data Name diff --git a/param/dxl_model/xc430_w240.model b/param/dxl_model/xc430_w240.model index 59e7d1f..6a1f335 100644 --- a/param/dxl_model/xc430_w240.model +++ b/param/dxl_model/xc430_w240.model @@ -5,6 +5,7 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.229 [control table] Address Size Data Name diff --git a/param/dxl_model/xd430_t210.model b/param/dxl_model/xd430_t210.model index 6ca9523..8b2fb3c 100644 --- a/param/dxl_model/xd430_t210.model +++ b/param/dxl_model/xd430_t210.model @@ -5,6 +5,7 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.229 [control table] Address Size Data Name diff --git a/param/dxl_model/xd430_t350.model b/param/dxl_model/xd430_t350.model index 6ca9523..8b2fb3c 100644 --- a/param/dxl_model/xd430_t350.model +++ b/param/dxl_model/xd430_t350.model @@ -5,6 +5,7 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.229 [control table] Address Size Data Name diff --git a/param/dxl_model/xd540_t150.model b/param/dxl_model/xd540_t150.model index f7d3ae5..0364415 100644 --- a/param/dxl_model/xd540_t150.model +++ b/param/dxl_model/xd540_t150.model @@ -5,6 +5,7 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.229 [control table] Address Size Data Name diff --git a/param/dxl_model/xd540_t270.model b/param/dxl_model/xd540_t270.model index a020845..bfe7dcf 100644 --- a/param/dxl_model/xd540_t270.model +++ b/param/dxl_model/xd540_t270.model @@ -5,6 +5,7 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.229 [control table] Address Size Data Name diff --git a/param/dxl_model/xh430_v210.model b/param/dxl_model/xh430_v210.model index 6ca9523..8b2fb3c 100644 --- a/param/dxl_model/xh430_v210.model +++ b/param/dxl_model/xh430_v210.model @@ -5,6 +5,7 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.229 [control table] Address Size Data Name diff --git a/param/dxl_model/xh430_v350.model b/param/dxl_model/xh430_v350.model index 6ca9523..8b2fb3c 100644 --- a/param/dxl_model/xh430_v350.model +++ b/param/dxl_model/xh430_v350.model @@ -5,6 +5,7 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.229 [control table] Address Size Data Name diff --git a/param/dxl_model/xh430_w210.model b/param/dxl_model/xh430_w210.model index 6ca9523..8b2fb3c 100644 --- a/param/dxl_model/xh430_w210.model +++ b/param/dxl_model/xh430_w210.model @@ -5,6 +5,7 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.229 [control table] Address Size Data Name diff --git a/param/dxl_model/xh430_w350.model b/param/dxl_model/xh430_w350.model index 6ca9523..8b2fb3c 100644 --- a/param/dxl_model/xh430_w350.model +++ b/param/dxl_model/xh430_w350.model @@ -5,6 +5,7 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.229 [control table] Address Size Data Name diff --git a/param/dxl_model/xh540_v150.model b/param/dxl_model/xh540_v150.model index f7d3ae5..0364415 100644 --- a/param/dxl_model/xh540_v150.model +++ b/param/dxl_model/xh540_v150.model @@ -5,6 +5,7 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.229 [control table] Address Size Data Name diff --git a/param/dxl_model/xh540_v270.model b/param/dxl_model/xh540_v270.model index f7d3ae5..0364415 100644 --- a/param/dxl_model/xh540_v270.model +++ b/param/dxl_model/xh540_v270.model @@ -5,6 +5,7 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.229 [control table] Address Size Data Name diff --git a/param/dxl_model/xh540_w150.model b/param/dxl_model/xh540_w150.model index 11893d2..4dd5bbe 100644 --- a/param/dxl_model/xh540_w150.model +++ b/param/dxl_model/xh540_w150.model @@ -6,6 +6,7 @@ value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 torque_constant 0.0045 +velocity_unit 0.229 [control table] Address Size Data Name diff --git a/param/dxl_model/xh540_w270.model b/param/dxl_model/xh540_w270.model index f7d3ae5..0364415 100644 --- a/param/dxl_model/xh540_w270.model +++ b/param/dxl_model/xh540_w270.model @@ -5,6 +5,7 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.229 [control table] Address Size Data Name diff --git a/param/dxl_model/xl320.model b/param/dxl_model/xl320.model index 4a61382..339130b 100644 --- a/param/dxl_model/xl320.model +++ b/param/dxl_model/xl320.model @@ -5,6 +5,7 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.111 [control table] Address Size Data Name diff --git a/param/dxl_model/xl330_m077.model b/param/dxl_model/xl330_m077.model index cb4a784..7fa8ab5 100644 --- a/param/dxl_model/xl330_m077.model +++ b/param/dxl_model/xl330_m077.model @@ -5,6 +5,7 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.229 [control table] Address Size Data Name diff --git a/param/dxl_model/xl330_m288.model b/param/dxl_model/xl330_m288.model index cb4a784..7fa8ab5 100644 --- a/param/dxl_model/xl330_m288.model +++ b/param/dxl_model/xl330_m288.model @@ -5,6 +5,7 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.229 [control table] Address Size Data Name diff --git a/param/dxl_model/xl430_w250.model b/param/dxl_model/xl430_w250.model index 59e7d1f..6a1f335 100644 --- a/param/dxl_model/xl430_w250.model +++ b/param/dxl_model/xl430_w250.model @@ -5,6 +5,7 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.229 [control table] Address Size Data Name diff --git a/param/dxl_model/xm430_w210.model b/param/dxl_model/xm430_w210.model index 6ca9523..8b2fb3c 100644 --- a/param/dxl_model/xm430_w210.model +++ b/param/dxl_model/xm430_w210.model @@ -5,6 +5,7 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.229 [control table] Address Size Data Name diff --git a/param/dxl_model/xm430_w350.model b/param/dxl_model/xm430_w350.model index 6ca9523..8b2fb3c 100644 --- a/param/dxl_model/xm430_w350.model +++ b/param/dxl_model/xm430_w350.model @@ -5,6 +5,7 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.229 [control table] Address Size Data Name diff --git a/param/dxl_model/xm540_w150.model b/param/dxl_model/xm540_w150.model index f7d3ae5..0364415 100644 --- a/param/dxl_model/xm540_w150.model +++ b/param/dxl_model/xm540_w150.model @@ -5,6 +5,7 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.229 [control table] Address Size Data Name diff --git a/param/dxl_model/xm540_w270.model b/param/dxl_model/xm540_w270.model index f7d3ae5..0364415 100644 --- a/param/dxl_model/xm540_w270.model +++ b/param/dxl_model/xm540_w270.model @@ -5,6 +5,7 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.229 [control table] Address Size Data Name diff --git a/param/dxl_model/xw430_t200.model b/param/dxl_model/xw430_t200.model index d6a2105..0a9b4a1 100644 --- a/param/dxl_model/xw430_t200.model +++ b/param/dxl_model/xw430_t200.model @@ -5,6 +5,7 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.229 [control table] Address Size Data Name diff --git a/param/dxl_model/xw430_t333.model b/param/dxl_model/xw430_t333.model index d6a2105..0a9b4a1 100644 --- a/param/dxl_model/xw430_t333.model +++ b/param/dxl_model/xw430_t333.model @@ -5,6 +5,7 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.229 [control table] Address Size Data Name diff --git a/param/dxl_model/xw540_h260.model b/param/dxl_model/xw540_h260.model index d6a2105..0a9b4a1 100644 --- a/param/dxl_model/xw540_h260.model +++ b/param/dxl_model/xw540_h260.model @@ -5,6 +5,7 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.229 [control table] Address Size Data Name diff --git a/param/dxl_model/xw540_t140.model b/param/dxl_model/xw540_t140.model index d6a2105..0a9b4a1 100644 --- a/param/dxl_model/xw540_t140.model +++ b/param/dxl_model/xw540_t140.model @@ -5,6 +5,7 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.229 [control table] Address Size Data Name diff --git a/param/dxl_model/xw540_t260.model b/param/dxl_model/xw540_t260.model index d6a2105..0a9b4a1 100644 --- a/param/dxl_model/xw540_t260.model +++ b/param/dxl_model/xw540_t260.model @@ -5,6 +5,7 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.229 [control table] Address Size Data Name diff --git a/param/dxl_model/ym070_210_a099.model b/param/dxl_model/ym070_210_a099.model index a530828..82e1ae7 100644 --- a/param/dxl_model/ym070_210_a099.model +++ b/param/dxl_model/ym070_210_a099.model @@ -5,6 +5,7 @@ value_of_max_radian_position 262144 value_of_min_radian_position -262144 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.01 [control table] Address Size Data Name diff --git a/param/dxl_model/ym070_210_m001.model b/param/dxl_model/ym070_210_m001.model index 4cc5c06..82e1ae7 100644 --- a/param/dxl_model/ym070_210_m001.model +++ b/param/dxl_model/ym070_210_m001.model @@ -1,10 +1,11 @@ [type info] name value -value_of_zero_radian_position 262144 -value_of_max_radian_position 524287 -value_of_min_radian_position 0 +value_of_zero_radian_position 0 +value_of_max_radian_position 262144 +value_of_min_radian_position -262144 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.01 [control table] Address Size Data Name diff --git a/param/dxl_model/ym070_210_r051.model b/param/dxl_model/ym070_210_r051.model index 04d5d9c..82e1ae7 100644 --- a/param/dxl_model/ym070_210_r051.model +++ b/param/dxl_model/ym070_210_r051.model @@ -1,10 +1,11 @@ [type info] name value -value_of_zero_radian_position 13369344 -value_of_max_radian_position 26738687 -value_of_min_radian_position 0 +value_of_zero_radian_position 0 +value_of_max_radian_position 262144 +value_of_min_radian_position -262144 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.01 [control table] Address Size Data Name diff --git a/param/dxl_model/ym070_210_r099.model b/param/dxl_model/ym070_210_r099.model index a530828..82e1ae7 100644 --- a/param/dxl_model/ym070_210_r099.model +++ b/param/dxl_model/ym070_210_r099.model @@ -5,6 +5,7 @@ value_of_max_radian_position 262144 value_of_min_radian_position -262144 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.01 [control table] Address Size Data Name diff --git a/param/dxl_model/ym080_220_m001.model b/param/dxl_model/ym080_220_m001.model index 4cc5c06..82e1ae7 100644 --- a/param/dxl_model/ym080_220_m001.model +++ b/param/dxl_model/ym080_220_m001.model @@ -1,10 +1,11 @@ [type info] name value -value_of_zero_radian_position 262144 -value_of_max_radian_position 524287 -value_of_min_radian_position 0 +value_of_zero_radian_position 0 +value_of_max_radian_position 262144 +value_of_min_radian_position -262144 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.01 [control table] Address Size Data Name diff --git a/param/dxl_model/ym080_230_a099.model b/param/dxl_model/ym080_230_a099.model index a530828..82e1ae7 100644 --- a/param/dxl_model/ym080_230_a099.model +++ b/param/dxl_model/ym080_230_a099.model @@ -5,6 +5,7 @@ value_of_max_radian_position 262144 value_of_min_radian_position -262144 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.01 [control table] Address Size Data Name diff --git a/param/dxl_model/ym080_230_b001.model b/param/dxl_model/ym080_230_b001.model index 994a64c..4660f52 100644 --- a/param/dxl_model/ym080_230_b001.model +++ b/param/dxl_model/ym080_230_b001.model @@ -5,6 +5,7 @@ value_of_max_radian_position 262144 value_of_min_radian_position -262144 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.01 [control table] Address Size Data Name diff --git a/param/dxl_model/ym080_230_r051.model b/param/dxl_model/ym080_230_r051.model index 04d5d9c..82e1ae7 100644 --- a/param/dxl_model/ym080_230_r051.model +++ b/param/dxl_model/ym080_230_r051.model @@ -1,10 +1,11 @@ [type info] name value -value_of_zero_radian_position 13369344 -value_of_max_radian_position 26738687 -value_of_min_radian_position 0 +value_of_zero_radian_position 0 +value_of_max_radian_position 262144 +value_of_min_radian_position -262144 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.01 [control table] Address Size Data Name diff --git a/param/dxl_model/ym080_230_r099.model b/param/dxl_model/ym080_230_r099.model index a530828..82e1ae7 100644 --- a/param/dxl_model/ym080_230_r099.model +++ b/param/dxl_model/ym080_230_r099.model @@ -5,6 +5,7 @@ value_of_max_radian_position 262144 value_of_min_radian_position -262144 min_radian -3.14159265 max_radian 3.14159265 +velocity_unit 0.01 [control table] Address Size Data Name diff --git a/src/dynamixel/dynamixel.cpp b/src/dynamixel/dynamixel.cpp index 17eb3c3..aad50ed 100644 --- a/src/dynamixel/dynamixel.cpp +++ b/src/dynamixel/dynamixel.cpp @@ -21,6 +21,8 @@ #include #include #include +#include +#include namespace dynamixel_hardware_interface { @@ -78,6 +80,68 @@ Dynamixel::~Dynamixel() fprintf(stderr, "Dynamixel destructor end\n"); } +DxlError Dynamixel::ReadDxlModelFile(uint8_t id, uint16_t model_num) +{ + try { + dxl_info_.ReadDxlModelFile(id, model_num); + return DxlError::OK; + } catch (const std::exception & e) { + fprintf(stderr, "[ReadDxlModelFile][ID:%03d] Error reading model file: %s\n", id, e.what()); + return DxlError::CANNOT_FIND_CONTROL_ITEM; + } +} + +DxlError Dynamixel::InitTorqueStates(std::vector id_arr, bool disable_torque) +{ + for (auto it_id : id_arr) { + try { + if (dxl_info_.CheckDxlControlItem(it_id, "Torque Enable")) { + uint32_t torque_state = 0; + DxlError result = ReadItem(it_id, "Torque Enable", torque_state); + if (result != DxlError::OK) { + fprintf(stderr, "[InitTorqueStates][ID:%03d] Error reading torque state\n", it_id); + return result; + } + + torque_state_[it_id] = torque_state; + + if (torque_state_[it_id] == TORQUE_ON) { + if (disable_torque) { + fprintf( + stderr, "[InitTorqueStates][ID:%03d] Torque is enabled, disabling torque\n", + it_id); + result = WriteItem(it_id, "Torque Enable", TORQUE_OFF); + if (result != DxlError::OK) { + fprintf(stderr, "[InitTorqueStates][ID:%03d] Error disabling torque\n", it_id); + return result; + } + torque_state_[it_id] = TORQUE_OFF; + } else { + torque_state_[it_id] = TORQUE_OFF; + fprintf( + stderr, + "[InitTorqueStates][ID:%03d] Torque is enabled, cannot proceed. Set " + "'disable_torque_at_init' parameter to 'true' to disable torque at initialization " + "or disable torque manually.\n", + it_id); + return DxlError::DLX_HARDWARE_ERROR; + } + } + + fprintf( + stderr, "[InitTorqueStates][ID:%03d] Current torque state: %s\n", it_id, + torque_state_[it_id] ? "ON" : "OFF"); + } + } catch (const std::exception & e) { + fprintf( + stderr, "[InitTorqueStates][ID:%03d] Error checking control item: %s\n", it_id, + e.what()); + return DxlError::CANNOT_FIND_CONTROL_ITEM; + } + } + return DxlError::OK; +} + DxlError Dynamixel::InitDxlComm( std::vector id_arr, std::string port_name, @@ -113,9 +177,12 @@ DxlError Dynamixel::InitDxlComm( } else if (dxl_error != 0) { fprintf(stderr, " - RX_PACKET_ERROR : %s\n", packet_handler_->getRxPacketError(dxl_error)); uint32_t err = 0; - ReadItem(it_id, "Hardware Error Status", err); - fprintf(stderr, "Read Hardware Error Status : %x\n", err); - return DxlError::CANNOT_FIND_CONTROL_ITEM; + if (ReadItem(it_id, "Hardware Error Status", err) == DxlError::OK) { + fprintf(stderr, "[ID:%03d] Read Hardware Error Status : %x\n", it_id, err); + } + fprintf(stderr, "[ID:%03d] Hardware Error detected, rebooting...\n", it_id); + Reboot(it_id); + return DxlError::DLX_HARDWARE_ERROR; } else { fprintf(stderr, " - Ping succeeded. Dynamixel model number : %d\n", dxl_model_number); } @@ -123,7 +190,7 @@ DxlError Dynamixel::InitDxlComm( try { dxl_info_.ReadDxlModelFile(it_id, dxl_model_number); } catch (const std::exception & e) { - fprintf(stderr, "Error reading model file for ID %d: %s\n", it_id, e.what()); + fprintf(stderr, "[InitDxlComm][ID:%03d] Error reading model file: %s\n", it_id, e.what()); return DxlError::CANNOT_FIND_CONTROL_ITEM; } } @@ -131,17 +198,6 @@ DxlError Dynamixel::InitDxlComm( read_data_list_.clear(); write_data_list_.clear(); - for (auto it_id : id_arr) { - try { - if (dxl_info_.CheckDxlControlItem(it_id, "Torque Enable")) { - torque_state_[it_id] = TORQUE_OFF; - } - } catch (const std::exception & e) { - fprintf(stderr, "Error checking control item for ID %d: %s\n", it_id, e.what()); - return DxlError::CANNOT_FIND_CONTROL_ITEM; - } - } - return DxlError::OK; } @@ -177,6 +233,7 @@ void Dynamixel::RWDataReset() DxlError Dynamixel::SetDxlReadItems( uint8_t id, + uint8_t comm_id, std::vector item_names, std::vector> data_vec_ptr) { @@ -185,31 +242,60 @@ DxlError Dynamixel::SetDxlReadItems( return DxlError::OK; } - RWItemList read_item; - read_item.id = id; + if (item_names.size() != data_vec_ptr.size()) { + fprintf( + stderr, "Incorrect Read Data Size!!![%zu] [%zu]\n", + item_names.size(), data_vec_ptr.size()); + return DxlError::SET_READ_ITEM_FAIL; + } + // Process items and get their addresses and sizes + std::vector item_ids; + std::vector item_addrs; + std::vector item_sizes; for (auto it_name : item_names) { uint16_t ITEM_ADDR; uint8_t ITEM_SIZE; if (dxl_info_.GetDxlControlItem(id, it_name, ITEM_ADDR, ITEM_SIZE) == false) { fprintf( - stderr, "[ID:%03d] Cannot find control item in model file. : %s\n", id, + stderr, "[ID:%03d] Cannot find control item in model file : %s\n", id, it_name.c_str()); return DxlError::CANNOT_FIND_CONTROL_ITEM; } - - read_item.item_name.push_back(it_name); - read_item.item_addr.push_back(ITEM_ADDR); - read_item.item_size.push_back(ITEM_SIZE); - } - if (item_names.size() != data_vec_ptr.size()) { - fprintf( - stderr, "Incorrect Read Data Size!!![%zu] [%zu]\n", - item_names.size(), data_vec_ptr.size()); - return DxlError::SET_READ_ITEM_FAIL; + item_ids.push_back(id); + item_addrs.push_back(ITEM_ADDR); + item_sizes.push_back(ITEM_SIZE); + } + + // Check if there's already an entry with this comm_id + for (auto & existing_item : read_data_list_) { + if (existing_item.comm_id == comm_id) { + // Found existing entry, append new items + existing_item.id_arr.insert(existing_item.id_arr.end(), item_ids.begin(), item_ids.end()); + existing_item.item_name.insert( + existing_item.item_name.end(), item_names.begin(), + item_names.end()); + existing_item.item_addr.insert( + existing_item.item_addr.end(), item_addrs.begin(), + item_addrs.end()); + existing_item.item_size.insert( + existing_item.item_size.end(), item_sizes.begin(), + item_sizes.end()); + existing_item.item_data_ptr_vec.insert( + existing_item.item_data_ptr_vec.end(), + data_vec_ptr.begin(), data_vec_ptr.end()); + return DxlError::OK; + } } - read_item.item_data_ptr_vec = data_vec_ptr; + // No existing entry found, create new one + RWItemList read_item; + read_item.comm_id = comm_id; + read_item.id_arr = std::move(item_ids); + read_item.item_name = std::move(item_names); + read_item.item_addr = std::move(item_addrs); + read_item.item_size = std::move(item_sizes); + read_item.item_data_ptr_vec = std::move(data_vec_ptr); read_data_list_.push_back(read_item); @@ -228,17 +314,21 @@ DxlError Dynamixel::SetMultiDxlRead() if (read_type_ == SYNC) { fprintf(stderr, "ID : "); for (auto it_read_data_list : read_data_list_) { - fprintf(stderr, "%d, ", it_read_data_list.id); + fprintf(stderr, "%d, ", it_read_data_list.comm_id); } fprintf(stderr, "\n"); fprintf(stderr, "Read items : "); - for (auto it_read_data_list_item_name : read_data_list_.at(0).item_name) { - fprintf(stderr, "\t%s", it_read_data_list_item_name.c_str()); + if (!read_data_list_.empty()) { + for (auto it_read_data_list_item_name : read_data_list_.at(0).item_name) { + fprintf(stderr, "\t%s", it_read_data_list_item_name.c_str()); + } + } else { + fprintf(stderr, "(none)"); } fprintf(stderr, "\n"); } else { for (auto it_read_data_list : read_data_list_) { - fprintf(stderr, "ID : %d", it_read_data_list.id); + fprintf(stderr, "ID : %d", it_read_data_list.comm_id); fprintf(stderr, "\tRead items : "); for (auto it_read_data_list_item_name : it_read_data_list.item_name) { fprintf(stderr, "\t%s", it_read_data_list_item_name.c_str()); @@ -256,6 +346,7 @@ DxlError Dynamixel::SetMultiDxlRead() DxlError Dynamixel::SetDxlWriteItems( uint8_t id, + uint8_t comm_id, std::vector item_names, std::vector> data_vec_ptr) { @@ -264,28 +355,57 @@ DxlError Dynamixel::SetDxlWriteItems( return DxlError::OK; } - RWItemList write_item; - write_item.id = id; + if (item_names.size() != data_vec_ptr.size()) { + fprintf( + stderr, "Incorrect Write Data Size!!![%zu] [%zu]\n", + item_names.size(), data_vec_ptr.size()); + return DxlError::SET_WRITE_ITEM_FAIL; + } + std::vector item_ids; + std::vector item_addrs; + std::vector item_sizes; for (auto it_name : item_names) { uint16_t ITEM_ADDR; uint8_t ITEM_SIZE; if (dxl_info_.GetDxlControlItem(id, it_name, ITEM_ADDR, ITEM_SIZE) == false) { fprintf( - stderr, "[ID:%03d] Cannot find control item in model file. : .%s\n", id, + stderr, "[ID:%03d] Cannot find control item in model file : %s\n", id, it_name.c_str()); return DxlError::CANNOT_FIND_CONTROL_ITEM; } - - write_item.item_name.push_back(it_name); - write_item.item_addr.push_back(ITEM_ADDR); - write_item.item_size.push_back(ITEM_SIZE); - } - if (item_names.size() != data_vec_ptr.size()) { - fprintf(stderr, "Incorrect Write Data Size!!!"); - return DxlError::SET_WRITE_ITEM_FAIL; + item_ids.push_back(id); + item_addrs.push_back(ITEM_ADDR); + item_sizes.push_back(ITEM_SIZE); + } + + for (auto & existing_item : write_data_list_) { + if (existing_item.comm_id == comm_id) { + existing_item.id_arr.insert(existing_item.id_arr.end(), item_ids.begin(), item_ids.end()); + existing_item.item_name.insert( + existing_item.item_name.end(), item_names.begin(), + item_names.end()); + existing_item.item_addr.insert( + existing_item.item_addr.end(), item_addrs.begin(), + item_addrs.end()); + existing_item.item_size.insert( + existing_item.item_size.end(), item_sizes.begin(), + item_sizes.end()); + existing_item.item_data_ptr_vec.insert( + existing_item.item_data_ptr_vec.end(), + data_vec_ptr.begin(), data_vec_ptr.end()); + return DxlError::OK; + } } - write_item.item_data_ptr_vec = data_vec_ptr; + + // No existing entry found, create new one + RWItemList write_item; + write_item.comm_id = comm_id; + write_item.id_arr = std::move(item_ids); + write_item.item_name = std::move(item_names); + write_item.item_addr = std::move(item_addrs); + write_item.item_size = std::move(item_sizes); + write_item.item_data_ptr_vec = std::move(data_vec_ptr); write_data_list_.push_back(write_item); @@ -303,7 +423,7 @@ DxlError Dynamixel::SetMultiDxlWrite() if (write_type_ == SYNC) { fprintf(stderr, "ID : "); for (auto it_id : write_data_list_) { - fprintf(stderr, "%d, ", it_id.id); + fprintf(stderr, "%d, ", it_id.comm_id); } fprintf(stderr, "\n"); fprintf(stderr, "Write items : "); @@ -317,7 +437,7 @@ DxlError Dynamixel::SetMultiDxlWrite() fprintf(stderr, "\n"); } else { for (auto it_id : write_data_list_) { - fprintf(stderr, "ID : %d", it_id.id); + fprintf(stderr, "ID : %d", it_id.comm_id); fprintf(stderr, "\tWrite items : "); for (auto it_name : it_id.item_name) { fprintf(stderr, "\t%s", it_name.c_str()); @@ -367,25 +487,25 @@ DxlError Dynamixel::DynamixelDisable(std::vector id_arr) return result; } -DxlError Dynamixel::SetOperatingMode(uint8_t dxl_id, uint8_t dynamixel_mode) -{ - if (WriteItem(dxl_id, "Operating Mode", dynamixel_mode) == false) { - return DxlError::ITEM_WRITE_FAIL; - } - - fprintf(stderr, "[ID:%03d] Succeeded to set operating mode(", dxl_id); - if (dynamixel_mode == DXL_POSITION_CTRL_MODE) { - fprintf(stderr, "Position Control Mode)\n"); - } else if (dynamixel_mode == DXL_CURRENT_CTRL_MODE) { - fprintf(stderr, "Current Control Mode)\n"); - } else if (dynamixel_mode == DXL_VELOCITY_CTRL_MODE) { - fprintf(stderr, "Velocity Control Mode)\n"); - } else { - fprintf(stderr, "Not Defined Control Mode)\n"); - } - - return DxlError::OK; -} +// DxlError Dynamixel::SetOperatingMode(uint8_t dxl_id, uint8_t dynamixel_mode) +// { +// if (WriteItem(dxl_id, "Operating Mode", dynamixel_mode) == false) { +// return DxlError::ITEM_WRITE_FAIL; +// } + +// fprintf(stderr, "[ID:%03d] Succeeded to set operating mode(", dxl_id); +// if (dynamixel_mode == DXL_POSITION_CTRL_MODE) { +// fprintf(stderr, "Position Control Mode)\n"); +// } else if (dynamixel_mode == DXL_CURRENT_CTRL_MODE) { +// fprintf(stderr, "Current Control Mode)\n"); +// } else if (dynamixel_mode == DXL_VELOCITY_CTRL_MODE) { +// fprintf(stderr, "Velocity Control Mode)\n"); +// } else { +// fprintf(stderr, "Not Defined Control Mode)\n"); +// } + +// return DxlError::OK; +// } DxlError Dynamixel::WriteItem(uint8_t id, std::string item_name, uint32_t data) { @@ -393,7 +513,7 @@ DxlError Dynamixel::WriteItem(uint8_t id, std::string item_name, uint32_t data) uint8_t ITEM_SIZE; if (dxl_info_.GetDxlControlItem(id, item_name, ITEM_ADDR, ITEM_SIZE) == false) { fprintf( - stderr, "[ID:%03d] Cannot find control item in model file. : %s\n", id, + stderr, "[WriteItem][ID:%03d] Cannot find control item in model file. : %s\n", id, item_name.c_str()); return DxlError::CANNOT_FIND_CONTROL_ITEM; } @@ -403,41 +523,57 @@ DxlError Dynamixel::WriteItem(uint8_t id, std::string item_name, uint32_t data) DxlError Dynamixel::WriteItem(uint8_t id, uint16_t addr, uint8_t size, uint32_t data) { - int dxl_comm_result = COMM_TX_FAIL; - uint8_t dxl_error = 0; - - - if (size == 1) { - dxl_comm_result = - packet_handler_->write1ByteTxRx( - port_handler_, id, addr, static_cast(data), - &dxl_error); - } else if (size == 2) { - dxl_comm_result = packet_handler_->write2ByteTxRx( - port_handler_, id, addr, - static_cast(data), &dxl_error); - } else if (size == 4) { - dxl_comm_result = packet_handler_->write4ByteTxRx( - port_handler_, id, addr, - static_cast(data), &dxl_error); - } + uint8_t comm_id = comm_id_[id]; + for (int i = 0; i < MAX_COMM_RETRIES; i++) { + int dxl_comm_result = COMM_TX_FAIL; + uint8_t dxl_error = 0; + + if (size == 1) { + dxl_comm_result = + packet_handler_->write1ByteTxRx( + port_handler_, comm_id, addr, static_cast(data), + &dxl_error); + } else if (size == 2) { + dxl_comm_result = packet_handler_->write2ByteTxRx( + port_handler_, comm_id, addr, + static_cast(data), &dxl_error); + } else if (size == 4) { + dxl_comm_result = packet_handler_->write4ByteTxRx( + port_handler_, comm_id, addr, + static_cast(data), &dxl_error); + } - if (dxl_comm_result != COMM_SUCCESS) { - fprintf( - stderr, - "[ID:%03d] COMM_ERROR : %s\n", - id, - packet_handler_->getTxRxResult(dxl_comm_result)); - return DxlError::ITEM_WRITE_FAIL; - } else if (dxl_error != 0) { - fprintf( - stderr, - "[ID:%03d] RX_PACKET_ERROR : %s\n", - id, - packet_handler_->getRxPacketError(dxl_error)); - return DxlError::ITEM_WRITE_FAIL; + if (dxl_comm_result != COMM_SUCCESS) { + fprintf( + stderr, + "[WriteItem][ID:%03d][comm_id:%03d] COMM_ERROR : %s (retry %d/%d)\n", + id, + comm_id, + packet_handler_->getTxRxResult(dxl_comm_result), + i + 1, + MAX_COMM_RETRIES); + if (i == MAX_COMM_RETRIES - 1) { + return DxlError::ITEM_WRITE_FAIL; + } + } else if (dxl_error != 0) { + fprintf( + stderr, + "[WriteItem][ID:%03d][comm_id:%03d] RX_PACKET_ERROR : %s (retry %d/%d)\n", + id, + comm_id, + packet_handler_->getRxPacketError(dxl_error), + i + 1, + MAX_COMM_RETRIES); + if (i == MAX_COMM_RETRIES - 1) { + return DxlError::ITEM_WRITE_FAIL; + } + } else { + return DxlError::OK; + } + std::this_thread::sleep_for(std::chrono::milliseconds(100)); } - return DxlError::OK; + fprintf(stderr, "MAX_COMM_RETRIES should be set to 1 or more\n"); + return DxlError::ITEM_WRITE_FAIL; } DxlError Dynamixel::InsertWriteItemBuf(uint8_t id, std::string item_name, uint32_t data) @@ -508,48 +644,66 @@ DxlError Dynamixel::ReadItem(uint8_t id, std::string item_name, uint32_t & data) uint8_t ITEM_SIZE; if (dxl_info_.GetDxlControlItem(id, item_name, ITEM_ADDR, ITEM_SIZE) == false) { fprintf( - stderr, "[ID:%03d] Cannot find control item in model file. : %s\n", id, + stderr, "[ReadItem][ID:%03d] Cannot find control item in model file. : %s\n", id, item_name.c_str()); return DxlError::CANNOT_FIND_CONTROL_ITEM; } - int dxl_comm_result = COMM_TX_FAIL; - uint8_t dxl_error = 0; - - if (ITEM_SIZE == 1) { - uint8_t read_data; - dxl_comm_result = packet_handler_->read1ByteTxRx( - port_handler_, id, ITEM_ADDR, - &read_data, &dxl_error); - data = read_data; - } else if (ITEM_SIZE == 2) { - uint16_t read_data; - dxl_comm_result = packet_handler_->read2ByteTxRx( - port_handler_, id, ITEM_ADDR, - &read_data, &dxl_error); - data = read_data; - } else if (ITEM_SIZE == 4) { - uint32_t read_data; - dxl_comm_result = packet_handler_->read4ByteTxRx( - port_handler_, id, ITEM_ADDR, - &read_data, &dxl_error); - data = read_data; - } + uint8_t comm_id = comm_id_[id]; + for (int i = 0; i < MAX_COMM_RETRIES; i++) { + int dxl_comm_result = COMM_TX_FAIL; + uint8_t dxl_error = 0; + + if (ITEM_SIZE == 1) { + uint8_t read_data; + dxl_comm_result = packet_handler_->read1ByteTxRx( + port_handler_, comm_id, ITEM_ADDR, + &read_data, &dxl_error); + data = read_data; + } else if (ITEM_SIZE == 2) { + uint16_t read_data; + dxl_comm_result = packet_handler_->read2ByteTxRx( + port_handler_, comm_id, ITEM_ADDR, + &read_data, &dxl_error); + data = read_data; + } else if (ITEM_SIZE == 4) { + uint32_t read_data; + dxl_comm_result = packet_handler_->read4ByteTxRx( + port_handler_, comm_id, ITEM_ADDR, + &read_data, &dxl_error); + data = read_data; + } - if (dxl_comm_result != COMM_SUCCESS) { - fprintf( - stderr, "[ID:%03d] COMM_ERROR : %s\n", - id, - packet_handler_->getTxRxResult(dxl_comm_result)); - return DxlError::ITEM_READ_FAIL; - } else if (dxl_error != 0) { - fprintf( - stderr, "[ID:%03d] RX_PACKET_ERROR : %s\n", - id, - packet_handler_->getRxPacketError(dxl_error)); - return DxlError::ITEM_READ_FAIL; + if (dxl_comm_result != COMM_SUCCESS) { + fprintf( + stderr, + "[ReadItem][ID:%03d][comm_id:%03d] COMM_ERROR : %s (retry %d/%d)\n", + id, + comm_id, + packet_handler_->getTxRxResult(dxl_comm_result), + i + 1, + MAX_COMM_RETRIES); + if (i == MAX_COMM_RETRIES - 1) { + return DxlError::ITEM_READ_FAIL; + } + } else if (dxl_error != 0) { + fprintf( + stderr, + "[ReadItem][ID:%03d][comm_id:%03d] RX_PACKET_ERROR : %s (retry %d/%d)\n", + id, + comm_id, + packet_handler_->getRxPacketError(dxl_error), + i + 1, + MAX_COMM_RETRIES); + if (i == MAX_COMM_RETRIES - 1) { + return DxlError::ITEM_READ_FAIL; + } + } else { + return DxlError::OK; + } } - return DxlError::OK; + fprintf(stderr, "MAX_COMM_RETRIES should be set to 1 or more\n"); + return DxlError::ITEM_READ_FAIL; } @@ -693,6 +847,10 @@ std::string Dynamixel::DxlErrorToString(DxlError error_num) DxlError Dynamixel::ReadMultiDxlData(double period_ms) { + if (read_data_list_.empty()) { + return DxlError::OK; + } + if (read_type_ == SYNC) { return GetDxlValueFromSyncRead(period_ms); } else { @@ -702,6 +860,10 @@ DxlError Dynamixel::ReadMultiDxlData(double period_ms) DxlError Dynamixel::WriteMultiDxlData() { + if (write_data_list_.empty()) { + return DxlError::OK; + } + if (write_type_ == SYNC) { return SetDxlValueToSyncWrite(); } else { @@ -716,15 +878,15 @@ bool Dynamixel::checkReadType() uint16_t indirect_addr[2]; // [i-1], [i] uint8_t indirect_size[2]; // [i-1], [i] - if (CheckIndirectReadAvailable(read_data_list_.at(dxl_index - 1).id) != DxlError::OK) { + if (CheckIndirectReadAvailable(read_data_list_.at(dxl_index - 1).comm_id) != DxlError::OK) { return BULK; } if (!dxl_info_.GetDxlControlItem( - read_data_list_.at(dxl_index).id, "Indirect Data Read", indirect_addr[1], + read_data_list_.at(dxl_index).comm_id, "Indirect Data Read", indirect_addr[1], indirect_size[1]) || !dxl_info_.GetDxlControlItem( - read_data_list_.at(dxl_index - 1).id, "Indirect Data Read", indirect_addr[0], + read_data_list_.at(dxl_index - 1).comm_id, "Indirect Data Read", indirect_addr[0], indirect_size[0])) { return BULK; @@ -762,15 +924,15 @@ bool Dynamixel::checkWriteType() uint16_t indirect_addr[2]; // [i-1], [i] uint8_t indirect_size[2]; // [i-1], [i] - if (CheckIndirectWriteAvailable(write_data_list_.at(dxl_index - 1).id) != DxlError::OK) { + if (CheckIndirectWriteAvailable(write_data_list_.at(dxl_index - 1).comm_id) != DxlError::OK) { return BULK; } if (!dxl_info_.GetDxlControlItem( - write_data_list_.at(dxl_index).id, "Indirect Data Write", indirect_addr[1], + write_data_list_.at(dxl_index).comm_id, "Indirect Data Write", indirect_addr[1], indirect_size[1]) || !dxl_info_.GetDxlControlItem( - write_data_list_.at(dxl_index - 1).id, "Indirect Data Write", indirect_addr[0], + write_data_list_.at(dxl_index - 1).comm_id, "Indirect Data Write", indirect_addr[0], indirect_size[0])) { return BULK; @@ -818,7 +980,7 @@ DxlError Dynamixel::SetSyncReadItemAndHandler() { std::vector id_arr; for (auto it_read_data : read_data_list_) { - id_arr.push_back(it_read_data.id); + id_arr.push_back(it_read_data.comm_id); } DynamixelDisable(id_arr); @@ -827,7 +989,7 @@ DxlError Dynamixel::SetSyncReadItemAndHandler() for (auto it_read_data : read_data_list_) { for (size_t item_index = 0; item_index < it_read_data.item_name.size(); item_index++) { auto result = AddIndirectRead( - it_read_data.id, + it_read_data.comm_id, it_read_data.item_name.at(item_index), it_read_data.item_addr.at(item_index), it_read_data.item_size.at(item_index)); @@ -835,10 +997,15 @@ DxlError Dynamixel::SetSyncReadItemAndHandler() if (result == DxlError::OK) { } else { fprintf( - stderr, "[ID:%03d] Failed to Indirect Address Read Item : [%s], %d\n", - it_read_data.id, + stderr, + "[ID:%03d] Failed to Set Indirect Address Read Item: [%s], Addr: %d, Size: %d, " + "Error code: %d\n", + it_read_data.comm_id, it_read_data.item_name.at(item_index).c_str(), + it_read_data.item_addr.at(item_index), + it_read_data.item_size.at(item_index), result); + return DxlError::SET_SYNC_READ_FAIL; } } } @@ -892,6 +1059,11 @@ DxlError Dynamixel::SetFastSyncReadHandler(std::vector id_arr) DxlError Dynamixel::SetSyncReadHandler(std::vector id_arr) { + if (id_arr.size() == 0) { + fprintf(stderr, "No Sync Read Item, not setting sync read handler\n"); + return DxlError::OK; + } + // Try to set up fast sync read first if (use_fast_read_protocol_) { DxlError fast_result = SetFastSyncReadHandler(id_arr); @@ -955,11 +1127,12 @@ DxlError Dynamixel::GetDxlValueFromSyncRead(double period_ms) if (comm_result == DxlError::OK) { // Success, process data, and use fast sync read permanently for (auto it_read_data : read_data_list_) { - uint8_t id = it_read_data.id; + uint8_t id = it_read_data.comm_id; uint16_t indirect_addr = indirect_info_read_[id].indirect_data_addr; ProcessReadData( id, indirect_addr, + it_read_data.id_arr, indirect_info_read_[id].item_name, indirect_info_read_[id].item_size, it_read_data.item_data_ptr_vec, @@ -983,7 +1156,7 @@ DxlError Dynamixel::GetDxlValueFromSyncRead(double period_ms) // Set up normal sync read handler std::vector id_arr; for (auto it_read_data : read_data_list_) { - id_arr.push_back(it_read_data.id); + id_arr.push_back(it_read_data.comm_id); } SetSyncReadHandler(id_arr); } @@ -1000,11 +1173,12 @@ DxlError Dynamixel::GetDxlValueFromSyncRead(double period_ms) return comm_result; } for (auto it_read_data : read_data_list_) { - uint8_t id = it_read_data.id; + uint8_t id = it_read_data.comm_id; uint16_t indirect_addr = indirect_info_read_[id].indirect_data_addr; ProcessReadData( id, indirect_addr, + it_read_data.id_arr, indirect_info_read_[id].item_name, indirect_info_read_[id].item_size, it_read_data.item_data_ptr_vec, @@ -1036,11 +1210,11 @@ DxlError Dynamixel::SetBulkReadItemAndHandler() std::vector direct_read_data_list; for (auto it_read_data : read_data_list_) { - if (CheckIndirectReadAvailable(it_read_data.id) == DxlError::OK) { - indirect_id_arr.push_back(it_read_data.id); + if (CheckIndirectReadAvailable(it_read_data.comm_id) == DxlError::OK) { + indirect_id_arr.push_back(it_read_data.comm_id); indirect_read_data_list.push_back(it_read_data); } else { - direct_id_arr.push_back(it_read_data.id); + direct_id_arr.push_back(it_read_data.comm_id); direct_read_data_list.push_back(it_read_data); } } @@ -1067,7 +1241,7 @@ DxlError Dynamixel::SetBulkReadItemAndHandler() } // Call AddDirectRead once per id if (AddDirectRead( - it_read_data.id, + it_read_data.comm_id, group_item_names, min_addr, total_size) != DxlError::OK) @@ -1089,7 +1263,7 @@ DxlError Dynamixel::SetBulkReadItemAndHandler() item_index++) { auto result = AddIndirectRead( - it_read_data.id, + it_read_data.comm_id, it_read_data.item_name.at(item_index), it_read_data.item_addr.at(item_index), it_read_data.item_size.at(item_index)); @@ -1097,20 +1271,26 @@ DxlError Dynamixel::SetBulkReadItemAndHandler() if (result == DxlError::OK) { fprintf( stderr, "[ID:%03d] Add Indirect Address Read Item : [%s]\n", - it_read_data.id, + it_read_data.comm_id, it_read_data.item_name.at(item_index).c_str()); - } else if (result == DxlError::SET_BULK_READ_FAIL) { + } else if (result == DxlError::INDIRECT_ADDR_FAIL) { fprintf( - stderr, "[ID:%03d] Failed to Indirect Address Read Item : [%s], %d\n", - it_read_data.id, + stderr, + "[ID:%03d] Failed to Set Indirect Address Read Item : [%s], Addr: %d, Size: %d, " + "Error code: %d\n", + it_read_data.comm_id, it_read_data.item_name.at(item_index).c_str(), + it_read_data.item_addr.at(item_index), + it_read_data.item_size.at(item_index), result); + return DxlError::INDIRECT_ADDR_FAIL; } else if (result == DxlError::CANNOT_FIND_CONTROL_ITEM) { fprintf( stderr, "[ID:%03d] 'Indirect Address Read' is not defined in control table, " "Cannot set Indirect Address Read for : [%s]\n", - it_read_data.id, + it_read_data.comm_id, it_read_data.item_name.at(item_index).c_str()); + return DxlError::CANNOT_FIND_CONTROL_ITEM; } } } @@ -1260,7 +1440,7 @@ DxlError Dynamixel::GetDxlValueFromBulkRead(double period_ms) } for (auto it_read_data : read_data_list_) { - uint8_t id = it_read_data.id; + uint8_t id = it_read_data.comm_id; uint16_t indirect_addr = indirect_info_read_[id].indirect_data_addr; if (CheckIndirectReadAvailable(id) != DxlError::OK) { ProcessDirectReadData( @@ -1275,6 +1455,7 @@ DxlError Dynamixel::GetDxlValueFromBulkRead(double period_ms) ProcessReadData( id, indirect_addr, + it_read_data.id_arr, indirect_info_read_[id].item_name, indirect_info_read_[id].item_size, it_read_data.item_data_ptr_vec, @@ -1304,8 +1485,8 @@ DxlError Dynamixel::GetDxlValueFromBulkRead(double period_ms) std::vector indirect_id_arr; for (auto it_read_data : read_data_list_) { - if (CheckIndirectReadAvailable(it_read_data.id) == DxlError::OK) { - indirect_id_arr.push_back(it_read_data.id); + if (CheckIndirectReadAvailable(it_read_data.comm_id) == DxlError::OK) { + indirect_id_arr.push_back(it_read_data.comm_id); } } @@ -1324,7 +1505,7 @@ DxlError Dynamixel::GetDxlValueFromBulkRead(double period_ms) } for (auto it_read_data : read_data_list_) { - uint8_t id = it_read_data.id; + uint8_t id = it_read_data.comm_id; uint16_t indirect_addr = indirect_info_read_[id].indirect_data_addr; if (CheckIndirectReadAvailable(id) != DxlError::OK) { ProcessDirectReadData( @@ -1339,6 +1520,7 @@ DxlError Dynamixel::GetDxlValueFromBulkRead(double period_ms) ProcessReadData( id, indirect_addr, + it_read_data.id_arr, indirect_info_read_[id].item_name, indirect_info_read_[id].item_size, it_read_data.item_data_ptr_vec, @@ -1435,8 +1617,9 @@ DxlError Dynamixel::ProcessReadCommunication( } DxlError Dynamixel::ProcessReadData( - uint8_t id, + uint8_t comm_id, uint16_t indirect_addr, + const std::vector & id_arr, const std::vector & item_names, const std::vector & item_sizes, const std::vector> & data_ptrs, @@ -1445,21 +1628,23 @@ DxlError Dynamixel::ProcessReadData( uint16_t current_addr = indirect_addr; for (size_t item_index = 0; item_index < item_names.size(); item_index++) { + uint8_t ID = id_arr[item_index]; uint8_t size = item_sizes[item_index]; if (item_index > 0) {current_addr += item_sizes[item_index - 1];} - uint32_t dxl_getdata = get_data_func(id, current_addr, size); + uint32_t dxl_getdata = get_data_func(comm_id, current_addr, size); if (item_names[item_index] == "Present Position") { *data_ptrs[item_index] = dxl_info_.ConvertValueToRadian( - id, + ID, static_cast(dxl_getdata)); } else if (item_names[item_index] == "Present Velocity") { - *data_ptrs[item_index] = - dxl_info_.ConvertValueRPMToVelocityRPS(static_cast(dxl_getdata)); + *data_ptrs[item_index] = dxl_info_.ConvertValueRPMToVelocityRPS( + ID, + static_cast(dxl_getdata)); } else if (item_names[item_index] == "Present Current") { *data_ptrs[item_index] = dxl_info_.ConvertCurrentToEffort( - id, + ID, static_cast(dxl_getdata)); } else { *data_ptrs[item_index] = static_cast(dxl_getdata); @@ -1469,7 +1654,7 @@ DxlError Dynamixel::ProcessReadData( } DxlError Dynamixel::ProcessDirectReadData( - uint8_t id, + uint8_t comm_id, const std::vector & item_addrs, const std::vector & item_sizes, const std::vector> & data_ptrs, @@ -1479,7 +1664,7 @@ DxlError Dynamixel::ProcessDirectReadData( uint16_t current_addr = item_addrs[item_index]; uint8_t size = item_sizes[item_index]; - uint32_t dxl_getdata = get_data_func(id, current_addr, size); + uint32_t dxl_getdata = get_data_func(comm_id, current_addr, size); *data_ptrs[item_index] = static_cast(dxl_getdata); } @@ -1528,8 +1713,15 @@ DxlError Dynamixel::AddIndirectRead( uint8_t using_size = indirect_info_read_[id].size; for (uint16_t i = 0; i < item_size; i++) { - if (WriteItem(id, INDIRECT_ADDR + (using_size * 2), 2, item_addr + i) != DxlError::OK) { - return DxlError::SET_BULK_READ_FAIL; + DxlError write_result = DxlError::INDIRECT_ADDR_FAIL; + uint16_t addr = INDIRECT_ADDR + (using_size * 2); + uint16_t item_addr_i = item_addr + i; + + write_result = WriteItem(id, addr, 2, item_addr_i); + + if (write_result != DxlError::OK) { + fprintf(stderr, "[AddIndirectRead][ID:%03d] WriteItem failed\n", id); + return DxlError::INDIRECT_ADDR_FAIL; } using_size++; } @@ -1548,7 +1740,7 @@ DxlError Dynamixel::SetSyncWriteItemAndHandler() { std::vector id_arr; for (auto it_write_data : write_data_list_) { - id_arr.push_back(it_write_data.id); + id_arr.push_back(it_write_data.comm_id); } DynamixelDisable(id_arr); @@ -1559,7 +1751,7 @@ DxlError Dynamixel::SetSyncWriteItemAndHandler() item_index++) { if (AddIndirectWrite( - it_write_data.id, + it_write_data.comm_id, it_write_data.item_name.at(item_index), it_write_data.item_addr.at(item_index), it_write_data.item_size.at(item_index)) != DxlError::OK) @@ -1581,6 +1773,11 @@ DxlError Dynamixel::SetSyncWriteItemAndHandler() DxlError Dynamixel::SetSyncWriteHandler(std::vector id_arr) { + if (id_arr.size() == 0) { + fprintf(stderr, "No Sync Write Item, not setting sync write handler\n"); + return DxlError::OK; + } + uint16_t INDIRECT_ADDR = 0; uint8_t INDIRECT_SIZE; @@ -1614,35 +1811,35 @@ DxlError Dynamixel::SetSyncWriteHandler(std::vector id_arr) DxlError Dynamixel::SetDxlValueToSyncWrite() { for (auto it_write_data : write_data_list_) { - uint8_t ID = it_write_data.id; - uint8_t * param_write_value = new uint8_t[indirect_info_write_[ID].size]; + uint8_t comm_id = it_write_data.comm_id; + uint8_t * param_write_value = new uint8_t[indirect_info_write_[comm_id].size]; uint8_t added_byte = 0; - - for (uint16_t item_index = 0; item_index < indirect_info_write_[ID].cnt; item_index++) { + for (uint16_t item_index = 0; item_index < indirect_info_write_[comm_id].cnt; item_index++) { double data = *it_write_data.item_data_ptr_vec.at(item_index); - if (indirect_info_write_[ID].item_name.at(item_index) == "Goal Position") { + uint8_t ID = it_write_data.id_arr.at(item_index); + if (indirect_info_write_[comm_id].item_name.at(item_index) == "Goal Position") { int32_t goal_position = dxl_info_.ConvertRadianToValue(ID, data); param_write_value[added_byte + 0] = DXL_LOBYTE(DXL_LOWORD(goal_position)); param_write_value[added_byte + 1] = DXL_HIBYTE(DXL_LOWORD(goal_position)); param_write_value[added_byte + 2] = DXL_LOBYTE(DXL_HIWORD(goal_position)); param_write_value[added_byte + 3] = DXL_HIBYTE(DXL_HIWORD(goal_position)); - } else if (indirect_info_write_[ID].item_name.at(item_index) == "Goal Velocity") { - int16_t goal_velocity = dxl_info_.ConvertVelocityRPSToValueRPM(data); + } else if (indirect_info_write_[comm_id].item_name.at(item_index) == "Goal Velocity") { + int32_t goal_velocity = dxl_info_.ConvertVelocityRPSToValueRPM(ID, data); param_write_value[added_byte + 0] = DXL_LOBYTE(DXL_LOWORD(goal_velocity)); param_write_value[added_byte + 1] = DXL_HIBYTE(DXL_LOWORD(goal_velocity)); param_write_value[added_byte + 2] = DXL_LOBYTE(DXL_HIWORD(goal_velocity)); param_write_value[added_byte + 3] = DXL_HIBYTE(DXL_HIWORD(goal_velocity)); - } else if (indirect_info_write_[ID].item_name.at(item_index) == "Goal Current") { + } else if (indirect_info_write_[comm_id].item_name.at(item_index) == "Goal Current") { int16_t goal_current = dxl_info_.ConvertEffortToCurrent(ID, data); param_write_value[added_byte + 0] = DXL_LOBYTE(goal_current); param_write_value[added_byte + 1] = DXL_HIBYTE(goal_current); } - added_byte += indirect_info_write_[ID].item_size.at(item_index); + added_byte += indirect_info_write_[comm_id].item_size.at(item_index); } - if (group_sync_write_->addParam(ID, param_write_value) != true) { - fprintf(stderr, "[ID:%03d] groupSyncWrite addparam failed\n", ID); + if (group_sync_write_->addParam(comm_id, param_write_value) != true) { + fprintf(stderr, "[ID:%03d] groupSyncWrite addparam failed\n", comm_id); return DxlError::SYNC_WRITE_FAIL; } } @@ -1662,7 +1859,7 @@ DxlError Dynamixel::SetBulkWriteItemAndHandler() { std::vector id_arr; for (auto it_write_data : write_data_list_) { - id_arr.push_back(it_write_data.id); + id_arr.push_back(it_write_data.comm_id); } group_bulk_write_ = new dynamixel::GroupBulkWrite(port_handler_, packet_handler_); @@ -1674,11 +1871,11 @@ DxlError Dynamixel::SetBulkWriteItemAndHandler() std::vector direct_write_data_list; for (auto it_write_data : write_data_list_) { - if (CheckIndirectWriteAvailable(it_write_data.id) == DxlError::OK) { - indirect_id_arr.push_back(it_write_data.id); + if (CheckIndirectWriteAvailable(it_write_data.comm_id) == DxlError::OK) { + indirect_id_arr.push_back(it_write_data.comm_id); indirect_write_data_list.push_back(it_write_data); } else { - direct_id_arr.push_back(it_write_data.id); + direct_id_arr.push_back(it_write_data.comm_id); direct_write_data_list.push_back(it_write_data); } } @@ -1716,17 +1913,17 @@ DxlError Dynamixel::SetBulkWriteItemAndHandler() if (addr_ranges[i].second != addr_ranges[i + 1].first) { fprintf( stderr, "[ID:%03d] Error: Gap detected between items at addresses %d and %d\n", - it_write_data.id, addr_ranges[i].second, addr_ranges[i + 1].first); + it_write_data.comm_id, addr_ranges[i].second, addr_ranges[i + 1].first); return DxlError::BULK_WRITE_FAIL; } } // Store direct write info - direct_info_write_[it_write_data.id].indirect_data_addr = min_addr; - direct_info_write_[it_write_data.id].size = total_size; - direct_info_write_[it_write_data.id].cnt = it_write_data.item_name.size(); - direct_info_write_[it_write_data.id].item_name = it_write_data.item_name; - direct_info_write_[it_write_data.id].item_size = it_write_data.item_size; + direct_info_write_[it_write_data.comm_id].indirect_data_addr = min_addr; + direct_info_write_[it_write_data.comm_id].size = total_size; + direct_info_write_[it_write_data.comm_id].cnt = it_write_data.item_name.size(); + direct_info_write_[it_write_data.comm_id].item_name = it_write_data.item_name; + direct_info_write_[it_write_data.comm_id].item_size = it_write_data.item_size; fprintf( stderr, @@ -1743,7 +1940,7 @@ DxlError Dynamixel::SetBulkWriteItemAndHandler() item_index++) { if (AddIndirectWrite( - it_write_data.id, + it_write_data.comm_id, it_write_data.item_name.at(item_index), it_write_data.item_addr.at(item_index), it_write_data.item_size.at(item_index)) != DxlError::OK) @@ -1754,7 +1951,7 @@ DxlError Dynamixel::SetBulkWriteItemAndHandler() fprintf( stderr, "[ID:%03d] Add Indirect Address Write Item : [%s]\n", - it_write_data.id, + it_write_data.comm_id, it_write_data.item_name.at(item_index).c_str()); } } @@ -1797,18 +1994,17 @@ DxlError Dynamixel::SetBulkWriteHandler(std::vector id_arr) DxlError Dynamixel::SetDxlValueToBulkWrite() { for (auto it_write_data : write_data_list_) { - uint8_t ID = it_write_data.id; + uint8_t comm_id = it_write_data.comm_id; uint8_t * param_write_value; uint8_t added_byte = 0; // Check if this is a direct write - if (direct_info_write_.find(ID) != direct_info_write_.end()) { - param_write_value = new uint8_t[direct_info_write_[ID].size]; + if (direct_info_write_.find(comm_id) != direct_info_write_.end()) { + param_write_value = new uint8_t[direct_info_write_[comm_id].size]; - for (uint16_t item_index = 0; item_index < direct_info_write_[ID].cnt; item_index++) { + for (uint16_t item_index = 0; item_index < direct_info_write_[comm_id].cnt; item_index++) { double data = *it_write_data.item_data_ptr_vec.at(item_index); - uint8_t size = direct_info_write_[ID].item_size.at(item_index); - + uint8_t size = direct_info_write_[comm_id].item_size.at(item_index); if (size == 4) { int32_t value = static_cast(data); param_write_value[added_byte + 0] = DXL_LOBYTE(DXL_LOWORD(value)); @@ -1826,47 +2022,48 @@ DxlError Dynamixel::SetDxlValueToBulkWrite() } if (group_bulk_write_->addParam( - ID, - direct_info_write_[ID].indirect_data_addr, - direct_info_write_[ID].size, + comm_id, + direct_info_write_[comm_id].indirect_data_addr, + direct_info_write_[comm_id].size, param_write_value) != true) { - fprintf(stderr, "[ID:%03d] groupBulkWrite addparam failed\n", ID); + fprintf(stderr, "[ID:%03d] groupBulkWrite addparam failed\n", comm_id); return DxlError::BULK_WRITE_FAIL; } } else { // Handle indirect write - param_write_value = new uint8_t[indirect_info_write_[ID].size]; + param_write_value = new uint8_t[indirect_info_write_[comm_id].size]; - for (uint16_t item_index = 0; item_index < indirect_info_write_[ID].cnt; item_index++) { + for (uint16_t item_index = 0; item_index < indirect_info_write_[comm_id].cnt; item_index++) { double data = *it_write_data.item_data_ptr_vec.at(item_index); - if (indirect_info_write_[ID].item_name.at(item_index) == "Goal Position") { + uint8_t ID = it_write_data.id_arr.at(item_index); + if (indirect_info_write_[comm_id].item_name.at(item_index) == "Goal Position") { int32_t goal_position = dxl_info_.ConvertRadianToValue(ID, data); param_write_value[added_byte + 0] = DXL_LOBYTE(DXL_LOWORD(goal_position)); param_write_value[added_byte + 1] = DXL_HIBYTE(DXL_LOWORD(goal_position)); param_write_value[added_byte + 2] = DXL_LOBYTE(DXL_HIWORD(goal_position)); param_write_value[added_byte + 3] = DXL_HIBYTE(DXL_HIWORD(goal_position)); - } else if (indirect_info_write_[ID].item_name.at(item_index) == "Goal Velocity") { - int32_t goal_velocity = dxl_info_.ConvertVelocityRPSToValueRPM(data); + } else if (indirect_info_write_[comm_id].item_name.at(item_index) == "Goal Velocity") { + int32_t goal_velocity = dxl_info_.ConvertVelocityRPSToValueRPM(ID, data); param_write_value[added_byte + 0] = DXL_LOBYTE(DXL_LOWORD(goal_velocity)); param_write_value[added_byte + 1] = DXL_HIBYTE(DXL_LOWORD(goal_velocity)); param_write_value[added_byte + 2] = DXL_LOBYTE(DXL_HIWORD(goal_velocity)); param_write_value[added_byte + 3] = DXL_HIBYTE(DXL_HIWORD(goal_velocity)); - } else if (indirect_info_write_[ID].item_name.at(item_index) == "Goal Current") { + } else if (indirect_info_write_[comm_id].item_name.at(item_index) == "Goal Current") { int16_t goal_current = dxl_info_.ConvertEffortToCurrent(ID, data); param_write_value[added_byte + 0] = DXL_LOBYTE(goal_current); param_write_value[added_byte + 1] = DXL_HIBYTE(goal_current); } - added_byte += indirect_info_write_[ID].item_size.at(item_index); + added_byte += indirect_info_write_[comm_id].item_size.at(item_index); } if (group_bulk_write_->addParam( - ID, - indirect_info_write_[ID].indirect_data_addr, - indirect_info_write_[ID].size, + comm_id, + indirect_info_write_[comm_id].indirect_data_addr, + indirect_info_write_[comm_id].size, param_write_value) != true) { - fprintf(stderr, "[ID:%03d] groupBulkWrite addparam failed\n", ID); + fprintf(stderr, "[ID:%03d] groupBulkWrite addparam failed\n", comm_id); return DxlError::BULK_WRITE_FAIL; } } diff --git a/src/dynamixel/dynamixel_info.cpp b/src/dynamixel/dynamixel_info.cpp index 33543e0..7ce0c8c 100644 --- a/src/dynamixel/dynamixel_info.cpp +++ b/src/dynamixel/dynamixel_info.cpp @@ -73,6 +73,8 @@ void DynamixelInfo::ReadDxlModelFile(uint8_t id, uint16_t model_num) std::string line; temp_dxl_info.model_num = model_num; + bool torque_constant_set = false; + bool velocity_unit_set = false; while (!open_file.eof() ) { getline(open_file, line); @@ -100,6 +102,10 @@ void DynamixelInfo::ReadDxlModelFile(uint8_t id, uint16_t model_num) temp_dxl_info.max_radian = static_cast(stod(strs.at(1))); } else if (strs.at(0) == "torque_constant") { temp_dxl_info.torque_constant = static_cast(stod(strs.at(1))); + torque_constant_set = true; + } else if (strs.at(0) == "velocity_unit") { + temp_dxl_info.velocity_unit = static_cast(stod(strs.at(1))); + velocity_unit_set = true; } } catch (const std::exception & e) { std::string error_msg = "Error processing line in model file: " + line + @@ -108,6 +114,20 @@ void DynamixelInfo::ReadDxlModelFile(uint8_t id, uint16_t model_num) } } + // Set default values and warn if parameters are missing + if (!torque_constant_set) { + fprintf( + stderr, "[WARN] Model file '%s' doesn't contain torque_constant parameter. " + "Using default value: 1.0\n", path.c_str()); + temp_dxl_info.torque_constant = 1.0; + } + if (!velocity_unit_set) { + fprintf( + stderr, "[WARN] Model file '%s' doesn't contain velocity_unit parameter. " + "Using default value: 0.01\n", path.c_str()); + temp_dxl_info.velocity_unit = 0.01; + } + getline(open_file, line); while (!open_file.eof() ) { getline(open_file, line); @@ -170,23 +190,25 @@ bool DynamixelInfo::CheckDxlControlItem(uint8_t id, std::string item_name) return false; } -bool DynamixelInfo::GetDxlTypeInfo( - uint8_t id, - int32_t & value_of_zero_radian_position, - int32_t & value_of_max_radian_position, - int32_t & value_of_min_radian_position, - double & min_radian, - double & max_radian, - double & torque_constant) -{ - value_of_zero_radian_position = dxl_info_[id].value_of_zero_radian_position; - value_of_max_radian_position = dxl_info_[id].value_of_max_radian_position; - value_of_min_radian_position = dxl_info_[id].value_of_min_radian_position; - min_radian = dxl_info_[id].min_radian; - max_radian = dxl_info_[id].max_radian; - torque_constant = dxl_info_[id].torque_constant; - return true; -} +// bool DynamixelInfo::GetDxlTypeInfo( +// uint8_t id, +// int32_t & value_of_zero_radian_position, +// int32_t & value_of_max_radian_position, +// int32_t & value_of_min_radian_position, +// double & min_radian, +// double & max_radian, +// double & torque_constant, +// double & velocity_unit) +// { +// value_of_zero_radian_position = dxl_info_[id].value_of_zero_radian_position; +// value_of_max_radian_position = dxl_info_[id].value_of_max_radian_position; +// value_of_min_radian_position = dxl_info_[id].value_of_min_radian_position; +// min_radian = dxl_info_[id].min_radian; +// max_radian = dxl_info_[id].max_radian; +// torque_constant = dxl_info_[id].torque_constant; +// velocity_unit = dxl_info_[id].velocity_unit; +// return true; +// } int32_t DynamixelInfo::ConvertRadianToValue(uint8_t id, double radian) { diff --git a/src/dynamixel_hardware_interface.cpp b/src/dynamixel_hardware_interface.cpp index f81d4a9..06a2a68 100644 --- a/src/dynamixel_hardware_interface.cpp +++ b/src/dynamixel_hardware_interface.cpp @@ -78,6 +78,22 @@ hardware_interface::CallbackReturn DynamixelHardware::on_init( e.what()); } + // Add new parameter for torque initialization + bool disable_torque_at_init = false; + if (info_.hardware_parameters.find("disable_torque_at_init") != info_.hardware_parameters.end()) { + disable_torque_at_init = info_.hardware_parameters.at("disable_torque_at_init") == "true"; + if (disable_torque_at_init) { + RCLCPP_INFO( + logger_, + "Torque will be disabled during initialization if it is enabled at initialization."); + } + } else { + RCLCPP_INFO( + logger_, + "If there is a torque enabled Dynamixel, the program will be terminated. Set " + "'disable_torque_at_init' parameter to 'true' to disable torque at initialization."); + } + RCLCPP_INFO_STREAM( logger_, "port_name " << port_name_.c_str() << " / baudrate " << baud_rate_.c_str()); @@ -103,37 +119,51 @@ hardware_interface::CallbackReturn DynamixelHardware::on_init( initRevoluteToPrismaticParam(); } for (const hardware_interface::ComponentInfo & gpio : info_.gpios) { - if (gpio.parameters.at("type") == "dxl") { - dxl_id_.push_back(static_cast(stoi(gpio.parameters.at("ID")))); - } else if (gpio.parameters.at("type") == "sensor") { - sensor_id_.push_back(static_cast(stoi(gpio.parameters.at("ID")))); - } else if (gpio.parameters.at("type") == "controller") { - controller_id_.push_back(static_cast(stoi(gpio.parameters.at("ID")))); + if (gpio.parameters.find("ID") == gpio.parameters.end()) { + RCLCPP_ERROR_STREAM(logger_, "ID is not found in gpio parameters"); + exit(-1); + } + if (gpio.parameters.find("type") == gpio.parameters.end()) { + RCLCPP_ERROR_STREAM(logger_, "type is not found in gpio parameters"); + exit(-1); + } + + uint8_t id = static_cast(stoi(gpio.parameters.at("ID"))); + const std::string & type = gpio.parameters.at("type"); + + if (type == "dxl") { + dxl_id_.push_back(id); + } else if (type == "sensor") { + sensor_id_.push_back(id); + } else if (type == "controller") { + controller_id_.push_back(id); + } else if (type == "virtual_dxl") { + dxl_comm_->ReadDxlModelFile(id, static_cast(stoi(gpio.parameters.at("model_num")))); + virtual_dxl_id_.push_back(id); } else { RCLCPP_ERROR_STREAM(logger_, "Invalid DXL / Sensor type"); exit(-1); } - } - bool trying_connect = true; - int trying_cnt = 60; - int cnt = 0; + uint8_t comm_id = (gpio.parameters.find("comm_id") != gpio.parameters.end()) ? + static_cast(stoi(gpio.parameters.at("comm_id"))) : id; + dxl_comm_->SetCommId(id, comm_id); + } if (controller_id_.size() > 0) { - while (trying_connect) { + for (int i = 0; i < 10; i++) { std::vector id_arr; for (auto controller : controller_id_) { id_arr.push_back(controller); } if (dxl_comm_->InitDxlComm(id_arr, port_name_, baud_rate_) == DxlError::OK) { RCLCPP_INFO_STREAM(logger_, "Trying to connect to the communication port..."); - trying_connect = false; + break; } else { - sleep(1); - cnt++; - if (cnt > trying_cnt) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + if (i == 9) { RCLCPP_ERROR_STREAM(logger_, "Cannot connect communication port! :("); - cnt = 0; + return hardware_interface::CallbackReturn::ERROR; } } } @@ -144,11 +174,7 @@ hardware_interface::CallbackReturn DynamixelHardware::on_init( return hardware_interface::CallbackReturn::ERROR; } - trying_connect = true; - trying_cnt = 60; - cnt = 0; - - while (trying_connect) { + for (int i = 0; i < 10; i++) { std::vector id_arr; for (auto dxl : dxl_id_) { id_arr.push_back(dxl); @@ -158,17 +184,29 @@ hardware_interface::CallbackReturn DynamixelHardware::on_init( } if (dxl_comm_->InitDxlComm(id_arr, port_name_, baud_rate_) == DxlError::OK) { RCLCPP_INFO_STREAM(logger_, "Trying to connect to the communication port..."); - trying_connect = false; + break; } else { - sleep(1); - cnt++; - if (cnt > trying_cnt) { + std::this_thread::sleep_for(std::chrono::seconds(1)); + if (i == 9) { RCLCPP_ERROR_STREAM(logger_, "Cannot connect communication port! :("); - cnt = 0; + return hardware_interface::CallbackReturn::ERROR; } } } + std::vector dxl_id_with_virtual_dxl; + dxl_id_with_virtual_dxl.insert(dxl_id_with_virtual_dxl.end(), dxl_id_.begin(), dxl_id_.end()); + dxl_id_with_virtual_dxl.insert( + dxl_id_with_virtual_dxl.end(), virtual_dxl_id_.begin(), + virtual_dxl_id_.end()); + if (dxl_comm_->InitTorqueStates( + dxl_id_with_virtual_dxl, + disable_torque_at_init) != DxlError::OK) + { + RCLCPP_ERROR_STREAM(logger_, "Error: InitTorqueStates"); + return hardware_interface::CallbackReturn::ERROR; + } + if (!InitDxlItems()) { RCLCPP_ERROR_STREAM(logger_, "Error: InitDxlItems"); return hardware_interface::CallbackReturn::ERROR; @@ -350,6 +388,20 @@ DynamixelHardware::export_state_interfaces() it.name, it.interface_name_vec.at(i), it.value_ptr_vec.at(i).get())); } } + for (auto it : hdl_gpio_controller_states_) { + for (size_t i = 0; i < it.value_ptr_vec.size(); i++) { + if (i >= it.interface_name_vec.size()) { + RCLCPP_ERROR_STREAM( + logger_, "Interface name vector size mismatch for gpio controller " << it.name << + ". Expected size: " << it.value_ptr_vec.size() << + ", Actual size: " << it.interface_name_vec.size()); + continue; + } + state_interfaces.emplace_back( + hardware_interface::StateInterface( + it.name, it.interface_name_vec.at(i), it.value_ptr_vec.at(i).get())); + } + } } catch (const std::exception & e) { RCLCPP_ERROR_STREAM(logger_, "Error in export_state_interfaces: " << e.what()); } @@ -447,7 +499,7 @@ hardware_interface::CallbackReturn DynamixelHardware::start() // sync commands = states joint SyncJointCommandWithStates(); - usleep(500 * 1000); + std::this_thread::sleep_for(std::chrono::milliseconds(500)); // Enable torque only for Dynamixels that have torque enabled in their parameters std::vector torque_enabled_ids; @@ -464,7 +516,7 @@ hardware_interface::CallbackReturn DynamixelHardware::start() break; } RCLCPP_ERROR_STREAM(logger_, "Failed to enable torque for Dynamixels, retry..."); - usleep(100 * 1000); + std::this_thread::sleep_for(std::chrono::milliseconds(100)); } } @@ -477,6 +529,7 @@ hardware_interface::CallbackReturn DynamixelHardware::stop() { if (dxl_comm_) { dxl_comm_->DynamixelDisable(dxl_id_); + dxl_comm_->DynamixelDisable(virtual_dxl_id_); } else { RCLCPP_ERROR_STREAM(logger_, "Dynamixel Hardware Stop Fail : dxl_comm_ is nullptr"); return hardware_interface::CallbackReturn::ERROR; @@ -650,7 +703,7 @@ bool DynamixelHardware::CommReset() auto start_time = this->now(); while ((this->now() - start_time) < rclcpp::Duration(3, 0)) { - usleep(200 * 1000); + std::this_thread::sleep_for(std::chrono::milliseconds(200)); RCLCPP_INFO_STREAM(logger_, "Reset Start"); bool result = true; for (auto id : dxl_id_) { @@ -659,7 +712,7 @@ bool DynamixelHardware::CommReset() result = false; break; } - usleep(200 * 1000); + std::this_thread::sleep_for(std::chrono::milliseconds(200)); } if (!result) {continue;} if (!InitControllerItems()) {continue;} @@ -668,44 +721,17 @@ bool DynamixelHardware::CommReset() if (!InitDxlWriteItems()) {continue;} RCLCPP_INFO_STREAM(logger_, "RESET Success"); - usleep(1000 * 1000); + std::this_thread::sleep_for(std::chrono::seconds(1)); start(); dxl_status_ = DXL_OK; return true; } RCLCPP_ERROR_STREAM(logger_, "RESET Failure"); - usleep(1000 * 1000); + std::this_thread::sleep_for(std::chrono::seconds(1)); start(); return false; } -bool DynamixelHardware::retryWriteItem(uint8_t id, const std::string & item_name, uint32_t value) -{ - rclcpp::Time start_time = this->now(); - rclcpp::Duration error_duration(0, 0); - - while (true) { - if (dxl_comm_->WriteItem(id, item_name, value) == DxlError::OK) { - RCLCPP_INFO_STREAM( - logger_, - "[ID:" << std::to_string(id) << "] item_name:" << item_name.c_str() << "\tdata:" << value); - return true; - } - - error_duration = this->now() - start_time; - if (error_duration.seconds() * 1000 >= err_timeout_ms_) { - RCLCPP_ERROR_STREAM( - logger_, - "[ID:" << std::to_string(id) << "] Write Item error (Timeout: " << - error_duration.seconds() * 1000 << "ms/" << err_timeout_ms_ << "ms)"); - return false; - } - RCLCPP_WARN_STREAM( - logger_, - "[ID:" << std::to_string(id) << "] Write Item retry..."); - } -} - bool DynamixelHardware::initItems(const std::string & type_filter) { RCLCPP_INFO_STREAM(logger_, "$$$$$ Init Items for type: " << type_filter); @@ -716,7 +742,7 @@ bool DynamixelHardware::initItems(const std::string & type_filter) uint8_t id = static_cast(stoi(gpio.parameters.at("ID"))); // Handle torque enable parameter - bool torque_enabled = type_filter == "dxl"; + bool torque_enabled = type_filter == "dxl" || type_filter == "virtual_dxl"; if (gpio.parameters.find("Torque Enable") != gpio.parameters.end()) { torque_enabled = std::stoi(gpio.parameters.at("Torque Enable")) != 0; } @@ -726,7 +752,14 @@ bool DynamixelHardware::initItems(const std::string & type_filter) for (const auto & param : gpio.parameters) { const std::string & param_name = param.first; if (param_name == "Operating Mode") { - if (!retryWriteItem(id, param_name, static_cast(stoi(param.second)))) { + RCLCPP_INFO_STREAM( + logger_, + "[ID:" << std::to_string(id) << "] item_name:" << param_name.c_str() << "\tdata:" << + param.second); + if (dxl_comm_->WriteItem( + id, param_name, + static_cast(stoi(param.second))) != DxlError::OK) + { return false; } } @@ -736,12 +769,20 @@ bool DynamixelHardware::initItems(const std::string & type_filter) for (const auto & param : gpio.parameters) { const std::string & param_name = param.first; if (param_name == "ID" || param_name == "type" || - param_name == "Torque Enable" || param_name == "Operating Mode") + param_name == "Torque Enable" || param_name == "Operating Mode" || + param_name == "model_num" || param_name == "comm_id") { continue; } if (param_name.find("Limit") != std::string::npos) { - if (!retryWriteItem(id, param_name, static_cast(stoi(param.second)))) { + RCLCPP_INFO_STREAM( + logger_, + "[ID:" << std::to_string(id) << "] item_name:" << param_name.c_str() << "\tdata:" << + param.second); + if (dxl_comm_->WriteItem( + id, param_name, + static_cast(stoi(param.second))) != DxlError::OK) + { return false; } } @@ -752,11 +793,19 @@ bool DynamixelHardware::initItems(const std::string & type_filter) const std::string & param_name = param.first; if (param_name == "ID" || param_name == "type" || param_name == "Torque Enable" || param_name == "Operating Mode" || + param_name == "model_num" || param_name == "comm_id" || param_name.find("Limit") != std::string::npos) { continue; } - if (!retryWriteItem(id, param_name, static_cast(stoi(param.second)))) { + RCLCPP_INFO_STREAM( + logger_, + "[ID:" << std::to_string(id) << "] item_name:" << param_name.c_str() << "\tdata:" << + param.second); + if (dxl_comm_->WriteItem( + id, param_name, + static_cast(stoi(param.second))) != DxlError::OK) + { return false; } } @@ -766,7 +815,7 @@ bool DynamixelHardware::initItems(const std::string & type_filter) bool DynamixelHardware::InitControllerItems() { - return initItems("controller"); + return initItems("controller") && initItems("virtual_dxl"); } bool DynamixelHardware::InitDxlItems() @@ -782,11 +831,13 @@ bool DynamixelHardware::InitDxlReadItems() if (!is_set_hdl_) { hdl_trans_states_.clear(); hdl_gpio_sensor_states_.clear(); + hdl_gpio_controller_states_.clear(); for (const hardware_interface::ComponentInfo & gpio : info_.gpios) { - if (gpio.state_interfaces.size() && gpio.parameters.at("type") == "dxl") { + if (gpio.parameters.at("type") == "dxl") { uint8_t id = static_cast(stoi(gpio.parameters.at("ID"))); HandlerVarType temp_read; temp_read.id = id; + temp_read.comm_id = id; temp_read.name = gpio.name; for (auto it : gpio.state_interfaces) { @@ -797,13 +848,12 @@ bool DynamixelHardware::InitDxlReadItems() dxl_hw_err_[id] = 0x00; } } - hdl_trans_states_.push_back(temp_read); - - } else if (gpio.state_interfaces.size() && gpio.parameters.at("type") == "sensor") { + } else if (gpio.parameters.at("type") == "sensor") { uint8_t id = static_cast(stoi(gpio.parameters.at("ID"))); HandlerVarType temp_sensor; temp_sensor.id = id; + temp_sensor.comm_id = id; temp_sensor.name = gpio.name; for (auto it : gpio.state_interfaces) { @@ -811,13 +861,50 @@ bool DynamixelHardware::InitDxlReadItems() temp_sensor.value_ptr_vec.push_back(std::make_shared(0.0)); } hdl_gpio_sensor_states_.push_back(temp_sensor); + } else if (gpio.parameters.at("type") == "controller") { + uint8_t id = static_cast(stoi(gpio.parameters.at("ID"))); + HandlerVarType temp_controller; + temp_controller.id = id; + temp_controller.comm_id = id; + temp_controller.name = gpio.name; + + for (auto it : gpio.state_interfaces) { + temp_controller.interface_name_vec.push_back(it.name); + temp_controller.value_ptr_vec.push_back(std::make_shared(0.0)); + } + hdl_gpio_controller_states_.push_back(temp_controller); + } else if (gpio.parameters.at("type") == "virtual_dxl") { + uint8_t id = static_cast(stoi(gpio.parameters.at("ID"))); + uint8_t comm_id = static_cast(stoi(gpio.parameters.at("comm_id"))); + HandlerVarType temp_read; + temp_read.id = id; + temp_read.comm_id = comm_id; + temp_read.name = gpio.name; + + for (auto it : gpio.state_interfaces) { + temp_read.interface_name_vec.push_back(it.name); + temp_read.value_ptr_vec.push_back(std::make_shared(0.0)); + + if (it.name == "Hardware Error Status") { + dxl_hw_err_[id] = 0x00; + } + } + hdl_trans_states_.push_back(temp_read); } } is_set_hdl_ = true; } + for (auto it : hdl_gpio_controller_states_) { + if (dxl_comm_->SetDxlReadItems( + it.id, it.comm_id, it.interface_name_vec, + it.value_ptr_vec) != DxlError::OK) + { + return false; + } + } for (auto it : hdl_trans_states_) { if (dxl_comm_->SetDxlReadItems( - it.id, it.interface_name_vec, + it.id, it.comm_id, it.interface_name_vec, it.value_ptr_vec) != DxlError::OK) { return false; @@ -825,7 +912,7 @@ bool DynamixelHardware::InitDxlReadItems() } for (auto it : hdl_gpio_sensor_states_) { if (dxl_comm_->SetDxlReadItems( - it.id, it.interface_name_vec, + it.id, it.comm_id, it.interface_name_vec, it.value_ptr_vec) != DxlError::OK) { return false; @@ -850,6 +937,7 @@ bool DynamixelHardware::InitDxlWriteItems() uint8_t id = static_cast(stoi(gpio.parameters.at("ID"))); HandlerVarType temp_write; temp_write.id = id; + temp_write.comm_id = id; temp_write.name = gpio.name; for (auto it : gpio.command_interfaces) { @@ -862,12 +950,12 @@ bool DynamixelHardware::InitDxlWriteItems() temp_write.interface_name_vec.push_back(it.name); temp_write.value_ptr_vec.push_back(std::make_shared(0.0)); } - hdl_trans_commands_.push_back(temp_write); } else if (gpio.parameters.at("type") == "controller") { uint8_t id = static_cast(stoi(gpio.parameters.at("ID"))); HandlerVarType temp_controller; temp_controller.id = id; + temp_controller.comm_id = id; temp_controller.name = gpio.name; for (auto it : gpio.command_interfaces) { @@ -875,6 +963,25 @@ bool DynamixelHardware::InitDxlWriteItems() temp_controller.value_ptr_vec.push_back(std::make_shared(0.0)); } hdl_gpio_controller_commands_.push_back(temp_controller); + } else if (gpio.parameters.at("type") == "virtual_dxl") { + uint8_t id = static_cast(stoi(gpio.parameters.at("ID"))); + uint8_t comm_id = static_cast(stoi(gpio.parameters.at("comm_id"))); + HandlerVarType temp_write; + temp_write.id = id; + temp_write.comm_id = comm_id; + temp_write.name = gpio.name; + + for (auto it : gpio.command_interfaces) { + if (it.name != "Goal Position" && + it.name != "Goal Velocity" && + it.name != "Goal Current") + { + continue; + } + temp_write.interface_name_vec.push_back(it.name); + temp_write.value_ptr_vec.push_back(std::make_shared(0.0)); + } + hdl_trans_commands_.push_back(temp_write); } } is_set_hdl_ = true; @@ -882,7 +989,7 @@ bool DynamixelHardware::InitDxlWriteItems() for (auto it : hdl_trans_commands_) { if (dxl_comm_->SetDxlWriteItems( - it.id, it.interface_name_vec, + it.id, it.comm_id, it.interface_name_vec, it.value_ptr_vec) != DxlError::OK) { return false; @@ -891,13 +998,12 @@ bool DynamixelHardware::InitDxlWriteItems() for (auto it : hdl_gpio_controller_commands_) { if (dxl_comm_->SetDxlWriteItems( - it.id, it.interface_name_vec, + it.id, it.comm_id, it.interface_name_vec, it.value_ptr_vec) != DxlError::OK) { return false; } } - if (dxl_comm_->SetMultiDxlWrite() != DxlError::OK) { return false; } @@ -980,7 +1086,7 @@ void DynamixelHardware::MapInterfaces( size_t outer_size, size_t inner_size, std::vector & outer_handlers, - const std::vector & inner_handlers, + std::vector & inner_handlers, double ** matrix, const std::unordered_map> & iface_map, const std::string & conversion_iface, @@ -1045,7 +1151,7 @@ void DynamixelHardware::CalcTransmissionToJoint() hdl_trans_states_, transmission_to_joint_matrix_, dynamixel_hardware_interface::ros2_to_dxl_state_map, - "Present Position", + hardware_interface::HW_IF_POSITION, conversion_joint_name_, conv ); @@ -1120,10 +1226,12 @@ void DynamixelHardware::ChangeDxlTorqueState() if (dxl_torque_status_ == REQUESTED_TO_ENABLE) { std::cout << "torque enable" << std::endl; dxl_comm_->DynamixelEnable(dxl_id_); + dxl_comm_->DynamixelEnable(virtual_dxl_id_); SyncJointCommandWithStates(); } else if (dxl_torque_status_ == REQUESTED_TO_DISABLE) { std::cout << "torque disable" << std::endl; dxl_comm_->DynamixelDisable(dxl_id_); + dxl_comm_->DynamixelDisable(virtual_dxl_id_); SyncJointCommandWithStates(); }