23 #ifndef __PLUGINS_DYNAMIXEL_SERVO_CHAIN_H_ 24 #define __PLUGINS_DYNAMIXEL_SERVO_CHAIN_H_ 26 #include <core/exception.h> 33 #define DYNAMIXEL_CONTROL_TABLE_LENGTH 0x32 34 #define DYNAMIXEL_MAX_NUM_SERVOS 254 42 DynamixelChain(
const char *device_file,
unsigned int default_timeout_ms = 30,
bool enable_echo_fix =
false,
bool enable_connection_stability =
false,
float min_voltage = 12.0,
float max_voltage = 16.0);
48 bool ping(
unsigned char id,
unsigned int timeout_ms = 100);
49 DeviceList
discover(
unsigned int total_timeout_ms = 50,
const std::vector<unsigned int> servos = std::vector<unsigned int>());
53 unsigned int value,
bool double_byte =
false);
55 unsigned char *values,
unsigned int num_values);
58 unsigned char addr,
unsigned char read_length);
65 const char *
get_model(
unsigned char id,
bool refresh =
false);
67 unsigned int get_position(
unsigned char id,
bool refresh =
false);
69 unsigned char get_baudrate(
unsigned char id,
bool refresh =
false);
70 unsigned char get_delay_time(
unsigned char id,
bool refresh =
false);
71 unsigned char get_error(
unsigned char id);
73 unsigned int &cw_limit,
unsigned int &ccw_limit,
74 bool refresh =
false);
77 unsigned char &low,
unsigned char &high,
78 bool refresh =
false);
79 unsigned int get_max_torque(
unsigned char id,
bool refresh =
false);
81 unsigned char get_alarm_led(
unsigned char id,
bool refresh =
false);
84 unsigned int &down_calib,
unsigned int &up_calib,
85 bool refresh =
false);
89 unsigned char &cw_margin,
unsigned char &cw_slope,
90 unsigned char &ccw_margin,
unsigned char &ccw_slope,
91 bool refresh =
false);
93 unsigned int get_goal_speed(
unsigned char id,
bool refresh =
false);
96 unsigned int get_speed(
unsigned char id,
bool refresh =
false);
97 unsigned int get_load(
unsigned char id,
bool refresh =
false);
98 unsigned char get_voltage(
unsigned char id,
bool refresh =
false);
100 bool is_moving(
unsigned char id,
bool refresh =
false);
101 bool is_locked(
unsigned char id,
bool refresh =
false);
102 unsigned int get_punch(
unsigned char id,
bool refresh =
false);
104 void set_id(
unsigned char id,
unsigned char new_id);
105 void set_baudrate(
unsigned char id,
unsigned char baudrate);
108 unsigned int cw_limit,
unsigned int ccw_limit);
113 void set_alarm_led(
unsigned char id,
unsigned char alarm_led);
119 unsigned char cw_margin,
unsigned char cw_slope,
120 unsigned char ccw_margin,
unsigned char ccw_slope);
125 void set_punch(
unsigned char id,
unsigned int punch);
149 static const unsigned char P_ID;
200 static const unsigned char INST_PING;
201 static const unsigned char INST_READ;
202 static const unsigned char INST_WRITE;
203 static const unsigned char INST_REG_WRITE;
204 static const unsigned char INST_ACTION;
205 static const unsigned char INST_RESET;
206 static const unsigned char INST_DIGITAL_RESET;
207 static const unsigned char INST_SYSTEM_READ;
208 static const unsigned char INST_SYSTEM_WRITE;
209 static const unsigned char INST_SYNC_WRITE;
210 static const unsigned char INST_SYNC_REG_WRITE;
213 static const unsigned char PACKET_OFFSET_ID;
214 static const unsigned char PACKET_OFFSET_LENGTH;
215 static const unsigned char PACKET_OFFSET_INST;
216 static const unsigned char PACKET_OFFSET_PARAM;
217 static const unsigned char PACKET_OFFSET_ERROR;
220 unsigned char calc_checksum(
const unsigned char id,
const unsigned char instruction,
221 const unsigned char *params,
const unsigned char plength);
222 void send(
const unsigned char id,
const unsigned char instruction,
223 const unsigned char *params,
const unsigned char plength);
224 void recv(
const unsigned char exp_length,
unsigned int timeout_ms = 0xFFFFFFFF);
225 void assert_valid_id(
unsigned char id);
226 unsigned int merge_twobyte_value(
unsigned int id,
227 unsigned char ind_l,
unsigned char ind_h);
228 unsigned int get_value(
unsigned int id,
bool refresh,
229 unsigned int ind_l,
unsigned int ind_h = 0xFFFFFFFF);
230 bool inline responds_read(
unsigned int id)
233 return ((__control_table[
id][P_RETURN_LEVEL] == SRL_RESPOND_READ) ||
234 (__control_table[
id][P_RETURN_LEVEL] == SRL_RESPOND_ALL));
237 bool inline responds_all(
unsigned int id)
239 return (__control_table[
id][P_RETURN_LEVEL] == SRL_RESPOND_ALL);
244 unsigned int __default_timeout_ms;
245 bool __enable_echo_fix;
246 bool __enable_connection_stability;
250 unsigned char __obuffer[260];
251 unsigned char __ibuffer[260];
253 int __obuffer_length;
254 int __ibuffer_length;
256 char __control_table[DYNAMIXEL_MAX_NUM_SERVOS][DYNAMIXEL_CONTROL_TABLE_LENGTH];
static const unsigned char P_ID
P_ID.
static const unsigned char P_RETURN_LEVEL
P_RETURN_LEVEL.
static const unsigned char SRL_RESPOND_NONE
SRL_RESPOND_NONE.
void get_compliance_values(unsigned char id, unsigned char &cw_margin, unsigned char &cw_slope, unsigned char &ccw_margin, unsigned char &ccw_slope, bool refresh=false)
Get compliance values.
std::list< unsigned char > DeviceList
List of servo IDs.
unsigned int get_speed(unsigned char id, bool refresh=false)
Get current speed.
static const unsigned char P_CCW_COMPLIANCE_SLOPE
P_CCW_COMPLIANCE_SLOPE.
Class to access a chain of Robotis dynamixel servos.
static const unsigned char P_UP_CALIBRATION_L
P_UP_CALIBRATION_L.
unsigned char get_alarm_led(unsigned char id, bool refresh=false)
Get alarm LED status.
static const unsigned char P_MODEL_NUMBER_L
P_MODEL_NUMBER_L.
static const float SEC_PER_60DEG_12V
SEC_PER_60DEG_12V.
unsigned char get_alarm_shutdown(unsigned char id, bool refresh=false)
Get shutdown on alarm state.
bool is_locked(unsigned char id, bool refresh=false)
Check is servo is locked.
void start_read_table_values(unsigned char id)
Start to receive table values.
static const unsigned char P_REGISTERED_INSTRUCTION
P_REGISTERED_INSTRUCTION.
bool is_torque_enabled(unsigned char id, bool refresh=false)
Check if torque is enabled.
static const unsigned char P_MOVING
P_MOVING.
DynamixelChain(const char *device_file, unsigned int default_timeout_ms=30, bool enable_echo_fix=false, bool enable_connection_stability=false, float min_voltage=12.0, float max_voltage=16.0)
Constructor.
static const unsigned char SRL_RESPOND_ALL
SRL_RESPOND_ALL.
static const unsigned char P_PRESENT_POSITION_L
P_PRESENT_POSITION_L.
static const unsigned char BROADCAST_ID
BROADCAST_ID.
float get_max_supported_speed(unsigned char id, bool refresh=false)
Get maximum supported speed.
unsigned char get_firmware_version(unsigned char id, bool refresh=false)
Get firmware version.
void set_alarm_led(unsigned char id, unsigned char alarm_led)
Set alarm LED settings.
void set_temperature_limit(unsigned char id, unsigned char temp_limit)
Set temperature limit.
static const unsigned char P_TORQUE_ENABLE
P_TORQUE_ENABLE.
void set_torques_enabled(bool enabled, unsigned char num_servos,...)
Enable or disable torque for multiple (selected) servos at once.
const char * get_model(unsigned char id, bool refresh=false)
Get model string.
unsigned int get_position(unsigned char id, bool refresh=false)
Get current position.
static const unsigned char P_DOWN_CALIBRATION_H
P_DOWN_CALIBRATION_H.
static const unsigned char P_GOAL_POSITION_L
P_GOAL_POSITION_L.
static const unsigned char P_MODEL_NUMBER_H
P_MODEL_NUMBER_H.
static const unsigned char P_TORQUE_LIMIT_H
P_TORQUE_LIMIT_H.
static const unsigned char P_VERSION
P_VERSION.
unsigned int get_max_torque(unsigned char id, bool refresh=false)
Get maximum torque.
unsigned int get_model_number(unsigned char id, bool refresh=false)
Get model.
unsigned int get_goal_speed(unsigned char id, bool refresh=false)
Get goal speed.
static const unsigned char P_CCW_ANGLE_LIMIT_H
P_CCW_ANGLE_LIMIT_H.
static const unsigned char P_PRESENT_LOAD_H
P_PRESENT_LOAD_H.
unsigned char get_error(unsigned char id)
Get error flags set by the servo.
unsigned char get_voltage(unsigned char id, bool refresh=false)
Get current voltage.
void set_angle_limits(unsigned char id, unsigned int cw_limit, unsigned int ccw_limit)
Set angle limits.
static const unsigned char P_PAUSE_TIME
P_PAUSE_TIME.
static const float RAD_PER_POS_TICK
RAD_PER_POS_TICK.
void goto_positions(unsigned int num_positions,...)
Move several servos to specified positions.
void get_voltage_limits(unsigned char id, unsigned char &low, unsigned char &high, bool refresh=false)
Get voltage limits.
static const unsigned char P_LOCK
P_LOCK.
void get_calibration(unsigned char id, unsigned int &down_calib, unsigned int &up_calib, bool refresh=false)
Get calibration data.
static const float SEC_PER_60DEG_16V
SEC_PER_60DEG_16V.
void get_angle_limits(unsigned char id, unsigned int &cw_limit, unsigned int &ccw_limit, bool refresh=false)
Get angle limits.
unsigned int get_goal_position(unsigned char id, bool refresh=false)
Get goal position.
static const unsigned char P_ALARM_LED
P_ALARM_LED.
void read_table_values(unsigned char id)
Read all table values for given servo.
bool is_moving(unsigned char id, bool refresh=false)
Check if servo is moving.
void goto_position(unsigned char id, unsigned int value)
Move servo to specified position.
static const unsigned char P_CW_COMPLIANCE_MARGIN
P_CW_COMPLIANCE_MARGIN.
static const unsigned char P_DOWN_CALIBRATION_L
P_DOWN_CALIBRATION_L.
static const unsigned char P_GOAL_SPEED_L
P_GOAL_SPEED_L.
static const unsigned char SRL_RESPOND_READ
SRL_RESPOND_READ.
static const unsigned char P_PRESENT_POSITION_H
P_PRESENT_POSITION_H.
void set_id(unsigned char id, unsigned char new_id)
Set ID.
unsigned char get_delay_time(unsigned char id, bool refresh=false)
Get time of the delay before replies are sent.
void open()
Open serial port.
void set_max_torque(unsigned char id, unsigned int max_torque)
Set maximum torque.
static const float POS_TICKS_PER_RAD
POS_TICKS_PER_RAD.
static const unsigned char P_SYSTEM_DATA2
P_SYSTEM_DATA2.
static const unsigned char P_CW_ANGLE_LIMIT_L
P_CW_ANGLE_LIMIT_L.
void set_alarm_shutdown(unsigned char id, unsigned char alarm_shutdown)
Set shutdown on alarm.
static const unsigned char P_GOAL_SPEED_H
P_GOAL_SPEED_H.
static const unsigned char P_PUNCH_H
P_PUNCH_H.
void set_led_enabled(unsigned char id, bool enabled)
Turn LED on or off.
~DynamixelChain()
Destructor.
static const unsigned char P_BAUD_RATE
P_BAUD_RATE.
static const unsigned char P_PRESENT_SPEED_L
P_PRESENT_SPEED_L.
bool ping(unsigned char id, unsigned int timeout_ms=100)
Ping servo.
static const unsigned char P_LED
P_LED.
unsigned int get_load(unsigned char id, bool refresh=false)
Get current load.
static const unsigned char P_CCW_COMPLIANCE_MARGIN
P_CCW_COMPLIANCE_MARGIN.
void set_compliance_values(unsigned char id, unsigned char cw_margin, unsigned char cw_slope, unsigned char ccw_margin, unsigned char ccw_slope)
Set compliance values.
void read_table_value(unsigned char id, unsigned char addr, unsigned char read_length)
Read a table value.
static const unsigned char P_OPERATING_MODE
P_OPERATING_MODE.
static const unsigned char P_UP_LIMIT_VOLTAGE
P_UP_LIMIT_VOLTAGE.
unsigned int get_punch(unsigned char id, bool refresh=false)
Get punch.
static const unsigned char P_DOWN_LIMIT_VOLTAGE
P_DOWN_LIMIT_VOLTAGE.
static const unsigned char P_UP_CALIBRATION_H
P_UP_CALIBRATION_H.
void set_goal_speeds(unsigned int num_servos,...)
Set goal speeds for multiple servos.
static const unsigned char P_MAX_TORQUE_L
P_MAX_TORQUE_L.
void set_status_return_level(unsigned char id, unsigned char status_return_level)
Set status return level.
static const unsigned char P_ALARM_SHUTDOWN
P_ALARM_SHUTDOWN.
static const unsigned char P_CW_ANGLE_LIMIT_H
P_CW_ANGLE_LIMIT_H.
static const unsigned int CENTER_POSITION
CENTER_POSITION.
DeviceList discover(unsigned int total_timeout_ms=50, const std::vector< unsigned int > servos=std::vector< unsigned int >())
Discover devices on the bus.
bool data_available()
Check data availability.
void lock_config(unsigned char id)
Lock config.
static const unsigned char P_CW_COMPLIANCE_SLOPE
P_CW_COMPLIANCE_SLOPE.
static const unsigned char P_LIMIT_TEMPERATURE
P_LIMIT_TEMPERATURE.
void set_baudrate(unsigned char id, unsigned char baudrate)
Set baud rate.
static const unsigned char P_TORQUE_LIMIT_L
P_TORQUE_LIMIT_L.
bool is_led_enabled(unsigned char id, bool refresh=false)
Check if LED is enabled.
unsigned char get_status_return_level(unsigned char id, bool refresh=false)
Get status return level.
void set_return_delay_time(unsigned char id, unsigned char return_delay_time)
Set return delay time.
void set_torque_enabled(unsigned char id, bool enabled)
Enable or disable torque.
static const unsigned char P_CCW_ANGLE_LIMIT_L
P_CCW_ANGLE_LIMIT_L.
static const unsigned char P_PRESENT_VOLTAGE
P_PRESENT_VOLTAGE.
void finish_read_table_values()
Finish control table receive operations.
void set_goal_speed(unsigned char id, unsigned int goal_speed)
Set goal speed.
unsigned char get_temperature_limit(unsigned char id, bool refresh=false)
Get temperature limit.
unsigned int get_torque_limit(unsigned char id, bool refresh=false)
Get torque limit.
static const unsigned char P_GOAL_POSITION_H
P_GOAL_POSITION_H.
void set_voltage_limits(unsigned char id, unsigned char low, unsigned char high)
Set voltage limits.
void set_torque_limit(unsigned char id, unsigned int torque_limit)
Set torque limit.
static const float MAX_ANGLE_DEG
MAX_ANGLE_DEG.
static const unsigned char P_MAX_TORQUE_H
P_MAX_TORQUE_H.
void write_table_values(unsigned char id, unsigned char start_addr, unsigned char *values, unsigned int num_values)
Write multiple table values.
static const unsigned int MAX_SPEED
MAX_SPEED.
static const unsigned char P_PRESENT_TEMPERATURE
P_PRESENT_TEMPERATURE.
static const unsigned int MAX_POSITION
MAX_POSITION.
static const unsigned char P_PRESENT_LOAD_L
P_PRESENT_LOAD_L.
static const unsigned char P_RETURN_DELAY_TIME
P_RETURN_DELAY_TIME.
unsigned char get_baudrate(unsigned char id, bool refresh=false)
Get baud rate.
unsigned char get_temperature(unsigned char id, bool refresh=false)
Get temperature.
static const unsigned char P_PRESENT_SPEED_H
P_PRESENT_SPEED_H.
static const unsigned char P_PUNCH_L
P_PUNCH_L.
void write_table_value(unsigned char id, unsigned char addr, unsigned int value, bool double_byte=false)
Write a table value.
void set_punch(unsigned char id, unsigned int punch)
Set punch.
static const float MAX_ANGLE_RAD
MAX_ANGLE_RAD.