#include <cstdio>#include <string>#include <joint_limits_interface/joint_limits.h>#include <joint_limits_interface/joint_limits_interface.h>#include <joint_limits_interface/joint_limits_rosparam.h>#include <dynamic_reconfigure/server.h>#include <crane_x7_msgs/ServoParameterConfig.h>

Go to the source code of this file.
| Classes | |
| class | JOINT_CONTROL | 
| struct | ST_DYNAMIXEL_REG_TABLE | 
| struct | ST_JOINT_PARAM | 
| Typedefs | |
| typedef struct ST_JOINT_PARAM | ST_JOINT_PARAM | 
| Variables | |
| static const ST_DYNAMIXEL_REG_TABLE | RegTable [] | 
| #define ADDR_BUS_WATCHDOG (RegTable[enTableId_BusWatchdog].address) | 
Definition at line 139 of file joint_control.h.
| #define ADDR_CURRENT_LIMIT (RegTable[enTableId_CurrentLimit].address) | 
Definition at line 126 of file joint_control.h.
| #define ADDR_DRIVE_MODE (RegTable[enTableId_DriveMode].address) | 
Definition at line 112 of file joint_control.h.
| #define ADDR_GOAL_CURRENT (RegTable[enTableId_GoalCurrent].address) | 
Definition at line 141 of file joint_control.h.
| #define ADDR_GOAL_POSITION (RegTable[enTableId_GoalPosition].address) | 
Definition at line 143 of file joint_control.h.
| #define ADDR_HOMING_OFFSET (RegTable[enTableId_HomingOffset].address) | 
Definition at line 116 of file joint_control.h.
| #define ADDR_MAX_VOL_LIMIT (RegTable[enTableId_MaxVolLimit].address) | 
Definition at line 122 of file joint_control.h.
| #define ADDR_MIN_VOL_LIMIT (RegTable[enTableId_MinVolLimit].address) | 
Definition at line 124 of file joint_control.h.
| #define ADDR_MOVING_THRESHOLD (RegTable[enTableId_MovingThreshold].address) | 
Definition at line 118 of file joint_control.h.
| #define ADDR_OPE_MODE (RegTable[enTableId_OpeMode].address) | 
Definition at line 114 of file joint_control.h.
| #define ADDR_POSITION_DGAIN (RegTable[enTableId_PositionDGain].address) | 
Definition at line 133 of file joint_control.h.
| #define ADDR_POSITION_IGAIN (RegTable[enTableId_PositionIGain].address) | 
Definition at line 135 of file joint_control.h.
| #define ADDR_POSITION_PGAIN (RegTable[enTableId_PositionPGain].address) | 
Definition at line 137 of file joint_control.h.
| #define ADDR_PRESENT_CURRENT (RegTable[enTableId_PresentCurrent].address) | 
Definition at line 145 of file joint_control.h.
| #define ADDR_PRESENT_MOVEMENT (ADDR_PRESENT_CURRENT) | 
Definition at line 154 of file joint_control.h.
| #define ADDR_PRESENT_POSITION (RegTable[enTableId_PresentPosition].address) | 
Definition at line 149 of file joint_control.h.
| #define ADDR_PRESENT_TEMP (RegTable[enTableId_PresentTemp].address) | 
Definition at line 151 of file joint_control.h.
| #define ADDR_PRESENT_VEL (RegTable[enTableId_PresentVelocity].address) | 
Definition at line 147 of file joint_control.h.
| #define ADDR_RETURN_DELAY (RegTable[enTableId_ReturnDelay].address) | 
Definition at line 110 of file joint_control.h.
| #define ADDR_TEMPRATURE_LIMIT (RegTable[enTableId_TempLimit].address) | 
Definition at line 120 of file joint_control.h.
| #define ADDR_TORQUE_ENABLE (RegTable[enTableId_TorqueEnable].address) | 
Definition at line 128 of file joint_control.h.
| #define ADDR_VELOCITY_IGAIN (RegTable[enTableId_VelocityIGain].address) | 
Definition at line 129 of file joint_control.h.
| #define ADDR_VELOCITY_PGAIN (RegTable[enTableId_VelocityPGain].address) | 
Definition at line 131 of file joint_control.h.
| #define BAUDRATE (3000000) | 
Definition at line 109 of file joint_control.h.
| #define DEFAULT_MAX_EFFORT (5.0) | 
Definition at line 180 of file joint_control.h.
| #define DXL_CURRENT2EFFORT | ( | c, | |
| coef | |||
| ) | (DXL_CURRENT_UNIT * (c) * (coef) * 0.001) | 
Definition at line 178 of file joint_control.h.
| #define DXL_CURRENT_UNIT (2.69) | 
Definition at line 163 of file joint_control.h.
| #define DXL_DEFAULT_PGAIN (800) | 
Definition at line 167 of file joint_control.h.
| #define DXL_EFFORT_CONST (1.79) | 
Definition at line 164 of file joint_control.h.
| #define DXL_FREE_DGAIN (0) | 
Definition at line 170 of file joint_control.h.
| #define DXL_FREE_IGAIN (0) | 
Definition at line 169 of file joint_control.h.
| #define DXL_FREE_PGAIN (0) | 
Definition at line 168 of file joint_control.h.
| #define DXL_MAX_LIMIT (4095.0) | 
Definition at line 162 of file joint_control.h.
| #define DXL_MIN_LIMIT (0.0) | 
Definition at line 161 of file joint_control.h.
| #define DXL_OFFSET_DEFAULT (2048) | 
Definition at line 172 of file joint_control.h.
| #define DXL_PGAIN_MAX (16383) | 
Definition at line 166 of file joint_control.h.
| #define DXL_TEMP_READ_DURATION (5) | 
Definition at line 165 of file joint_control.h.
| #define DXL_TORQUE_ON_STEP_MAX (DXL_TORQUE_ON_TIME / (1000 / DXL_TORQUR_ON_STEP)) | 
Definition at line 176 of file joint_control.h.
| #define DXL_TORQUE_ON_TIME (5*1000) | 
Definition at line 174 of file joint_control.h.
| #define DXL_TORQUR_ON_STEP (20) | 
Definition at line 175 of file joint_control.h.
| #define DXL_VELOCITY2RAD_S | ( | v | ) | ((v) * 0.229 * 0.1047) | 
Definition at line 182 of file joint_control.h.
| #define DXL_WATCHDOG_RESET_VALUE (0) | 
Definition at line 184 of file joint_control.h.
| #define EFFORT2DXL_CURRENT | ( | e, | |
| coef | |||
| ) | ((e) / (coef) / 0.001 / DXL_CURRENT_UNIT) | 
Definition at line 179 of file joint_control.h.
| #define EFFORT_LIMITING_CNT (10) | 
Definition at line 181 of file joint_control.h.
| #define LEN_BUS_WATCHDOG (RegTable[enTableId_BusWatchdog].length) | 
Definition at line 140 of file joint_control.h.
| #define LEN_CURRENT_LIMIT (RegTable[enTableId_CurrentLimit].length) | 
Definition at line 127 of file joint_control.h.
| #define LEN_DRIVE_MODE (RegTable[enTableId_DriveMode].length) | 
Definition at line 113 of file joint_control.h.
| #define LEN_GOAL_CURRENT (RegTable[enTableId_GoalCurrent].length) | 
Definition at line 142 of file joint_control.h.
| #define LEN_GOAL_POSITION (RegTable[enTableId_GoalPosition].length) | 
Definition at line 144 of file joint_control.h.
| #define LEN_HOMING_OFFSET (RegTable[enTableId_HomingOffset].length) | 
Definition at line 117 of file joint_control.h.
| #define LEN_MAX_VOL_LIMIT (RegTable[enTableId_MaxVolLimit].length) | 
Definition at line 123 of file joint_control.h.
| #define LEN_MIN_VOL_LIMIT (RegTable[enTableId_MinVolLimit].length) | 
Definition at line 125 of file joint_control.h.
| #define LEN_MOVING_THRESHOLD (RegTable[enTableId_MovingThreshold].length) | 
Definition at line 119 of file joint_control.h.
| #define LEN_OPE_MODE (RegTable[enTableId_OpeMode].length) | 
Definition at line 115 of file joint_control.h.
| #define LEN_POSITION_DGAIN (RegTable[enTableId_PositionDGain].length) | 
Definition at line 134 of file joint_control.h.
| #define LEN_POSITION_IGAIN (RegTable[enTableId_PositionIGain].length) | 
Definition at line 136 of file joint_control.h.
| #define LEN_POSITION_PGAIN (RegTable[enTableId_PositionPGain].length) | 
Definition at line 138 of file joint_control.h.
| #define LEN_PRESENT_CURRENT (RegTable[enTableId_PresentCurrent].length) | 
Definition at line 146 of file joint_control.h.
| #define LEN_PRESENT_MOVEMENT (LEN_PRESENT_CURRENT+LEN_PRESENT_VEL+LEN_PRESENT_POSITION) | 
Definition at line 155 of file joint_control.h.
| #define LEN_PRESENT_POSITION (RegTable[enTableId_PresentPosition].length) | 
Definition at line 150 of file joint_control.h.
| #define LEN_PRESENT_TEMP (RegTable[enTableId_PresentTemp].length) | 
Definition at line 152 of file joint_control.h.
| #define LEN_PRESENT_VEL (RegTable[enTableId_PresentVelocity].length) | 
Definition at line 148 of file joint_control.h.
| #define LEN_PRETURN_DELAY (RegTable[enTableId_ReturnDelay].length) | 
Definition at line 111 of file joint_control.h.
| #define LEN_TEMPRATURE_LIMIT (RegTable[enTableId_TempLimit].length) | 
Definition at line 121 of file joint_control.h.
| #define LEN_VELOCITY_IGAIN (RegTable[enTableId_VelocityIGain].length) | 
Definition at line 130 of file joint_control.h.
| #define LEN_VELOCITY_PGAIN (RegTable[enTableId_VelocityPGain].length) | 
Definition at line 132 of file joint_control.h.
| #define MSEC2DXL_WATCHDOG | ( | msec | ) | ((uint8_t)(msec) / 20) | 
Definition at line 185 of file joint_control.h.
| #define OPERATING_MODE_CURR_POS (5) | 
Definition at line 191 of file joint_control.h.
| #define OPERATING_MODE_CURRENT (0) | 
Definition at line 187 of file joint_control.h.
| #define OPERATING_MODE_EXT_POS (4) | 
Definition at line 190 of file joint_control.h.
| #define OPERATING_MODE_POSITION (3) | 
Definition at line 189 of file joint_control.h.
| #define OPERATING_MODE_PWM (16) | 
Definition at line 192 of file joint_control.h.
| #define OPERATING_MODE_VELOCITY (1) | 
Definition at line 188 of file joint_control.h.
| #define POSITION_STEP (4096.0) | 
Definition at line 160 of file joint_control.h.
| #define PROTOCOL_VERSION (2.0) | 
Definition at line 13 of file joint_control.h.
| #define REG_LENGTH_BYTE (1) | 
Definition at line 29 of file joint_control.h.
| #define REG_LENGTH_DWORD (4) | 
Definition at line 31 of file joint_control.h.
| #define REG_LENGTH_WORD (2) | 
Definition at line 30 of file joint_control.h.
| #define TORQUE_DISABLE (0) | 
Definition at line 158 of file joint_control.h.
| #define TORQUE_ENABLE (1) | 
Definition at line 157 of file joint_control.h.
| typedef struct ST_JOINT_PARAM ST_JOINT_PARAM | 
| enum EN_DXL_MEMTYPE | 
| Enumerator | |
|---|---|
| enDXL_ROM | |
| enDXL_RAM | |
Definition at line 16 of file joint_control.h.
| enum EN_TABLE_ID | 
Definition at line 33 of file joint_control.h.
| 
 | static | 
Definition at line 60 of file joint_control.h.