#include <dynamixel_io.h>
Public Member Functions | |
DynamixelIO (std::string device, std::string baud) | |
bool | getAlarmLed (int servo_id, uint8_t &alarm_led) |
bool | getAlarmShutdown (int servo_id, uint8_t &alarm_shutdown) |
bool | getAngleLimits (int servo_id, uint16_t &cw_angle_limit, uint16_t &ccw_angle_limit) |
bool | getBaudRate (int servo_id, uint8_t &baud_rate) |
const DynamixelData * | getCachedParameters (int servo_id) |
bool | getCCWAngleLimit (int servo_id, uint16_t &ccw_angle) |
bool | getCCWComplianceMargin (int servo_id, uint8_t &ccw_compliance_margin) |
bool | getCCWComplianceSlope (int servo_id, uint8_t &ccw_compliance_slope) |
bool | getComplianceMargins (int servo_id, uint8_t &cw_compliance_margin, uint8_t &ccw_compliance_margin) |
bool | getComplianceSlopes (int servo_id, uint8_t &cw_compliance_slope, uint8_t &ccw_compliance_slope) |
bool | getCWAngleLimit (int servo_id, uint16_t &cw_angle) |
bool | getCWComplianceMargin (int servo_id, uint8_t &cw_compliance_margin) |
bool | getCWComplianceSlope (int servo_id, uint8_t &cw_compliance_slope) |
bool | getFeedback (int servo_id, DynamixelStatus &status) |
bool | getFirmwareVersion (int servo_id, uint8_t &firmware_version) |
bool | getLedStatus (int servo_id, bool &led_enabled) |
bool | getLoad (int servo_id, int16_t &load) |
bool | getMaxTorque (int servo_id, uint16_t &max_torque) |
bool | getMaxVoltageLimit (int servo_id, float &max_voltage_limit) |
bool | getMinVoltageLimit (int servo_id, float &min_voltage_limit) |
bool | getModelNumber (int servo_id, uint16_t &model_number) |
bool | getMoving (int servo_id, bool &is_moving) |
bool | getPosition (int servo_id, uint16_t &position) |
bool | getReturnDelayTime (int servo_id, uint8_t &return_delay_time) |
bool | getTargetPosition (int servo_id, uint16_t &target_position) |
bool | getTargetVelocity (int servo_id, int16_t &target_velocity) |
bool | getTemperature (int servo_id, uint8_t &temperature) |
bool | getTemperatureLimit (int servo_id, uint8_t &max_temperature) |
bool | getTorqueEnable (int servo_id, bool &torque_enabled) |
bool | getTorqueLimit (int servo_id, uint16_t &torque_limit) |
bool | getVelocity (int servo_id, int16_t &velocity) |
bool | getVoltage (int servo_id, float &voltage) |
bool | getVoltageLimits (int servo_id, float &min_voltage_limit, float &max_voltage_limit) |
bool | ping (int servo_id) |
bool | resetOverloadError (int servo_id) |
bool | setAlarmLed (int servo_id, uint8_t alarm_led) |
bool | setAlarmShutdown (int servo_id, uint8_t alarm_shutdown) |
bool | setAngleLimits (int servo_id, uint16_t cw_angle, uint16_t ccw_angle) |
bool | setBaudRate (int servo_id, uint8_t baud_rate) |
bool | setCCWAngleLimit (int servo_id, uint16_t ccw_angle) |
bool | setCCWComplianceMargin (int servo_id, uint8_t ccw_margin) |
bool | setCCWComplianceSlope (int servo_id, uint8_t ccw_slope) |
bool | setComplianceMargins (int servo_id, uint8_t cw_margin, uint8_t ccw_margin) |
bool | setComplianceSlopes (int servo_id, uint8_t cw_slope, uint8_t ccw_slope) |
bool | setCWAngleLimit (int servo_id, uint16_t cw_angle) |
bool | setCWComplianceMargin (int servo_id, uint8_t cw_margin) |
bool | setCWComplianceSlope (int servo_id, uint8_t cw_slope) |
bool | setId (int servo_id, uint8_t id) |
bool | setLed (int servo_id, bool on) |
bool | setMaxTorque (int servo_id, uint16_t max_torque) |
bool | setMaxVoltageLimit (int servo_id, float max_voltage_limit) |
bool | setMinVoltageLimit (int servo_id, float min_voltage_limit) |
bool | setMultiComplianceMargins (std::vector< std::vector< int > > value_pairs) |
bool | setMultiComplianceSlopes (std::vector< std::vector< int > > value_pairs) |
bool | setMultiPosition (std::vector< std::vector< int > > value_pairs) |
bool | setMultiPositionVelocity (std::vector< std::vector< int > > value_pairs) |
bool | setMultiTorqueEnabled (std::vector< std::vector< int > > value_pairs) |
bool | setMultiTorqueLimit (std::vector< std::vector< int > > value_pairs) |
bool | setMultiValues (std::vector< std::map< std::string, int > > value_maps) |
bool | setMultiVelocity (std::vector< std::vector< int > > value_pairs) |
bool | setPosition (int servo_id, uint16_t position) |
bool | setReturnDelayTime (int servo_id, uint8_t return_delay_time) |
bool | setTemperatureLimit (int servo_id, uint8_t max_temperature) |
bool | setTorqueEnable (int servo_id, bool on) |
bool | setTorqueLimit (int servo_id, uint16_t torque_limit) |
bool | setVelocity (int servo_id, int16_t velocity) |
bool | setVoltageLimits (int servo_id, float min_voltage_limit, float max_voltage_limit) |
~DynamixelIO () | |
Public Attributes | |
double | last_reset_sec |
long long unsigned int | read_count |
long long unsigned int | read_error_count |
Protected Member Functions | |
void | checkForErrors (int servo_id, uint8_t error_code, std::string command_failed) |
DynamixelData * | findCachedParameters (int servo_id) |
bool | read (int servo_id, int address, int size, std::vector< uint8_t > &response) |
bool | syncWrite (int address, const std::vector< std::vector< uint8_t > > &data) |
bool | updateCachedParameters (int servo_id, DynamixelData *data) |
bool | write (int servo_id, int address, const std::vector< uint8_t > &data, std::vector< uint8_t > &response) |
Protected Attributes | |
std::map< int, DynamixelData * > | cache_ |
std::set< int > | connected_motors_ |
Private Member Functions | |
bool | readResponse (std::vector< uint8_t > &response) |
bool | waitForBytes (ssize_t n_bytes, uint16_t timeout_ms) |
bool | writePacket (const void *const buffer, size_t count) |
Private Attributes | |
flexiport::Port * | port_ |
pthread_mutex_t | serial_mutex_ |
Definition at line 90 of file dynamixel_io.h.
dynamixel_hardware_interface::DynamixelIO::DynamixelIO | ( | std::string | device = "/dev/ttyUSB0" , |
std::string | baud = "1000000" |
||
) |
Definition at line 47 of file src/dynamixel_io.cpp.
Definition at line 69 of file src/dynamixel_io.cpp.
void dynamixel_hardware_interface::DynamixelIO::checkForErrors | ( | int | servo_id, |
uint8_t | error_code, | ||
std::string | command_failed | ||
) | [protected] |
Definition at line 1436 of file src/dynamixel_io.cpp.
DynamixelData* dynamixel_hardware_interface::DynamixelIO::findCachedParameters | ( | int | servo_id | ) | [inline, protected] |
Definition at line 193 of file dynamixel_io.h.
bool dynamixel_hardware_interface::DynamixelIO::getAlarmLed | ( | int | servo_id, |
uint8_t & | alarm_led | ||
) |
Definition at line 309 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::getAlarmShutdown | ( | int | servo_id, |
uint8_t & | alarm_shutdown | ||
) |
Definition at line 323 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::getAngleLimits | ( | int | servo_id, |
uint16_t & | cw_angle_limit, | ||
uint16_t & | ccw_angle_limit | ||
) |
Definition at line 195 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::getBaudRate | ( | int | servo_id, |
uint8_t & | baud_rate | ||
) |
Definition at line 167 of file src/dynamixel_io.cpp.
const DynamixelData * dynamixel_hardware_interface::DynamixelIO::getCachedParameters | ( | int | servo_id | ) |
Definition at line 82 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::getCCWAngleLimit | ( | int | servo_id, |
uint16_t & | ccw_angle | ||
) |
Definition at line 224 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::getCCWComplianceMargin | ( | int | servo_id, |
uint8_t & | ccw_compliance_margin | ||
) |
Definition at line 395 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::getCCWComplianceSlope | ( | int | servo_id, |
uint8_t & | ccw_compliance_slope | ||
) |
Definition at line 438 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::getComplianceMargins | ( | int | servo_id, |
uint8_t & | cw_compliance_margin, | ||
uint8_t & | ccw_compliance_margin | ||
) |
Definition at line 366 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::getComplianceSlopes | ( | int | servo_id, |
uint8_t & | cw_compliance_slope, | ||
uint8_t & | ccw_compliance_slope | ||
) |
Definition at line 409 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::getCWAngleLimit | ( | int | servo_id, |
uint16_t & | cw_angle | ||
) |
Definition at line 210 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::getCWComplianceMargin | ( | int | servo_id, |
uint8_t & | cw_compliance_margin | ||
) |
Definition at line 381 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::getCWComplianceSlope | ( | int | servo_id, |
uint8_t & | cw_compliance_slope | ||
) |
Definition at line 424 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::getFeedback | ( | int | servo_id, |
DynamixelStatus & | status | ||
) |
Definition at line 586 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::getFirmwareVersion | ( | int | servo_id, |
uint8_t & | firmware_version | ||
) |
Definition at line 153 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::getLedStatus | ( | int | servo_id, |
bool & | led_enabled | ||
) |
Definition at line 352 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::getLoad | ( | int | servo_id, |
int16_t & | load | ||
) |
Definition at line 528 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::getMaxTorque | ( | int | servo_id, |
uint16_t & | max_torque | ||
) |
Definition at line 295 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::getMaxVoltageLimit | ( | int | servo_id, |
float & | max_voltage_limit | ||
) |
Definition at line 267 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::getMinVoltageLimit | ( | int | servo_id, |
float & | min_voltage_limit | ||
) |
Definition at line 253 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::getModelNumber | ( | int | servo_id, |
uint16_t & | model_number | ||
) |
Definition at line 139 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::getMoving | ( | int | servo_id, |
bool & | is_moving | ||
) |
Definition at line 572 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::getPosition | ( | int | servo_id, |
uint16_t & | position | ||
) |
Definition at line 498 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::getReturnDelayTime | ( | int | servo_id, |
uint8_t & | return_delay_time | ||
) |
Definition at line 181 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::getTargetPosition | ( | int | servo_id, |
uint16_t & | target_position | ||
) |
Definition at line 453 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::getTargetVelocity | ( | int | servo_id, |
int16_t & | target_velocity | ||
) |
Definition at line 467 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::getTemperature | ( | int | servo_id, |
uint8_t & | temperature | ||
) |
Definition at line 558 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::getTemperatureLimit | ( | int | servo_id, |
uint8_t & | max_temperature | ||
) |
Definition at line 281 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::getTorqueEnable | ( | int | servo_id, |
bool & | torque_enabled | ||
) |
Definition at line 338 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::getTorqueLimit | ( | int | servo_id, |
uint16_t & | torque_limit | ||
) |
Definition at line 483 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::getVelocity | ( | int | servo_id, |
int16_t & | velocity | ||
) |
Definition at line 512 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::getVoltage | ( | int | servo_id, |
float & | voltage | ||
) |
Definition at line 544 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::getVoltageLimits | ( | int | servo_id, |
float & | min_voltage_limit, | ||
float & | max_voltage_limit | ||
) |
Definition at line 238 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::ping | ( | int | servo_id | ) |
Definition at line 89 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::read | ( | int | servo_id, |
int | address, | ||
int | size, | ||
std::vector< uint8_t > & | response | ||
) | [protected] |
Definition at line 1491 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::readResponse | ( | std::vector< uint8_t > & | response | ) | [private] |
Definition at line 1643 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::resetOverloadError | ( | int | servo_id | ) |
Definition at line 121 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::setAlarmLed | ( | int | servo_id, |
uint8_t | alarm_led | ||
) |
Definition at line 859 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::setAlarmShutdown | ( | int | servo_id, |
uint8_t | alarm_shutdown | ||
) |
Definition at line 878 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::setAngleLimits | ( | int | servo_id, |
uint16_t | cw_angle, | ||
uint16_t | ccw_angle | ||
) |
Definition at line 691 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::setBaudRate | ( | int | servo_id, |
uint8_t | baud_rate | ||
) |
Definition at line 653 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::setCCWAngleLimit | ( | int | servo_id, |
uint16_t | ccw_angle | ||
) |
Definition at line 734 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::setCCWComplianceMargin | ( | int | servo_id, |
uint8_t | ccw_margin | ||
) |
Definition at line 975 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::setCCWComplianceSlope | ( | int | servo_id, |
uint8_t | ccw_slope | ||
) |
Definition at line 1034 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::setComplianceMargins | ( | int | servo_id, |
uint8_t | cw_margin, | ||
uint8_t | ccw_margin | ||
) |
Definition at line 935 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::setComplianceSlopes | ( | int | servo_id, |
uint8_t | cw_slope, | ||
uint8_t | ccw_slope | ||
) |
Definition at line 994 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::setCWAngleLimit | ( | int | servo_id, |
uint16_t | cw_angle | ||
) |
Definition at line 714 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::setCWComplianceMargin | ( | int | servo_id, |
uint8_t | cw_margin | ||
) |
Definition at line 956 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::setCWComplianceSlope | ( | int | servo_id, |
uint8_t | cw_slope | ||
) |
Definition at line 1015 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::setId | ( | int | servo_id, |
uint8_t | id | ||
) |
Definition at line 637 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::setLed | ( | int | servo_id, |
bool | on | ||
) |
Definition at line 916 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::setMaxTorque | ( | int | servo_id, |
uint16_t | max_torque | ||
) |
Definition at line 839 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::setMaxVoltageLimit | ( | int | servo_id, |
float | max_voltage_limit | ||
) |
Definition at line 799 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::setMinVoltageLimit | ( | int | servo_id, |
float | min_voltage_limit | ||
) |
Definition at line 778 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::setMultiComplianceMargins | ( | std::vector< std::vector< int > > | value_pairs | ) |
Definition at line 1221 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::setMultiComplianceSlopes | ( | std::vector< std::vector< int > > | value_pairs | ) |
Definition at line 1247 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::setMultiPosition | ( | std::vector< std::vector< int > > | value_pairs | ) |
Definition at line 1123 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::setMultiPositionVelocity | ( | std::vector< std::vector< int > > | value_pairs | ) |
Definition at line 1182 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::setMultiTorqueEnabled | ( | std::vector< std::vector< int > > | value_pairs | ) |
Definition at line 1273 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::setMultiTorqueLimit | ( | std::vector< std::vector< int > > | value_pairs | ) |
Definition at line 1296 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::setMultiValues | ( | std::vector< std::map< std::string, int > > | value_maps | ) |
Definition at line 1317 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::setMultiVelocity | ( | std::vector< std::vector< int > > | value_pairs | ) |
Definition at line 1148 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::setPosition | ( | int | servo_id, |
uint16_t | position | ||
) |
Definition at line 1053 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::setReturnDelayTime | ( | int | servo_id, |
uint8_t | return_delay_time | ||
) |
Definition at line 672 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::setTemperatureLimit | ( | int | servo_id, |
uint8_t | max_temperature | ||
) |
Definition at line 820 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::setTorqueEnable | ( | int | servo_id, |
bool | on | ||
) |
Definition at line 897 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::setTorqueLimit | ( | int | servo_id, |
uint16_t | torque_limit | ||
) |
Definition at line 1105 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::setVelocity | ( | int | servo_id, |
int16_t | velocity | ||
) |
Definition at line 1075 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::setVoltageLimits | ( | int | servo_id, |
float | min_voltage_limit, | ||
float | max_voltage_limit | ||
) |
Definition at line 754 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::syncWrite | ( | int | address, |
const std::vector< std::vector< uint8_t > > & | data | ||
) | [protected] |
Definition at line 1560 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::updateCachedParameters | ( | int | servo_id, |
DynamixelData * | data | ||
) | [protected] |
Definition at line 1395 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::waitForBytes | ( | ssize_t | n_bytes, |
uint16_t | timeout_ms | ||
) | [private] |
Definition at line 1615 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::write | ( | int | servo_id, |
int | address, | ||
const std::vector< uint8_t > & | data, | ||
std::vector< uint8_t > & | response | ||
) | [protected] |
Definition at line 1514 of file src/dynamixel_io.cpp.
bool dynamixel_hardware_interface::DynamixelIO::writePacket | ( | const void *const | buffer, |
size_t | count | ||
) | [private] |
Definition at line 1637 of file src/dynamixel_io.cpp.
std::map<int, DynamixelData*> dynamixel_hardware_interface::DynamixelIO::cache_ [protected] |
Definition at line 190 of file dynamixel_io.h.
std::set<int> dynamixel_hardware_interface::DynamixelIO::connected_motors_ [protected] |
Definition at line 191 of file dynamixel_io.h.
Definition at line 98 of file dynamixel_io.h.
flexiport::Port* dynamixel_hardware_interface::DynamixelIO::port_ [private] |
Definition at line 216 of file dynamixel_io.h.
long long unsigned int dynamixel_hardware_interface::DynamixelIO::read_count |
Definition at line 97 of file dynamixel_io.h.
long long unsigned int dynamixel_hardware_interface::DynamixelIO::read_error_count |
Definition at line 96 of file dynamixel_io.h.
pthread_mutex_t dynamixel_hardware_interface::DynamixelIO::serial_mutex_ [private] |
Definition at line 217 of file dynamixel_io.h.