23#include "servo_chain.h"
25#include <core/exceptions/software.h>
26#include <core/exceptions/system.h>
29#include <utils/math/angle.h>
111const unsigned char DynamixelChain::INST_PING = 0x01;
112const unsigned char DynamixelChain::INST_READ = 0x02;
113const unsigned char DynamixelChain::INST_WRITE = 0x03;
114const unsigned char DynamixelChain::INST_REG_WRITE = 0x04;
115const unsigned char DynamixelChain::INST_ACTION = 0x05;
116const unsigned char DynamixelChain::INST_RESET = 0x06;
117const unsigned char DynamixelChain::INST_DIGITAL_RESET = 0x07;
118const unsigned char DynamixelChain::INST_SYSTEM_READ = 0x0C;
119const unsigned char DynamixelChain::INST_SYSTEM_WRITE = 0x0D;
120const unsigned char DynamixelChain::INST_SYNC_WRITE = 0x83;
121const unsigned char DynamixelChain::INST_SYNC_REG_WRITE = 0x84;
123const unsigned char DynamixelChain::PACKET_OFFSET_ID = 2;
124const unsigned char DynamixelChain::PACKET_OFFSET_LENGTH = 3;
125const unsigned char DynamixelChain::PACKET_OFFSET_INST = 4;
126const unsigned char DynamixelChain::PACKET_OFFSET_PARAM = 5;
127const unsigned char DynamixelChain::PACKET_OFFSET_ERROR = 4;
156 unsigned int default_timeout_ms,
157 bool enable_echo_fix,
158 bool enable_connection_stability,
162 default_timeout_ms_ = default_timeout_ms;
163 device_file_ = strdup(device_file);
167 enable_echo_fix_ = enable_echo_fix;
168 enable_connection_stability_ = enable_connection_stability;
169 min_voltage_ = min_voltage;
170 max_voltage_ = max_voltage;
171 memset(control_table_, 0, DYNAMIXEL_MAX_NUM_SERVOS * DYNAMIXEL_CONTROL_TABLE_LENGTH);
178 for (
size_t i = 0; i <
sizeof(obuffer_) /
sizeof(obuffer_[0]); ++i) {
181 for (
size_t i = 0; i <
sizeof(ibuffer_) /
sizeof(ibuffer_[0]); ++i) {
196 struct termios param;
198 fd_ =
::open(device_file_, O_NOCTTY | O_RDWR);
202 tcflush(fd_, TCIOFLUSH);
204 if (tcgetattr(fd_, ¶m) == -1) {
205 Exception e(errno,
"Getting the port parameters failed");
211 cfsetospeed(¶m, B57600);
212 cfsetispeed(¶m, B57600);
214 param.c_lflag &= ~(ICANON | ECHO | ECHOE | ECHOK | ECHONL | ISIG | IEXTEN);
215 param.c_iflag &= ~(INLCR | IGNCR | ICRNL | IGNBRK | PARMRK);
218 param.c_iflag &= ~(IXON | IXOFF | IXANY);
219 param.c_cflag &= ~CRTSCTS;
221 param.c_cflag |= (CREAD | CLOCAL);
224 param.c_cflag &= ~CS5 & ~CS6 & ~CS7 & ~CS8;
225 param.c_cflag |= CS8;
228 param.c_cflag &= ~(PARENB | PARODD);
229 param.c_iflag &= ~(INPCK | ISTRIP);
232 param.c_cflag &= ~CSTOPB;
235 param.c_oflag &= ~OPOST;
237 param.c_cc[VMIN] = 1;
238 param.c_cc[VTIME] = 0;
240 tcflush(fd_, TCIOFLUSH);
242 if (tcsetattr(fd_, TCSANOW, ¶m) != 0) {
243 Exception e(errno,
"Setting the port parameters failed");
252#ifdef TIMETRACKER_VISCA
254 track_file.open(
"tracker_visca.txt");
255 ttcls_pantilt_get_send = tracker->addClass(
"getPanTilt: send");
256 ttcls_pantilt_get_read = tracker->addClass(
"getPanTilt: read");
257 ttcls_pantilt_get_handle = tracker->addClass(
"getPanTilt: handling responses");
258 ttcls_pantilt_get_interpret = tracker->addClass(
"getPanTilt: interpreting");
282DynamixelChain::calc_checksum(
const unsigned char id,
283 const unsigned char instruction,
284 const unsigned char *params,
285 const unsigned char plength)
287 unsigned int checksum =
id + instruction + plength + 2;
288 for (
unsigned char i = 0; i < plength; ++i) {
289 checksum += params[i];
292 return ~(checksum & 0xFF);
302DynamixelChain::send(
const unsigned char id,
303 const unsigned char instruction,
304 const unsigned char *params,
305 const unsigned char plength)
310 obuffer_[PACKET_OFFSET_ID] = id;
311 obuffer_[PACKET_OFFSET_LENGTH] = plength + 2;
312 obuffer_[PACKET_OFFSET_INST] = instruction;
314 unsigned int checksum =
id + plength + 2;
316 for (
unsigned char i = 0; i < plength; ++i) {
317 obuffer_[PACKET_OFFSET_PARAM + i] = params[i];
318 checksum += params[i];
322 obuffer_[3 + plength + 2] = calc_checksum(
id, instruction, params, plength);
323 obuffer_length_ = plength + 2 + 4;
325#ifdef DEBUG_ServoChain_COMM
327 for (
int i = 0; i < obuffer_length_; ++i) {
328 printf(
"%X ", obuffer_[i]);
333 int written = write(fd_, obuffer_, obuffer_length_);
336 if (enable_echo_fix_) {
339 while (readd < obuffer_length_) {
340 readd += read(fd_, ibuffer_ + readd, obuffer_length_ - readd);
342#ifdef DEBUG_ServoChain_COMM
343 printf(
"Read %d junk bytes: ", readd);
344 for (
int i = 0; i < readd; ++i) {
345 printf(
"%X ", ibuffer_[i]);
351 throw Exception(errno,
"Failed to write ServoChain packet %x for %x", instruction,
id);
352 }
else if (written < obuffer_length_) {
353 throw Exception(
"Failed to write ServoChain packet %x for %x, only %d of %d bytes sent",
366DynamixelChain::recv(
const unsigned char exp_length,
unsigned int timeout_ms)
368 timeval timeout = {0,
369 (suseconds_t)(timeout_ms == 0xFFFFFFFF ? default_timeout_ms_ : timeout_ms)
374 FD_SET(fd_, &read_fds);
377 rv = select(fd_ + 1, &read_fds, NULL, NULL, &timeout);
380 throw Exception(errno,
"Select on FD failed");
381 }
else if (rv == 0) {
383 throw TimeoutException(
"Timeout reached while waiting for incoming ServoChain data");
390 while (bytes_read < 6) {
391#ifdef DEBUG_ServoChain_COMM
392 printf(
"Trying to read %d bytes\n", 6 - bytes_read);
394 if (enable_connection_stability_) {
396 rv = select(fd_ + 1, &read_fds, NULL, NULL, &timeout);
399 throw Exception(errno,
"Select on FD failed");
400 }
else if (rv == 0) {
402 throw TimeoutException(
"Timeout reached while waiting for incoming ServoChain data");
405 bytes_read += read(fd_, ibuffer_ + bytes_read, 6 - bytes_read);
406#ifdef DEBUG_ServoChain_COMM
407 printf(
"%d bytes read ", bytes_read);
408 for (
int i = 0; i < bytes_read; ++i) {
409 printf(
"%X ", ibuffer_[i]);
414 if ((ibuffer_[0] != 0xFF) || (ibuffer_[1] != 0xFF)) {
415 throw Exception(
"Packet does not start with 0xFFFF.");
417 if (exp_length != ibuffer_[PACKET_OFFSET_LENGTH] - 2) {
418 tcflush(fd_, TCIFLUSH);
419 throw Exception(
"Wrong packet length, expected %u, got %u",
421 ibuffer_[PACKET_OFFSET_LENGTH] - 2);
423 const unsigned char plength = ibuffer_[PACKET_OFFSET_LENGTH] - 2;
424#ifdef DEBUG_ServoChain_COMM
425 printf(
"header read, params have length %d\n", plength);
429 while (bytes_read < plength) {
430 if (enable_connection_stability_) {
432 rv = select(fd_ + 1, &read_fds, NULL, NULL, &timeout);
435 throw Exception(errno,
"Select on FD failed");
436 }
else if (rv == 0) {
438 throw TimeoutException(
"Timeout reached while waiting for incoming ServoChain data");
441 bytes_read += read(fd_, &ibuffer_[6] + bytes_read, plength - bytes_read);
443 if (bytes_read < plength) {
444 throw Exception(
"Failed to read packet data");
448 ibuffer_length_ = plength + 2 + 4;
449#ifdef DEBUG_ServoChain_COMM
451 for (
int i = 0; i < ibuffer_length_; ++i) {
452 printf(
"%X ", ibuffer_[i]);
458 unsigned char checksum = calc_checksum(ibuffer_[PACKET_OFFSET_ID],
459 ibuffer_[PACKET_OFFSET_INST],
460 &ibuffer_[PACKET_OFFSET_PARAM],
462 if (checksum != ibuffer_[plength + 5]) {
463 throw Exception(
"Checksum error while receiving packet, expected %d, got %d",
465 ibuffer_[plength + 5]);
468 ibuffer_length_ = plength + 2 + 4;
478 ioctl(fd_, FIONREAD, &num_bytes);
479 return (num_bytes > 0);
507 if (servos.size() == 0) {
512 for (
unsigned int i = 0; i < DYNAMIXEL_MAX_NUM_SERVOS; ++i) {
515 rv.push_back(ibuffer_[PACKET_OFFSET_ID]);
523 for (std::vector<unsigned int>::const_iterator iS = servos.begin(); iS != servos.end(); ++iS)
525 send(*iS, INST_PING, NULL, 0);
527 rv.push_back(ibuffer_[PACKET_OFFSET_ID]);
529 throw Exception(
"Desired servo with ID %u not found!", *iS);
534 for (DeviceList::iterator i = rv.begin(); i != rv.end(); ++i) {
538 e.
append(
"Failed to receive control table for servo %u", *i);
558 send(
id, INST_PING, NULL, 0);
590 unsigned char param[2];
592 param[1] = DYNAMIXEL_CONTROL_TABLE_LENGTH;
594 send(
id, INST_READ, param, 2);
606 recv(DYNAMIXEL_CONTROL_TABLE_LENGTH);
608 if (ibuffer_length_ != 5 + DYNAMIXEL_CONTROL_TABLE_LENGTH + 1) {
609 throw Exception(
"Input buffer of invalid size: %u vs. %u",
611 5 + DYNAMIXEL_CONTROL_TABLE_LENGTH + 1);
613 memcpy(control_table_[ibuffer_[PACKET_OFFSET_ID]],
614 &ibuffer_[PACKET_OFFSET_PARAM],
615 DYNAMIXEL_CONTROL_TABLE_LENGTH);
631 unsigned char param[2];
633 param[1] = read_length;
635 send(
id, INST_READ, param, 2);
638 if (ibuffer_length_ != (5 + read_length + 1)) {
639 throw Exception(
"Input buffer not of expected size, expected %u, got %u",
640 (5 + read_length + 1),
644 for (
unsigned int i = 0; i < read_length; ++i) {
645 control_table_[id][addr + i] = ibuffer_[PACKET_OFFSET_PARAM + i];
662 unsigned char param[3];
664 param[1] = value & 0xFF;
665 param[2] = (value >> 8) & 0xFF;
668 send(
id, INST_WRITE, param, double_byte ? 3 : 2);
671 for (
unsigned int i = 0; i < DYNAMIXEL_MAX_NUM_SERVOS; ++i) {
672 control_table_[i][addr] = param[1];
674 control_table_[i][addr + 1] = param[2];
677 control_table_[id][addr] = param[1];
679 control_table_[id][addr + 1] = param[2];
698 unsigned char start_addr,
699 unsigned char *values,
700 unsigned int num_values)
702 unsigned char param[num_values + 1];
703 param[0] = start_addr;
704 for (
unsigned int i = 0; i < num_values; ++i) {
705 param[i + 1] = values[i];
709 send(
id, INST_WRITE, param, num_values + 1);
712 for (
unsigned int i = 0; i < DYNAMIXEL_MAX_NUM_SERVOS; ++i) {
713 for (
unsigned int j = 0; j < num_values; ++j) {
714 control_table_[i][start_addr + j] = values[j];
718 for (
unsigned int j = 0; j < num_values; ++j) {
719 control_table_[id][start_addr + j] = values[j];
737DynamixelChain::assert_valid_id(
unsigned char id)
740 throw Exception(
"Data can only be queried for a specific servo");
741 }
else if (
id >= DYNAMIXEL_MAX_NUM_SERVOS) {
752DynamixelChain::merge_twobyte_value(
unsigned int id,
unsigned char ind_l,
unsigned char ind_h)
754 unsigned int rv = (control_table_[id][ind_h] & 0xFF) << 8;
755 rv |= control_table_[id][ind_l] & 0xFF;
768DynamixelChain::get_value(
unsigned int id,
bool refresh,
unsigned int ind_l,
unsigned int ind_h)
775 if (ind_h == 0xFFFFFFFF) {
776 return control_table_[id][ind_l];
778 return merge_twobyte_value(
id, ind_l, ind_h);
792 case 12:
return "AX-12";
793 case 18:
return "AX-18";
794 case 24:
return "RX-24F";
795 case 28:
return "RX-28";
796 case 29:
return "MX-28";
797 case 54:
return "MX-64";
798 case 64:
return "RX-64";
799 case 104:
return "MAX-12W";
800 case 107:
return "EX-106+";
801 case 310:
return "MX-64AT";
802 case 320:
return "MX-106";
803 default:
return "UNKNOWN";
837 return get_value(
id, refresh,
P_VERSION);
869 return ibuffer_[PACKET_OFFSET_ERROR];
880 unsigned int &cw_limit,
881 unsigned int &ccw_limit,
967 unsigned int &down_calib,
968 unsigned int &up_calib,
994 return (get_value(
id, refresh,
P_LED) == 1);
1007 unsigned char &cw_margin,
1008 unsigned char &cw_slope,
1009 unsigned char &ccw_margin,
1010 unsigned char &ccw_slope,
1051 if ((voltage < min_voltage_) || (voltage > max_voltage_)) {
1052 throw OutOfBoundsException(
"Voltage is outside of specs", voltage, min_voltage_, max_voltage_);
1058 float range_sec_per_deg = sec_per_deg_12V - sec_per_deg_16V;
1059 float pos = voltage - 12.0;
1061 float sec_per_deg = sec_per_deg_16V + pos * range_sec_per_deg;
1062 float deg_per_sec = 1.0 / sec_per_deg;
1130 return (get_value(
id, refresh,
P_MOVING) == 1);
1141 return (get_value(
id, refresh,
P_LOCK) == 1);
1215 unsigned char param[2];
1283 if (num_servos > 120) {
1285 throw Exception(
"You cannot set more than 120 servos at once");
1289 va_start(arg, num_servos);
1291 unsigned int plength = 2 * num_servos + 2;
1292 unsigned char param[plength];
1295 for (
unsigned int i = 0; i < num_servos; ++i) {
1296 unsigned char id = va_arg(arg,
unsigned int);
1297 param[2 + i * 2] = id;
1298 param[2 + i * 2 + 1] = enabled ? 1 : 0;
1324 unsigned char cw_margin,
1325 unsigned char cw_slope,
1326 unsigned char ccw_margin,
1327 unsigned char ccw_slope)
1329 unsigned char param[4];
1330 param[0] = cw_margin;
1331 param[1] = ccw_margin;
1332 param[2] = cw_slope;
1333 param[3] = ccw_slope;
1356 if (num_servos > 83) {
1358 throw Exception(
"You cannot set more than 83 speeds at once");
1362 va_start(arg, num_servos);
1364 unsigned int plength = 3 * num_servos + 2;
1365 unsigned char param[plength];
1368 for (
unsigned int i = 0; i < num_servos; ++i) {
1369 unsigned char id = va_arg(arg,
unsigned int);
1370 unsigned int value = va_arg(arg,
unsigned int);
1372 param[2 + i * 3] = id;
1373 param[2 + i * 3 + 1] = (value & 0xFF);
1374 param[2 + i * 3 + 2] = (value >> 8) & 0xFF;
1432 if (num_servos > 83) {
1434 throw Exception(
"You cannot set more than 83 servos at once");
1438 va_start(arg, num_servos);
1440 unsigned int plength = 3 * num_servos + 2;
1441 unsigned char param[plength];
1444 for (
unsigned int i = 0; i < num_servos; ++i) {
1445 unsigned char id = va_arg(arg,
unsigned int);
1446 unsigned int value = va_arg(arg,
unsigned int);
1447 param[2 + i * 3] = id;
1448 param[2 + i * 3 + 1] = (value & 0xFF);
1449 param[2 + i * 3 + 2] = (value >> 8) & 0xFF;
void set_punch(unsigned char id, unsigned int punch)
Set punch.
static const unsigned char P_PRESENT_POSITION_H
P_PRESENT_POSITION_H.
~DynamixelChain()
Destructor.
static const unsigned char P_DOWN_CALIBRATION_L
P_DOWN_CALIBRATION_L.
static const unsigned char P_TORQUE_LIMIT_L
P_TORQUE_LIMIT_L.
void set_goal_speed(unsigned char id, unsigned int goal_speed)
Set goal speed.
static const unsigned char P_ID
P_ID.
static const float MAX_ANGLE_RAD
MAX_ANGLE_RAD.
void set_alarm_shutdown(unsigned char id, unsigned char alarm_shutdown)
Set shutdown on alarm.
static const unsigned char P_ALARM_LED
P_ALARM_LED.
static const unsigned char P_PRESENT_LOAD_H
P_PRESENT_LOAD_H.
static const unsigned char P_TORQUE_ENABLE
P_TORQUE_ENABLE.
static const unsigned char P_CCW_ANGLE_LIMIT_H
P_CCW_ANGLE_LIMIT_H.
static const float MAX_ANGLE_DEG
MAX_ANGLE_DEG.
void set_angle_limits(unsigned char id, unsigned int cw_limit, unsigned int ccw_limit)
Set angle limits.
unsigned char get_voltage(unsigned char id, bool refresh=false)
Get current voltage.
static const unsigned char P_CCW_COMPLIANCE_SLOPE
P_CCW_COMPLIANCE_SLOPE.
std::list< unsigned char > DeviceList
List of servo IDs.
static const unsigned char SRL_RESPOND_ALL
SRL_RESPOND_ALL.
static const unsigned char P_GOAL_POSITION_L
P_GOAL_POSITION_L.
static const unsigned char P_PRESENT_LOAD_L
P_PRESENT_LOAD_L.
void set_alarm_led(unsigned char id, unsigned char alarm_led)
Set alarm LED settings.
static const unsigned char SRL_RESPOND_READ
SRL_RESPOND_READ.
void read_table_value(unsigned char id, unsigned char addr, unsigned char read_length)
Read a table value.
void set_status_return_level(unsigned char id, unsigned char status_return_level)
Set status return level.
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.
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.
static const unsigned char P_PRESENT_SPEED_L
P_PRESENT_SPEED_L.
static const unsigned char P_CW_COMPLIANCE_SLOPE
P_CW_COMPLIANCE_SLOPE.
bool is_led_enabled(unsigned char id, bool refresh=false)
Check if LED is enabled.
unsigned int get_model_number(unsigned char id, bool refresh=false)
Get model.
static const unsigned char P_CCW_ANGLE_LIMIT_L
P_CCW_ANGLE_LIMIT_L.
static const unsigned char P_UP_LIMIT_VOLTAGE
P_UP_LIMIT_VOLTAGE.
void set_goal_speeds(unsigned int num_servos,...)
Set goal speeds for multiple servos.
static const float SEC_PER_60DEG_12V
SEC_PER_60DEG_12V.
static const unsigned char P_CW_COMPLIANCE_MARGIN
P_CW_COMPLIANCE_MARGIN.
unsigned int get_goal_speed(unsigned char id, bool refresh=false)
Get goal speed.
DeviceList discover(unsigned int total_timeout_ms=50, const std::vector< unsigned int > &servos=std::vector< unsigned int >())
Discover devices on the bus.
static const unsigned char P_PRESENT_SPEED_H
P_PRESENT_SPEED_H.
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.
void set_torque_limit(unsigned char id, unsigned int torque_limit)
Set torque limit.
unsigned char get_temperature_limit(unsigned char id, bool refresh=false)
Get temperature limit.
void set_return_delay_time(unsigned char id, unsigned char return_delay_time)
Set return delay time.
unsigned int get_max_torque(unsigned char id, bool refresh=false)
Get maximum torque.
static const unsigned char P_UP_CALIBRATION_L
P_UP_CALIBRATION_L.
static const unsigned char P_MODEL_NUMBER_H
P_MODEL_NUMBER_H.
static const unsigned char P_PAUSE_TIME
P_PAUSE_TIME.
static const unsigned char P_LIMIT_TEMPERATURE
P_LIMIT_TEMPERATURE.
void read_table_values(unsigned char id)
Read all table values for given servo.
static const unsigned char P_DOWN_LIMIT_VOLTAGE
P_DOWN_LIMIT_VOLTAGE.
static const float SEC_PER_60DEG_16V
SEC_PER_60DEG_16V.
static const unsigned int CENTER_POSITION
CENTER_POSITION.
static const unsigned char P_REGISTERED_INSTRUCTION
P_REGISTERED_INSTRUCTION.
unsigned int get_position(unsigned char id, bool refresh=false)
Get current position.
void write_table_value(unsigned char id, unsigned char addr, unsigned int value, bool double_byte=false)
Write a table value.
static const unsigned char P_TORQUE_LIMIT_H
P_TORQUE_LIMIT_H.
void set_id(unsigned char id, unsigned char new_id)
Set ID.
void get_calibration(unsigned char id, unsigned int &down_calib, unsigned int &up_calib, bool refresh=false)
Get calibration data.
unsigned char get_baudrate(unsigned char id, bool refresh=false)
Get baud rate.
static const unsigned char P_MODEL_NUMBER_L
P_MODEL_NUMBER_L.
bool ping(unsigned char id, unsigned int timeout_ms=100)
Ping servo.
static const unsigned char P_SYSTEM_DATA2
P_SYSTEM_DATA2.
void get_angle_limits(unsigned char id, unsigned int &cw_limit, unsigned int &ccw_limit, bool refresh=false)
Get angle limits.
static const unsigned char P_RETURN_LEVEL
P_RETURN_LEVEL.
void goto_position(unsigned char id, unsigned int value)
Move servo to specified position.
unsigned int get_punch(unsigned char id, bool refresh=false)
Get punch.
void set_torque_enabled(unsigned char id, bool enabled)
Enable or disable torque.
static const unsigned char SRL_RESPOND_NONE
SRL_RESPOND_NONE.
void lock_config(unsigned char id)
Lock config.
static const unsigned char P_CW_ANGLE_LIMIT_H
P_CW_ANGLE_LIMIT_H.
void finish_read_table_values()
Finish control table receive operations.
bool data_available()
Check data availability.
static const unsigned char P_OPERATING_MODE
P_OPERATING_MODE.
float get_max_supported_speed(unsigned char id, bool refresh=false)
Get maximum supported speed.
void get_voltage_limits(unsigned char id, unsigned char &low, unsigned char &high, bool refresh=false)
Get voltage limits.
void set_max_torque(unsigned char id, unsigned int max_torque)
Set maximum torque.
static const unsigned int MAX_SPEED
MAX_SPEED.
static const unsigned char P_PRESENT_TEMPERATURE
P_PRESENT_TEMPERATURE.
static const unsigned char P_MAX_TORQUE_H
P_MAX_TORQUE_H.
static const unsigned char P_PRESENT_POSITION_L
P_PRESENT_POSITION_L.
static const float POS_TICKS_PER_RAD
POS_TICKS_PER_RAD.
void open()
Open serial port.
void goto_positions(unsigned int num_positions,...)
Move several servos to specified positions.
void set_baudrate(unsigned char id, unsigned char baudrate)
Set baud rate.
static const float RAD_PER_POS_TICK
RAD_PER_POS_TICK.
static const unsigned char P_BAUD_RATE
P_BAUD_RATE.
unsigned char get_alarm_led(unsigned char id, bool refresh=false)
Get alarm LED status.
static const unsigned char P_RETURN_DELAY_TIME
P_RETURN_DELAY_TIME.
unsigned int get_speed(unsigned char id, bool refresh=false)
Get current speed.
static const unsigned char P_CCW_COMPLIANCE_MARGIN
P_CCW_COMPLIANCE_MARGIN.
unsigned int get_goal_position(unsigned char id, bool refresh=false)
Get goal position.
unsigned char get_error(unsigned char id)
Get error flags set by the servo.
static const unsigned int MAX_POSITION
MAX_POSITION.
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 char P_PUNCH_H
P_PUNCH_H.
static const unsigned char P_PUNCH_L
P_PUNCH_L.
void set_torques_enabled(bool enabled, unsigned int num_servos,...)
Enable or disable torque for multiple (selected) servos at once.
bool is_moving(unsigned char id, bool refresh=false)
Check if servo is moving.
void set_led_enabled(unsigned char id, bool enabled)
Turn LED on or off.
unsigned char get_temperature(unsigned char id, bool refresh=false)
Get temperature.
static const unsigned char P_MOVING
P_MOVING.
static const unsigned char BROADCAST_ID
BROADCAST_ID.
void set_voltage_limits(unsigned char id, unsigned char low, unsigned char high)
Set voltage limits.
static const unsigned char P_UP_CALIBRATION_H
P_UP_CALIBRATION_H.
static const unsigned char P_GOAL_SPEED_L
P_GOAL_SPEED_L.
static const unsigned char P_MAX_TORQUE_L
P_MAX_TORQUE_L.
void set_temperature_limit(unsigned char id, unsigned char temp_limit)
Set temperature limit.
bool is_torque_enabled(unsigned char id, bool refresh=false)
Check if torque is enabled.
static const unsigned char P_PRESENT_VOLTAGE
P_PRESENT_VOLTAGE.
const char * get_model(unsigned char id, bool refresh=false)
Get model string.
unsigned char get_delay_time(unsigned char id, bool refresh=false)
Get time of the delay before replies are sent.
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.
static const unsigned char P_GOAL_POSITION_H
P_GOAL_POSITION_H.
static const unsigned char P_LOCK
P_LOCK.
static const unsigned char P_VERSION
P_VERSION.
unsigned int get_torque_limit(unsigned char id, bool refresh=false)
Get torque limit.
static const unsigned char P_DOWN_CALIBRATION_H
P_DOWN_CALIBRATION_H.
static const unsigned char P_ALARM_SHUTDOWN
P_ALARM_SHUTDOWN.
unsigned char get_firmware_version(unsigned char id, bool refresh=false)
Get firmware version.
void start_read_table_values(unsigned char id)
Start to receive table values.
unsigned char get_status_return_level(unsigned char id, bool refresh=false)
Get status return level.
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_CW_ANGLE_LIMIT_L
P_CW_ANGLE_LIMIT_L.
static const unsigned char P_GOAL_SPEED_H
P_GOAL_SPEED_H.
File could not be opened.
Base class for exceptions in Fawkes.
void print_trace() noexcept
Prints trace to stderr.
void append(const char *format,...) noexcept
Append messages to the message list.
The current system call has timed out before completion.
Fawkes library namespace.
float deg2rad(float deg)
Convert an angle given in degrees to radians.