#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>

Include dependency graph for joint_control.h:

This graph shows which files directly or indirectly include this file:

Go to the source code of this file.

Classes
class	JOINT_CONTROL

struct	ST_DYNAMIXEL_REG_TABLE

struct	ST_JOINT_PARAM

Macros
#define	ADDR_BUS_WATCHDOG (RegTable[enTableId_BusWatchdog].address)

#define	ADDR_CURRENT_LIMIT (RegTable[enTableId_CurrentLimit].address)

#define	ADDR_DRIVE_MODE (RegTable[enTableId_DriveMode].address)

#define	ADDR_GOAL_CURRENT (RegTable[enTableId_GoalCurrent].address)

#define	ADDR_GOAL_POSITION (RegTable[enTableId_GoalPosition].address)

#define	ADDR_HOMING_OFFSET (RegTable[enTableId_HomingOffset].address)

#define	ADDR_MAX_VOL_LIMIT (RegTable[enTableId_MaxVolLimit].address)

#define	ADDR_MIN_VOL_LIMIT (RegTable[enTableId_MinVolLimit].address)

#define	ADDR_MOVING_THRESHOLD (RegTable[enTableId_MovingThreshold].address)

#define	ADDR_OPE_MODE (RegTable[enTableId_OpeMode].address)

#define	ADDR_POSITION_DGAIN (RegTable[enTableId_PositionDGain].address)

#define	ADDR_POSITION_IGAIN (RegTable[enTableId_PositionIGain].address)

#define	ADDR_POSITION_PGAIN (RegTable[enTableId_PositionPGain].address)

#define	ADDR_PRESENT_CURRENT (RegTable[enTableId_PresentCurrent].address)

#define	ADDR_PRESENT_MOVEMENT (ADDR_PRESENT_CURRENT)

#define	ADDR_PRESENT_POSITION (RegTable[enTableId_PresentPosition].address)

#define	ADDR_PRESENT_TEMP (RegTable[enTableId_PresentTemp].address)

#define	ADDR_PRESENT_VEL (RegTable[enTableId_PresentVelocity].address)

#define	ADDR_RETURN_DELAY (RegTable[enTableId_ReturnDelay].address)

#define	ADDR_TEMPRATURE_LIMIT (RegTable[enTableId_TempLimit].address)

#define	ADDR_TORQUE_ENABLE (RegTable[enTableId_TorqueEnable].address)

#define	ADDR_VELOCITY_IGAIN (RegTable[enTableId_VelocityIGain].address)

#define	ADDR_VELOCITY_PGAIN (RegTable[enTableId_VelocityPGain].address)

#define	BAUDRATE (3000000)

#define	DEFAULT_MAX_EFFORT (5.0)

#define	DXL_CURRENT2EFFORT(c, coef) (DXL_CURRENT_UNIT * (c) * (coef) * 0.001)

#define	DXL_CURRENT_UNIT (2.69)

#define	DXL_DEFAULT_PGAIN (800)

#define	DXL_EFFORT_CONST (1.79)

#define	DXL_FREE_DGAIN (0)

#define	DXL_FREE_IGAIN (0)

#define	DXL_FREE_PGAIN (0)

#define	DXL_MAX_LIMIT (4095.0)

#define	DXL_MIN_LIMIT (0.0)

#define	DXL_OFFSET_DEFAULT (2048)

#define	DXL_PGAIN_MAX (16383)

#define	DXL_TEMP_READ_DURATION (5)

#define	DXL_TORQUE_ON_STEP_MAX (DXL_TORQUE_ON_TIME / (1000 / DXL_TORQUR_ON_STEP))

#define	DXL_TORQUE_ON_TIME (5*1000)

#define	DXL_TORQUR_ON_STEP (20)

#define	DXL_VELOCITY2RAD_S(v) ((v) * 0.229 * 0.1047)

#define	DXL_WATCHDOG_RESET_VALUE (0)

#define	EFFORT2DXL_CURRENT(e, coef) ((e) / (coef) / 0.001 / DXL_CURRENT_UNIT)

#define	EFFORT_LIMITING_CNT (10)

#define	LEN_BUS_WATCHDOG (RegTable[enTableId_BusWatchdog].length)

#define	LEN_CURRENT_LIMIT (RegTable[enTableId_CurrentLimit].length)

#define	LEN_DRIVE_MODE (RegTable[enTableId_DriveMode].length)

#define	LEN_GOAL_CURRENT (RegTable[enTableId_GoalCurrent].length)

#define	LEN_GOAL_POSITION (RegTable[enTableId_GoalPosition].length)

#define	LEN_HOMING_OFFSET (RegTable[enTableId_HomingOffset].length)

#define	LEN_MAX_VOL_LIMIT (RegTable[enTableId_MaxVolLimit].length)

#define	LEN_MIN_VOL_LIMIT (RegTable[enTableId_MinVolLimit].length)

#define	LEN_MOVING_THRESHOLD (RegTable[enTableId_MovingThreshold].length)

#define	LEN_OPE_MODE (RegTable[enTableId_OpeMode].length)

#define	LEN_POSITION_DGAIN (RegTable[enTableId_PositionDGain].length)

#define	LEN_POSITION_IGAIN (RegTable[enTableId_PositionIGain].length)

#define	LEN_POSITION_PGAIN (RegTable[enTableId_PositionPGain].length)

#define	LEN_PRESENT_CURRENT (RegTable[enTableId_PresentCurrent].length)

#define	LEN_PRESENT_MOVEMENT (LEN_PRESENT_CURRENT+LEN_PRESENT_VEL+LEN_PRESENT_POSITION)

#define	LEN_PRESENT_POSITION (RegTable[enTableId_PresentPosition].length)

#define	LEN_PRESENT_TEMP (RegTable[enTableId_PresentTemp].length)

#define	LEN_PRESENT_VEL (RegTable[enTableId_PresentVelocity].length)

#define	LEN_PRETURN_DELAY (RegTable[enTableId_ReturnDelay].length)

#define	LEN_TEMPRATURE_LIMIT (RegTable[enTableId_TempLimit].length)

#define	LEN_VELOCITY_IGAIN (RegTable[enTableId_VelocityIGain].length)

#define	LEN_VELOCITY_PGAIN (RegTable[enTableId_VelocityPGain].length)

#define	MSEC2DXL_WATCHDOG(msec) ((uint8_t)(msec) / 20)

#define	OPERATING_MODE_CURR_POS (5)

#define	OPERATING_MODE_CURRENT (0)

#define	OPERATING_MODE_EXT_POS (4)

#define	OPERATING_MODE_POSITION (3)

#define	OPERATING_MODE_PWM (16)

#define	OPERATING_MODE_VELOCITY (1)

#define	POSITION_STEP (4096.0)

#define	PROTOCOL_VERSION (2.0)

#define	REG_LENGTH_BYTE (1)

#define	REG_LENGTH_DWORD (4)

#define	REG_LENGTH_WORD (2)

#define	TORQUE_DISABLE (0)

#define	TORQUE_ENABLE (1)

Typedefs
typedef struct ST_JOINT_PARAM	ST_JOINT_PARAM

Variables
static const ST_DYNAMIXEL_REG_TABLE	RegTable []

Macro Definition Documentation

#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 Documentation

typedef struct ST_JOINT_PARAM ST_JOINT_PARAM

Enumeration Type Documentation

enum EN_DXL_MEMTYPE

Enumerator
enDXL_ROM
enDXL_RAM

Definition at line 16 of file joint_control.h.

enum EN_TABLE_ID

Enumerator
enTableId_ReturnDelay
enTableId_DriveMode
enTableId_OpeMode
enTableId_HomingOffset
enTableId_MovingThreshold
enTableId_TempLimit
enTableId_MaxVolLimit
enTableId_MinVolLimit
enTableId_CurrentLimit
enTableId_Shutdown
enTableId_TorqueEnable
enTableId_VelocityIGain
enTableId_VelocityPGain
enTableId_PositionDGain
enTableId_PositionIGain
enTableId_PositionPGain
enTableId_BusWatchdog
enTableId_GoalCurrent
enTableId_GoalVelocity
enTableId_GoalPosition
enTableId_PresentCurrent
enTableId_PresentVelocity
enTableId_PresentPosition
enTableId_PresentTemp

Definition at line 33 of file joint_control.h.

Variable Documentation

const ST_DYNAMIXEL_REG_TABLE RegTable[]

static

Initial value:

={
    
    { "RETURN_DELAY_TIME",  9,      REG_LENGTH_BYTE,  250,    enDXL_ROM,  false },
    { "DRIVE_MODE",         10,     REG_LENGTH_BYTE,  0,      enDXL_ROM,  false },
    { "OPERATION_MODE",     11,     REG_LENGTH_BYTE,  3,      enDXL_ROM,  false },
    { "HOMING_OFFSET",      20,     REG_LENGTH_DWORD, 0,      enDXL_ROM,  false },
    { "MOVING_THRESHOLD",   24,     REG_LENGTH_DWORD, 10,     enDXL_ROM,  false },
    { "TEMPRATURE_LIMIT",   31,     REG_LENGTH_BYTE,  80,     enDXL_ROM,  false },
    { "MAX_VOL_LIMIT",      32,     REG_LENGTH_WORD,  160,    enDXL_ROM,  false },
    { "MIN_VOL_LIMIT",      34,     REG_LENGTH_WORD,  95,     enDXL_ROM,  false },
    { "CURRENT_LIMIT",      38,     REG_LENGTH_WORD,  1193,   enDXL_ROM,  false },
    { "SHUTDOWN",           63,     REG_LENGTH_BYTE,  52,     enDXL_ROM,  false },
    { "TORQUE_ENABLE",      64,     REG_LENGTH_BYTE,  0,      enDXL_RAM,  false },
    { "VELOCITY_I_GAIN",    76,     REG_LENGTH_WORD,  1920,   enDXL_RAM,  true  },
    { "VELOCITY_P_GAIN",    78,     REG_LENGTH_WORD,  100,    enDXL_RAM,  true  },
    { "POSITION_D_GAIN",    80,     REG_LENGTH_WORD,  0,      enDXL_RAM,  true  },
    { "POSITION_I_GAIN",    82,     REG_LENGTH_WORD,  0,      enDXL_RAM,  true  },
    { "POSITION_P_GAIN",    84,     REG_LENGTH_WORD,  800,    enDXL_RAM,  false },
    { "BUS_WATCHDOG",       98,     REG_LENGTH_BYTE,  0,      enDXL_RAM,  false },
    { "GOAL_CURRENT",       102,    REG_LENGTH_WORD,  0,      enDXL_RAM,  false },
    { "GOAL_VELOCITY",      104,    REG_LENGTH_DWORD, 0,      enDXL_RAM,  false },
    { "GOAL_POSITION",      116,    REG_LENGTH_DWORD, 0,      enDXL_RAM,  false },
    { "PRESENT_CURRENT",    126,    REG_LENGTH_WORD,  0,      enDXL_RAM,  false },
    { "PRESENT_VELOCITY",   128,    REG_LENGTH_DWORD, 0,      enDXL_RAM,  false },
    { "PRESENT_POSITION",   132,    REG_LENGTH_DWORD, 0,      enDXL_RAM,  false },
    { "PRESENT_TEMPRATURE", 146,    REG_LENGTH_BYTE,  0,      enDXL_RAM,  false },
}

Definition at line 60 of file joint_control.h.

Enumerations
enum	EN_DXL_MEMTYPE { enDXL_ROM, enDXL_RAM }

enum	EN_TABLE_ID { enTableId_ReturnDelay = 0, enTableId_DriveMode, enTableId_OpeMode, enTableId_HomingOffset, enTableId_MovingThreshold, enTableId_TempLimit, enTableId_MaxVolLimit, enTableId_MinVolLimit, enTableId_CurrentLimit, enTableId_Shutdown, enTableId_TorqueEnable, enTableId_VelocityIGain, enTableId_VelocityPGain, enTableId_PositionDGain, enTableId_PositionIGain, enTableId_PositionPGain, enTableId_BusWatchdog, enTableId_GoalCurrent, enTableId_GoalVelocity, enTableId_GoalPosition, enTableId_PresentCurrent, enTableId_PresentVelocity, enTableId_PresentPosition, enTableId_PresentTemp }

Classes

Macros

Typedefs

Enumerations

Variables

Macro Definition Documentation

Typedef Documentation

Enumeration Type Documentation

Variable Documentation