diff --git a/README.md b/README.md index 6c5e6c3b..cc37050d 100644 --- a/README.md +++ b/README.md @@ -32,7 +32,7 @@ If you want your USB2AX serial interface to appear in `/dev` as `usb2axN` (where If the reading time seems too long, check the value of the USB latency timer. On ubuntu this value is retrieved with this command `cat /sys/bus/usb-serial/devices/ttyUSB0/latency_timer`. It is the time, in milliseconds, for which the device driver buffers data before making it available. -You can change this timer with the command `echo 4 | sudo tee /sys/bus/usb-serial/devices/ttyUSB0/latency_timer` which sets it to 4 ms for the device `/dev/ttyUSB0`. +You can change this timer with the command `echo 0 | sudo tee /sys/bus/usb-serial/devices/ttyUSB0/latency_timer` which sets it to 0 ms for the device `/dev/ttyUSB0`. ## Using Libdynamixel on Mac diff --git a/src/demos/operating_mode.cpp b/src/demos/operating_mode.cpp index 7bc79ef5..207d64e9 100644 --- a/src/demos/operating_mode.cpp +++ b/src/demos/operating_mode.cpp @@ -1,9 +1,9 @@ #include +#include #include #include -#include using namespace dynamixel; using namespace servos; @@ -26,6 +26,12 @@ std::string string_operating_mode(Usb2Dynamixel& controller, typename Protocol:: case OperatingMode::multi_turn: str_mode = "multi-turn"; break; + case OperatingMode::multi_turn_torque: + str_mode = "multi-turn-torque"; + break; + case OperatingMode::PWM: + str_mode = "PWM"; + break; default: str_mode = "unknown"; } @@ -87,4 +93,4 @@ int main(int argc, char** argv) } return 0; -} \ No newline at end of file +} diff --git a/src/demos/wscript b/src/demos/wscript index 683d4a2e..cf785b27 100644 --- a/src/demos/wscript +++ b/src/demos/wscript @@ -4,6 +4,5 @@ def build(bld): bld(features='cxx cxxprogram', source='dynamixel_simple_check.cpp', target="dynamixel_simple_check", includes=". ..") - bld(features='cxx cxxprogram', source='dynamixel_test.cpp', target="dynamixel_test", includes=". ..") bld(features='cxx cxxprogram', source='omnipointer_arm.cpp', target="omnipointer_arm", includes=". ..") bld(features='cxx cxxprogram', source='operating_mode.cpp', target="operating_mode", includes=". ..") diff --git a/src/dynamixel/auto_detect.hpp b/src/dynamixel/auto_detect.hpp index 536a4b5a..244932bb 100644 --- a/src/dynamixel/auto_detect.hpp +++ b/src/dynamixel/auto_detect.hpp @@ -1,8 +1,8 @@ #ifndef DYNAMIXEL_AUTO_DETECT_HPP_ #define DYNAMIXEL_AUTO_DETECT_HPP_ -#include #include +#include #include "errors/error.hpp" #include "servos.hpp" @@ -35,6 +35,8 @@ namespace dynamixel { inline std::shared_ptr> get_servo(protocols::Protocol2::id_t id, uint16_t model, protocols::Protocol2::address_t selected_protocol) { switch (model) { + case servos::Xm430W350::ct_t::model_number_value: + return std::make_shared(id); case servos::Xl320::ct_t::model_number_value: return std::make_shared(id); case servos::ProL4210S300::ct_t::model_number_value: diff --git a/src/dynamixel/baudrate.hpp b/src/dynamixel/baudrate.hpp index 18d4c7c0..a83e44e6 100644 --- a/src/dynamixel/baudrate.hpp +++ b/src/dynamixel/baudrate.hpp @@ -1,11 +1,11 @@ #ifndef DYNAMIXEL_BAUDRATE_HPP_ #define DYNAMIXEL_BAUDRATE_HPP_ -#include #include +#include -#include "protocols.hpp" #include "errors/error.hpp" +#include "protocols.hpp" namespace dynamixel { /** The template specifications of this method are used to set the baudrate @@ -45,6 +45,12 @@ namespace dynamixel { return 3; case 1000000: return 1; + case 2250000: + return 250; + case 2500000: + return 251; + case 3000000: + return 252; default: std::stringstream err_message; err_message << "Invalid baudrate for protocol 1: " << baudrate; @@ -80,6 +86,6 @@ namespace dynamixel { throw errors::Error(err_message.str()); } } -} +} // namespace dynamixel #endif diff --git a/src/dynamixel/controllers/usb2dynamixel.hpp b/src/dynamixel/controllers/usb2dynamixel.hpp index 0f724c25..3b6c6bb8 100644 --- a/src/dynamixel/controllers/usb2dynamixel.hpp +++ b/src/dynamixel/controllers/usb2dynamixel.hpp @@ -24,18 +24,20 @@ namespace dynamixel { @param error number, usually the value of errno @return std::string of the explanation **/ - std::string write_error_string(int error_number) + inline std::string write_error_string(int error_number) { switch (error_number) { case EAGAIN: return "EAGAIN: The file descriptor fd refers to a file other than " "a socket and has been marked nonblocking (O_NONBLOCK), and " "the write would block."; - // On some OS, EWOULDBLOCK has the same value as EAGAIN and would hence not compile. - // case EWOULDBLOCK: - // return "EWOULDBLOCK: The file descriptor fd refers to a socket " - // "and has been marked nonblocking (O_NONBLOCK), and the " - // "write would block."; + // On some OS, EWOULDBLOCK has the same value as EAGAIN and would hence not compile. +#ifdef __APPLE__ + case EWOULDBLOCK: + return "EWOULDBLOCK: The file descriptor fd refers to a socket " + "and has been marked nonblocking (O_NONBLOCK), and the " + "write would block."; +#endif case EBADF: return "EBADF: fd is not a valid file descriptor or is not open for writing."; case EDESTADDRREQ: @@ -147,10 +149,10 @@ namespace dynamixel { const int ret = write(_fd, packet.data(), packet.size()); - // std::cout << "Send: "; - // for (size_t i = 0; i < packet.size(); ++i) - // std::cout << "0x" << std::setfill('0') << std::setw(2) << std::hex << (unsigned int)packet[i] << " "; - // std::cout << std::endl; + // std::cout << "Send: "; + // for (size_t i = 0; i < packet.size(); ++i) + // std::cout << "0x" << std::setfill('0') << std::setw(2) << std::hex << (unsigned int)packet[i] << " "; + // std::cout << std::endl; if (ret == -1) { throw errors::Error("Usb2Dynamixel::Send write error " + write_error_string(errno)); @@ -178,23 +180,23 @@ namespace dynamixel { std::vector packet; packet.reserve(_recv_buffer_size); - // std::cout << "Receive:" << std::endl; + // std::cout << "Receive:" << std::endl; do { double current_time = get_time(); uint8_t byte; int res = read(_fd, &byte, 1); if (res > 0) { - // std::cout << std::setfill('0') << std::setw(2) - // << std::hex << (unsigned int)byte << " "; + // std::cout << std::setfill('0') << std::setw(2) + // << std::hex << (unsigned int)byte << " "; packet.push_back(byte); state = status.decode_packet(packet, _report_bad_packet); if (state == DecodeState::INVALID) { - // std::cout << "\tBad packet: "; - // for (const auto byte : packet) - // std::cout << std::setfill('0') << std::setw(2) << std::hex << (unsigned int)byte << " "; - // std::cout << std::endl; + // std::cout << "\tBad packet: "; + // for (const auto byte : packet) + // std::cout << std::setfill('0') << std::setw(2) << std::hex << (unsigned int)byte << " "; + // std::cout << std::endl; packet.clear(); } @@ -206,8 +208,8 @@ namespace dynamixel { return false; } while (state != DecodeState::DONE); - // std::cout << std::endl; - // std::cout << std::dec; + // std::cout << std::endl; + // std::cout << std::dec; return true; } diff --git a/src/dynamixel/errors/bad_packet.hpp b/src/dynamixel/errors/bad_packet.hpp index c45995e2..c17b03b7 100644 --- a/src/dynamixel/errors/bad_packet.hpp +++ b/src/dynamixel/errors/bad_packet.hpp @@ -1,8 +1,8 @@ #ifndef DYNAMIXEL_BAD_PACKET_ERROR_HPP_ #define DYNAMIXEL_BAD_PACKET_ERROR_HPP_ -#include #include +#include #include "error.hpp" @@ -39,7 +39,7 @@ namespace dynamixel { private: const std::vector _packet; }; - } -} + } // namespace errors +} // namespace dynamixel #endif diff --git a/src/dynamixel/errors/crc_error.hpp b/src/dynamixel/errors/crc_error.hpp index 6be126ee..2cc3cb42 100644 --- a/src/dynamixel/errors/crc_error.hpp +++ b/src/dynamixel/errors/crc_error.hpp @@ -1,9 +1,9 @@ #ifndef DYNAMIXEL_ERRORS_CRC_ERROR_HPP_ #define DYNAMIXEL_ERRORS_CRC_ERROR_HPP_ -#include -#include #include +#include +#include #include "error.hpp" @@ -11,7 +11,8 @@ namespace dynamixel { namespace errors { class CrcError : public Error { public: - CrcError(uint8_t id, uint8_t protocol, uint32_t expected, uint32_t received) : _id(id), _protocol(protocol), _expected(expected), _received(received) + CrcError(uint8_t id, uint8_t protocol, uint32_t expected, uint32_t received) + : _id(id), _protocol(protocol), _expected(expected), _received(received) { std::stringstream err_message; err_message << "Status: checksum error while decoding packet with ID " << (int)id; @@ -44,7 +45,7 @@ namespace dynamixel { uint8_t _id, _protocol; uint32_t _expected, _received; }; - } -} + } // namespace errors +} // namespace dynamixel #endif diff --git a/src/dynamixel/errors/error.hpp b/src/dynamixel/errors/error.hpp index 70af8bd1..044ebdbe 100644 --- a/src/dynamixel/errors/error.hpp +++ b/src/dynamixel/errors/error.hpp @@ -47,8 +47,8 @@ namespace dynamixel { { return err.print(os); } - } -} + } // namespace errors +} // namespace dynamixel #define CHECK(val, msg) check(__FILE__, __LINE__, (val), msg) #endif diff --git a/src/dynamixel/errors/servo_limit_error.hpp b/src/dynamixel/errors/servo_limit_error.hpp index 5ae063c1..633e698d 100644 --- a/src/dynamixel/errors/servo_limit_error.hpp +++ b/src/dynamixel/errors/servo_limit_error.hpp @@ -1,9 +1,9 @@ #ifndef DYNAMIXEL_ERRORS_SERVO_LIMIT_ERROR_HPP_ #define DYNAMIXEL_ERRORS_SERVO_LIMIT_ERROR_HPP_ -#include -#include #include +#include +#include #include "error.hpp" @@ -46,7 +46,7 @@ namespace dynamixel { int _id; double _max, _min, _value; }; - } -} + } // namespace errors +} // namespace dynamixel #endif diff --git a/src/dynamixel/errors/status_error.hpp b/src/dynamixel/errors/status_error.hpp index 8fa074d6..83c605cd 100644 --- a/src/dynamixel/errors/status_error.hpp +++ b/src/dynamixel/errors/status_error.hpp @@ -1,8 +1,8 @@ #ifndef DYNAMIXEL_STATUS_ERROR_HPP_ #define DYNAMIXEL_STATUS_ERROR_HPP_ -#include #include +#include #include "error.hpp" @@ -38,7 +38,7 @@ namespace dynamixel { private: uint8_t _id, _protocol, _error_byte; }; - } -} + } // namespace errors +} // namespace dynamixel #endif diff --git a/src/dynamixel/errors/unpack_error.hpp b/src/dynamixel/errors/unpack_error.hpp index 0c0d0df5..f4411765 100644 --- a/src/dynamixel/errors/unpack_error.hpp +++ b/src/dynamixel/errors/unpack_error.hpp @@ -1,9 +1,9 @@ #ifndef DYNAMIXEL_ERRORS_UNPACK_ERROR_HPP_ #define DYNAMIXEL_ERRORS_UNPACK_ERROR_HPP_ -#include -#include #include +#include +#include #include "error.hpp" diff --git a/src/dynamixel/errors/vector_size_errors.hpp b/src/dynamixel/errors/vector_size_errors.hpp new file mode 100644 index 00000000..cd655bc4 --- /dev/null +++ b/src/dynamixel/errors/vector_size_errors.hpp @@ -0,0 +1,55 @@ +#ifndef DYNAMIXEL_VECTOR_SIZE_ERROR_HPP_ +#define DYNAMIXEL_VECTOR_SIZE_ERROR_HPP_ + +#include + +#include "error.hpp" + +namespace dynamixel { + namespace errors { + class VectorEmptyError : public Error { + public: + VectorEmptyError(const std::string& method_name, + const std::string& vector_name) + : _method_name(method_name), _vector_name(vector_name) + { + this->_msg = "The vector " + vector_name + ", passed to " + + method_name + " cannot be empty (size 0)"; + } + + std::string vector_name() const + { + return _vector_name; + } + + std::string method_name() const + { + return _method_name; + } + + protected: + const std::string _method_name, _vector_name; + }; + + class VectorSizesDifferError : public Error { + public: + VectorSizesDifferError(const std::string& method_name, + const std::string& name1, const std::string& name2) + : _method_name(method_name) + { + this->_msg = "The vectors " + name1 + " and " + name2 + + ", passed to " + method_name + " should have the same size."; + } + + std::string method_name() const + { + return _method_name; + } + + protected: + const std::string _method_name; + }; + } // namespace errors +} // namespace dynamixel + +#endif diff --git a/src/dynamixel/instructions/bulk_read.hpp b/src/dynamixel/instructions/bulk_read.hpp index 85a5087f..edd52dce 100644 --- a/src/dynamixel/instructions/bulk_read.hpp +++ b/src/dynamixel/instructions/bulk_read.hpp @@ -4,7 +4,7 @@ #include #include -#include "../errors/error.hpp" +#include "../errors/vector_size_errors.hpp" #include "../instruction_packet.hpp" namespace dynamixel { @@ -12,16 +12,27 @@ namespace dynamixel { template class BulkRead : public InstructionPacket { public: - BulkRead(const std::vector& address, const std::vector& ids, + BulkRead(const std::vector& ids, const std::vector& address, const std::vector& data_length) - : InstructionPacket(T::broadcast_id, T::Instructions::bulk_read, _get_parameters(address, ids, data_length)) {} - // const std::vector& address + : InstructionPacket(T::broadcast_id, T::Instructions::bulk_read, _get_parameters(ids, address, data_length)) {} + + BulkRead(const std::vector& ids, const typename T::address_t& address, + const uint8_t& data_length) + : InstructionPacket(T::broadcast_id, T::Instructions::bulk_read, _get_parameters(ids, address, data_length)) {} + + BulkRead(const std::vector& ids, const std::vector& address, + const std::vector& data_length) + : InstructionPacket(T::broadcast_id, T::Instructions::bulk_read, _get_parameters(ids, address, data_length)) {} + + BulkRead(const std::vector& ids, const typename T::address_t& address, + const uint16_t& data_length) + : InstructionPacket(T::broadcast_id, T::Instructions::bulk_read, _get_parameters(ids, address, data_length)) {} + protected: - std::vector _get_parameters(const std::vector& address, const std::vector& ids, + std::vector _get_parameters(const std::vector& ids, const std::vector& address, const std::vector& data_length) { - if (ids.size() == 0) - throw errors::Error("BulkRead: ids vector of size zero"); + _check_arguments(ids, address, data_length); std::vector parameters(3 * ids.size() + 3); @@ -39,26 +50,78 @@ namespace dynamixel { return parameters; } - std::vector _get_parameters(const std::vector& address, const std::vector& ids, - const std::vector& data_length) + std::vector _get_parameters(const std::vector& ids, const uint8_t& address, + const uint8_t& data_length) { if (ids.size() == 0) - throw errors::Error("BulkRead: ids vector of size zero"); + throw errors::VectorEmptyError("BulkRead", "ids"); std::vector parameters(3 * ids.size() + 3); parameters[0] = 0x00; + + size_t curr = 1; + + for (size_t i = 0; i < ids.size(); i++) { + + parameters[curr++] = data_length; + parameters[curr++] = ids[i]; + parameters[curr++] = address; + } + + return parameters; + } + + std::vector _get_parameters(const std::vector& ids, const std::vector& address, + const std::vector& data_length) + { + _check_arguments(ids, address, data_length); + + std::vector parameters(5 * ids.size() + 5); + size_t curr = 0; + + for (size_t m = 0; m < ids.size(); m++) { + parameters[curr++] = ids[m]; + parameters[curr++] = (uint8_t)(address[m] & 0xFF); + parameters[curr++] = (uint8_t)(address[m] >> 8) & 0xFF; + parameters[curr++] = (uint8_t)(data_length[m] & 0xFF); + parameters[curr++] = (uint8_t)(data_length[m] >> 8) & 0xFF; + } + + return parameters; + } + + std::vector _get_parameters(const std::vector& ids, const uint16_t& address, + const uint16_t& data_length) + { + if (ids.size() == 0) + throw errors::VectorEmptyError("BulkRead", "ids"); + + std::vector parameters(5 * ids.size() + 5); + size_t curr = 0; + for (size_t m = 0; m < ids.size(); m++) { - parameters[m + 1] = (uint8_t)(data_length[m] & 0xFF); - parameters[m + 2] = (uint8_t)(data_length[m] >> 8) & 0xFF; - parameters[m + 3] = (uint8_t)(ids[m] & 0xFF); - parameters[m + 4] = (uint8_t)(ids[m] >> 8) & 0xFF; - parameters[m + 5] = (uint8_t)(address[m] & 0xFF); - parameters[m + 6] = (uint8_t)(address[m] >> 8) & 0xFF; + parameters[curr++] = ids[m]; + parameters[curr++] = (uint8_t)(address & 0xFF); + parameters[curr++] = (uint8_t)(address >> 8) & 0xFF; + parameters[curr++] = (uint8_t)(data_length & 0xFF); + parameters[curr++] = (uint8_t)(data_length >> 8) & 0xFF; } return parameters; } + + template + void _check_arguments(std::vector ids, std::vector addresses, + std::vector lengths) + { + if (ids.size() == 0) + throw errors::VectorEmptyError("BulkRead", "ids"); + if (ids.size() != addresses.size()) + throw errors::VectorSizesDifferError("BulkRead", "ids", "addresses"); + if (ids.size() != lengths.size()) + throw errors::VectorSizesDifferError("BulkRead", "ids", "lengths"); + } }; } // namespace instructions } // namespace dynamixel diff --git a/src/dynamixel/instructions/bulk_write.hpp b/src/dynamixel/instructions/bulk_write.hpp new file mode 100644 index 00000000..052d7f50 --- /dev/null +++ b/src/dynamixel/instructions/bulk_write.hpp @@ -0,0 +1,50 @@ +#ifndef DYNAMIXEL_INSTRUCTIONS_BULK_WRITE_HPP_ +#define DYNAMIXEL_INSTRUCTIONS_BULK_WRITE_HPP_ + +#include +#include + +#include "../errors/vector_size_errors.hpp" +#include "../instruction_packet.hpp" + +namespace dynamixel { + namespace instructions { + template + class BulkWrite : public InstructionPacket { + public: + // Only for protocol 2 + BulkWrite(const std::vector& ids, const typename T::address_t& address, + const uint16_t& data_length, const std::vector>& data) + : InstructionPacket(T::broadcast_id, T::Instructions::bulk_write, _get_parameters(ids, address, data_length, data)) {} + + protected: + std::vector _get_parameters(const std::vector& ids, const uint16_t& address, + const uint16_t& data_length, const std::vector>& data) + { + if (ids.size() == 0) + throw errors::VectorEmptyError("BulkWrite", "ids"); + + std::vector parameters((5 + data_length) * ids.size() + 5); + size_t curr = 0; + + for (size_t m = 0; m < ids.size(); m++) { + parameters[curr++] = ids[m]; + parameters[curr++] = (uint8_t)(address & 0xFF); + parameters[curr++] = (uint8_t)(address >> 8) & 0xFF; + parameters[curr++] = (uint8_t)(data_length & 0xFF); + parameters[curr++] = (uint8_t)(data_length >> 8) & 0xFF; + if (data[m].size() != data_length) + throw errors::Error("BulkWrite: mismatch between declared" + "data length and data vector size"); + for (size_t i = 0; i < data[m].size(); ++i) { + parameters[curr++] = data[m][i]; + } + } + + return parameters; + } + }; + } // namespace instructions +} // namespace dynamixel + +#endif diff --git a/src/dynamixel/instructions/sync_read.hpp b/src/dynamixel/instructions/sync_read.hpp new file mode 100644 index 00000000..e3e450a6 --- /dev/null +++ b/src/dynamixel/instructions/sync_read.hpp @@ -0,0 +1,44 @@ +#ifndef DYNAMIXEL_INSTRUCTIONS_SYNC_READ_HPP_ +#define DYNAMIXEL_INSTRUCTIONS_SYNC_READ_HPP_ + +#include +#include + +#include "../errors/vector_size_errors.hpp" +#include "../instruction_packet.hpp" + +namespace dynamixel { + namespace instructions { + template + class SyncRead : public InstructionPacket { + public: + SyncRead(typename T::address_t address, const std::vector& ids, uint16_t data_length) + : InstructionPacket(T::broadcast_id, T::Instructions::sync_read, _get_parameters(address, ids, data_length)) {} + + protected: + std::vector _get_parameters(uint16_t address, const std::vector& ids, + uint16_t data_length) + { + if (ids.size() == 0) + throw errors::VectorEmptyError("SyncRead", "ids"); + + std::vector parameters(ids.size() + 9); + + parameters[0] = (uint8_t)(address & 0xFF); + parameters[1] = (uint8_t)(address >> 8) & 0xFF; + parameters[2] = (uint8_t)(data_length & 0xFF); + parameters[3] = (uint8_t)(data_length >> 8) & 0xFF; + + size_t curr = 4; + + for (size_t i = 0; i < ids.size(); ++i) { + parameters[curr++] = ids[i]; + } + + return parameters; + } + }; + } // namespace instructions +} // namespace dynamixel + +#endif diff --git a/src/dynamixel/instructions/sync_write.hpp b/src/dynamixel/instructions/sync_write.hpp index 2f7044cd..2ef73e6e 100644 --- a/src/dynamixel/instructions/sync_write.hpp +++ b/src/dynamixel/instructions/sync_write.hpp @@ -1,11 +1,11 @@ #ifndef DYNAMIXEL_INSTRUCTIONS_SYNC_WRITE_HPP_ #define DYNAMIXEL_INSTRUCTIONS_SYNC_WRITE_HPP_ -#include #include +#include +#include "../errors/vector_size_errors.hpp" #include "../instruction_packet.hpp" -#include "../errors/error.hpp" namespace dynamixel { namespace instructions { @@ -21,9 +21,9 @@ namespace dynamixel { const std::vector>& data) { if (ids.size() == 0) - throw errors::Error("SyncWrite: ids vector of size zero"); + throw errors::VectorEmptyError("SyncWrite", "ids"); if (ids.size() != data.size()) - throw errors::Error("SyncWrite: mismatching vectors size for ids and data"); + throw errors::VectorSizesDifferError("SyncWrite", "ids", "data"); typename T::length_t data_length = data[0].size(); std::vector parameters((data_length + 1) * ids.size() + 2); @@ -50,9 +50,9 @@ namespace dynamixel { const std::vector>& data) { if (ids.size() == 0) - throw errors::Error("SyncWrite: ids vector of size zero"); + throw errors::VectorEmptyError("SyncWrite", "ids"); if (ids.size() != data.size()) - throw errors::Error("SyncWrite: mismatching vectors size for ids and data"); + throw errors::VectorSizesDifferError("SyncWrite", "ids", "data"); typename T::length_t data_length = data[0].size(); std::vector parameters((data_length + 1) * ids.size() + 4); @@ -77,7 +77,7 @@ namespace dynamixel { return parameters; } }; - } -} + } // namespace instructions +} // namespace dynamixel #endif diff --git a/src/dynamixel/operating_mode.hpp b/src/dynamixel/operating_mode.hpp index e7b032eb..c944db38 100644 --- a/src/dynamixel/operating_mode.hpp +++ b/src/dynamixel/operating_mode.hpp @@ -85,6 +85,10 @@ namespace dynamixel { return OperatingMode::joint; else if (4 == mode) return OperatingMode::multi_turn; + else if (5 == mode) + return OperatingMode::multi_turn_torque; + else if (16 == mode) + return OperatingMode::PWM; else return OperatingMode::unknown; } @@ -120,6 +124,12 @@ namespace dynamixel { case OperatingMode::multi_turn: return "multi_turn"; break; + case OperatingMode::multi_turn_torque: + return "multi_turn_torque"; + break; + case OperatingMode::PWM: + return "PWM/Voltage"; + break; case OperatingMode::unknown: default: return "unknown"; diff --git a/src/dynamixel/servos.hpp b/src/dynamixel/servos.hpp index 9dd01ae1..fbb3eaca 100644 --- a/src/dynamixel/servos.hpp +++ b/src/dynamixel/servos.hpp @@ -5,24 +5,25 @@ #include "servos/ax12w.hpp" #include "servos/ax18.hpp" #include "servos/ex106.hpp" +#include "servos/mx106.hpp" +#include "servos/mx106_p2.hpp" #include "servos/mx12.hpp" #include "servos/mx28.hpp" #include "servos/mx28_p2.hpp" #include "servos/mx64.hpp" #include "servos/mx64_p2.hpp" -#include "servos/mx106.hpp" -#include "servos/mx106_p2.hpp" -#include "servos/xl320.hpp" -#include "servos/pro_h54_200_s500.hpp" -#include "servos/pro_h54_100_s500.hpp" #include "servos/pro_h42_20_s300.hpp" -#include "servos/pro_m54_60_s250.hpp" -#include "servos/pro_m54_40_s250.hpp" -#include "servos/pro_m42_10_s260.hpp" +#include "servos/pro_h54_100_s500.hpp" +#include "servos/pro_h54_200_s500.hpp" +#include "servos/pro_l42_10_s300.hpp" #include "servos/pro_l54_30_s400.hpp" #include "servos/pro_l54_30_s500.hpp" #include "servos/pro_l54_50_s290.hpp" #include "servos/pro_l54_50_s500.hpp" -#include "servos/pro_l42_10_s300.hpp" +#include "servos/pro_m42_10_s260.hpp" +#include "servos/pro_m54_40_s250.hpp" +#include "servos/pro_m54_60_s250.hpp" +#include "servos/xl320.hpp" +#include "servos/xm430_w350.hpp" #endif diff --git a/src/dynamixel/servos/ax12.hpp b/src/dynamixel/servos/ax12.hpp index 36f77c07..91a9efb0 100644 --- a/src/dynamixel/servos/ax12.hpp +++ b/src/dynamixel/servos/ax12.hpp @@ -3,8 +3,8 @@ #include -#include "servo.hpp" #include "../protocols/protocol1.hpp" +#include "servo.hpp" namespace dynamixel { namespace servos { @@ -116,7 +116,7 @@ namespace dynamixel { READ_WRITE_FIELD(lock); READ_WRITE_FIELD(punch); }; - } -} + } // namespace servos +} // namespace dynamixel #endif diff --git a/src/dynamixel/servos/ax12w.hpp b/src/dynamixel/servos/ax12w.hpp index a2711c65..3664d27c 100644 --- a/src/dynamixel/servos/ax12w.hpp +++ b/src/dynamixel/servos/ax12w.hpp @@ -3,8 +3,8 @@ #include -#include "servo.hpp" #include "../protocols/protocol1.hpp" +#include "servo.hpp" namespace dynamixel { namespace servos { @@ -114,7 +114,7 @@ namespace dynamixel { READ_WRITE_FIELD(lock); READ_WRITE_FIELD(punch); }; - } -} + } // namespace servos +} // namespace dynamixel #endif diff --git a/src/dynamixel/servos/ax18.hpp b/src/dynamixel/servos/ax18.hpp index 3ca5c069..e6c2ffd5 100644 --- a/src/dynamixel/servos/ax18.hpp +++ b/src/dynamixel/servos/ax18.hpp @@ -3,8 +3,8 @@ #include -#include "servo.hpp" #include "../protocols/protocol1.hpp" +#include "servo.hpp" namespace dynamixel { namespace servos { @@ -114,7 +114,7 @@ namespace dynamixel { READ_WRITE_FIELD(lock); READ_WRITE_FIELD(punch); }; - } -} + } // namespace servos +} // namespace dynamixel #endif diff --git a/src/dynamixel/servos/base_servo.hpp b/src/dynamixel/servos/base_servo.hpp index 2fc469b5..4e3ff05b 100644 --- a/src/dynamixel/servos/base_servo.hpp +++ b/src/dynamixel/servos/base_servo.hpp @@ -1,8 +1,8 @@ #ifndef DYNAMIXEL_SERVOS_BASE_SERVO_HPP_ #define DYNAMIXEL_SERVOS_BASE_SERVO_HPP_ -#include "../instruction_packet.hpp" #include "../errors/error.hpp" +#include "../instruction_packet.hpp" #define BASE_FIELD(Name) \ virtual InstructionPacket get_##Name() const \ @@ -31,6 +31,8 @@ namespace dynamixel { wheel, joint, multi_turn, + multi_turn_torque, + PWM, unknown }; namespace servos { @@ -193,6 +195,16 @@ namespace dynamixel { throw errors::Error("parse_present_position_angle not implemented in model"); } + virtual InstructionPacket sync_goal_position_angle(const std::vector& ids, const std::vector& pos) const + { + throw errors::Error("sync_goal_position_angle not implemented in model"); + } + + virtual InstructionPacket bulk_read_position_angle(const std::vector& ids) const + { + throw errors::Error("bulk_read_position_angle not implemented in model"); + } + // ================================================================= // Speed-specific @@ -206,6 +218,16 @@ namespace dynamixel { throw errors::Error("reg_moving_speed_angle not implemented in model"); } + virtual InstructionPacket sync_moving_speed_angle(const std::vector& ids, const std::vector& rad_per_s, OperatingMode operating_mode = OperatingMode::joint) const + { + throw errors::Error("sync_moving_speed_angle not implemented in model"); + } + + virtual InstructionPacket bulk_read_speed_angle(const std::vector& ids) const + { + throw errors::Error("bulk_read_speed_angle not implemented in model"); + } + virtual double parse_joint_speed(const StatusPacket& st) const { throw errors::Error("parse_joint_speed not implemented in model"); diff --git a/src/dynamixel/servos/ex106.hpp b/src/dynamixel/servos/ex106.hpp index 0023b904..7aebbb66 100644 --- a/src/dynamixel/servos/ex106.hpp +++ b/src/dynamixel/servos/ex106.hpp @@ -3,8 +3,8 @@ #include -#include "servo.hpp" #include "../protocols/protocol1.hpp" +#include "servo.hpp" namespace dynamixel { namespace servos { @@ -120,7 +120,7 @@ namespace dynamixel { READ_WRITE_FIELD(punch); READ_FIELD(present_current); }; - } -} + } // namespace servos +} // namespace dynamixel #endif diff --git a/src/dynamixel/servos/mx106.hpp b/src/dynamixel/servos/mx106.hpp index 68340783..d734e922 100644 --- a/src/dynamixel/servos/mx106.hpp +++ b/src/dynamixel/servos/mx106.hpp @@ -3,8 +3,8 @@ #include -#include "servo.hpp" #include "../protocols/protocol1.hpp" +#include "servo.hpp" namespace dynamixel { namespace servos { @@ -132,7 +132,7 @@ namespace dynamixel { READ_WRITE_FIELD(goal_torque); READ_WRITE_FIELD(goal_acceleration); }; - } -} + } // namespace servos +} // namespace dynamixel #endif diff --git a/src/dynamixel/servos/mx106_p2.hpp b/src/dynamixel/servos/mx106_p2.hpp index df52eb53..1b572005 100644 --- a/src/dynamixel/servos/mx106_p2.hpp +++ b/src/dynamixel/servos/mx106_p2.hpp @@ -3,8 +3,8 @@ #include -#include "servo.hpp" #include "../protocols/protocol2.hpp" +#include "servo.hpp" namespace dynamixel { namespace servos { @@ -174,7 +174,7 @@ namespace dynamixel { READ_FIELD(speed_trajectory); READ_FIELD(position_trajectory); }; - } -} + } // namespace servos +} // namespace dynamixel #endif diff --git a/src/dynamixel/servos/mx12.hpp b/src/dynamixel/servos/mx12.hpp index 88491ecc..a99e5c49 100644 --- a/src/dynamixel/servos/mx12.hpp +++ b/src/dynamixel/servos/mx12.hpp @@ -3,8 +3,8 @@ #include -#include "servo.hpp" #include "../protocols/protocol1.hpp" +#include "servo.hpp" namespace dynamixel { namespace servos { @@ -120,7 +120,7 @@ namespace dynamixel { READ_WRITE_FIELD(punch); READ_WRITE_FIELD(goal_acceleration); }; - } -} + } // namespace servos +} // namespace dynamixel #endif diff --git a/src/dynamixel/servos/mx28.hpp b/src/dynamixel/servos/mx28.hpp index 26dffb48..33ab90d7 100644 --- a/src/dynamixel/servos/mx28.hpp +++ b/src/dynamixel/servos/mx28.hpp @@ -3,8 +3,8 @@ #include -#include "servo.hpp" #include "../protocols/protocol1.hpp" +#include "servo.hpp" namespace dynamixel { namespace servos { @@ -120,7 +120,7 @@ namespace dynamixel { READ_WRITE_FIELD(punch); READ_WRITE_FIELD(goal_acceleration); }; - } -} + } // namespace servos +} // namespace dynamixel #endif diff --git a/src/dynamixel/servos/mx28_p2.hpp b/src/dynamixel/servos/mx28_p2.hpp index 5b63e477..841cfdf4 100644 --- a/src/dynamixel/servos/mx28_p2.hpp +++ b/src/dynamixel/servos/mx28_p2.hpp @@ -3,8 +3,8 @@ #include -#include "servo.hpp" #include "../protocols/protocol2.hpp" +#include "servo.hpp" namespace dynamixel { namespace servos { @@ -174,7 +174,7 @@ namespace dynamixel { READ_FIELD(speed_trajectory); READ_FIELD(position_trajectory); }; - } -} + } // namespace servos +} // namespace dynamixel #endif diff --git a/src/dynamixel/servos/mx64.hpp b/src/dynamixel/servos/mx64.hpp index 344278dd..99e8abf5 100644 --- a/src/dynamixel/servos/mx64.hpp +++ b/src/dynamixel/servos/mx64.hpp @@ -3,8 +3,8 @@ #include -#include "servo.hpp" #include "../protocols/protocol1.hpp" +#include "servo.hpp" namespace dynamixel { namespace servos { @@ -129,7 +129,7 @@ namespace dynamixel { READ_WRITE_FIELD(goal_torque); READ_WRITE_FIELD(goal_acceleration); }; - } -} + } // namespace servos +} // namespace dynamixel #endif diff --git a/src/dynamixel/servos/mx64_p2.hpp b/src/dynamixel/servos/mx64_p2.hpp index 559939c3..30788f84 100644 --- a/src/dynamixel/servos/mx64_p2.hpp +++ b/src/dynamixel/servos/mx64_p2.hpp @@ -3,8 +3,8 @@ #include -#include "servo.hpp" #include "../protocols/protocol2.hpp" +#include "servo.hpp" namespace dynamixel { namespace servos { @@ -174,7 +174,7 @@ namespace dynamixel { READ_FIELD(speed_trajectory); READ_FIELD(position_trajectory); }; - } -} + } // namespace servos +} // namespace dynamixel #endif diff --git a/src/dynamixel/servos/pro_h42_20_s300.hpp b/src/dynamixel/servos/pro_h42_20_s300.hpp index d341c67e..7eec5601 100644 --- a/src/dynamixel/servos/pro_h42_20_s300.hpp +++ b/src/dynamixel/servos/pro_h42_20_s300.hpp @@ -3,8 +3,8 @@ #include -#include "servo.hpp" #include "../protocols/protocol2.hpp" +#include "servo.hpp" namespace dynamixel { namespace servos { @@ -132,7 +132,7 @@ namespace dynamixel { READ_FIELD(present_current); READ_FIELD(hardware_error_status); }; - } -} + } // namespace servos +} // namespace dynamixel #endif diff --git a/src/dynamixel/servos/pro_h54_100_s500.hpp b/src/dynamixel/servos/pro_h54_100_s500.hpp index 2168903d..cb1b7262 100644 --- a/src/dynamixel/servos/pro_h54_100_s500.hpp +++ b/src/dynamixel/servos/pro_h54_100_s500.hpp @@ -3,8 +3,8 @@ #include -#include "servo.hpp" #include "../protocols/protocol2.hpp" +#include "servo.hpp" namespace dynamixel { namespace servos { @@ -132,7 +132,7 @@ namespace dynamixel { READ_FIELD(present_current); READ_FIELD(hardware_error_status); }; - } -} + } // namespace servos +} // namespace dynamixel #endif diff --git a/src/dynamixel/servos/pro_h54_200_s500.hpp b/src/dynamixel/servos/pro_h54_200_s500.hpp index 907bb4bb..47c98ca9 100644 --- a/src/dynamixel/servos/pro_h54_200_s500.hpp +++ b/src/dynamixel/servos/pro_h54_200_s500.hpp @@ -3,8 +3,8 @@ #include -#include "servo.hpp" #include "../protocols/protocol2.hpp" +#include "servo.hpp" namespace dynamixel { namespace servos { @@ -132,7 +132,7 @@ namespace dynamixel { READ_FIELD(present_current); READ_FIELD(hardware_error_status); }; - } -} + } // namespace servos +} // namespace dynamixel #endif diff --git a/src/dynamixel/servos/pro_l42_10_s300.hpp b/src/dynamixel/servos/pro_l42_10_s300.hpp index bac68074..fd13ef5f 100644 --- a/src/dynamixel/servos/pro_l42_10_s300.hpp +++ b/src/dynamixel/servos/pro_l42_10_s300.hpp @@ -3,8 +3,8 @@ #include -#include "servo.hpp" #include "../protocols/protocol2.hpp" +#include "servo.hpp" namespace dynamixel { namespace servos { diff --git a/src/dynamixel/servos/pro_l54_30_s400.hpp b/src/dynamixel/servos/pro_l54_30_s400.hpp index 57a1db9a..d710d090 100644 --- a/src/dynamixel/servos/pro_l54_30_s400.hpp +++ b/src/dynamixel/servos/pro_l54_30_s400.hpp @@ -3,8 +3,8 @@ #include -#include "servo.hpp" #include "../protocols/protocol2.hpp" +#include "servo.hpp" namespace dynamixel { namespace servos { @@ -132,7 +132,7 @@ namespace dynamixel { READ_FIELD(present_current); READ_FIELD(hardware_error_status); }; - } -} + } // namespace servos +} // namespace dynamixel #endif diff --git a/src/dynamixel/servos/pro_l54_30_s500.hpp b/src/dynamixel/servos/pro_l54_30_s500.hpp index f7e03be7..8b9ff071 100644 --- a/src/dynamixel/servos/pro_l54_30_s500.hpp +++ b/src/dynamixel/servos/pro_l54_30_s500.hpp @@ -3,8 +3,8 @@ #include -#include "servo.hpp" #include "../protocols/protocol2.hpp" +#include "servo.hpp" namespace dynamixel { namespace servos { @@ -132,7 +132,7 @@ namespace dynamixel { READ_FIELD(present_current); READ_FIELD(hardware_error_status); }; - } -} + } // namespace servos +} // namespace dynamixel #endif diff --git a/src/dynamixel/servos/pro_l54_50_s290.hpp b/src/dynamixel/servos/pro_l54_50_s290.hpp index 7c0fca58..c6c4f64a 100644 --- a/src/dynamixel/servos/pro_l54_50_s290.hpp +++ b/src/dynamixel/servos/pro_l54_50_s290.hpp @@ -3,8 +3,8 @@ #include -#include "servo.hpp" #include "../protocols/protocol2.hpp" +#include "servo.hpp" namespace dynamixel { namespace servos { @@ -132,7 +132,7 @@ namespace dynamixel { READ_FIELD(present_current); READ_FIELD(hardware_error_status); }; - } -} + } // namespace servos +} // namespace dynamixel #endif diff --git a/src/dynamixel/servos/pro_l54_50_s500.hpp b/src/dynamixel/servos/pro_l54_50_s500.hpp index c81b91cb..3892ca9b 100644 --- a/src/dynamixel/servos/pro_l54_50_s500.hpp +++ b/src/dynamixel/servos/pro_l54_50_s500.hpp @@ -3,8 +3,8 @@ #include -#include "servo.hpp" #include "../protocols/protocol2.hpp" +#include "servo.hpp" namespace dynamixel { namespace servos { @@ -132,7 +132,7 @@ namespace dynamixel { READ_FIELD(present_current); READ_FIELD(hardware_error_status); }; - } -} + } // namespace servos +} // namespace dynamixel #endif diff --git a/src/dynamixel/servos/pro_m42_10_s260.hpp b/src/dynamixel/servos/pro_m42_10_s260.hpp index 3d79f76c..fdb83107 100644 --- a/src/dynamixel/servos/pro_m42_10_s260.hpp +++ b/src/dynamixel/servos/pro_m42_10_s260.hpp @@ -3,8 +3,8 @@ #include -#include "servo.hpp" #include "../protocols/protocol2.hpp" +#include "servo.hpp" namespace dynamixel { namespace servos { @@ -132,7 +132,7 @@ namespace dynamixel { READ_FIELD(present_current); READ_FIELD(hardware_error_status); }; - } -} + } // namespace servos +} // namespace dynamixel #endif diff --git a/src/dynamixel/servos/pro_m54_40_s250.hpp b/src/dynamixel/servos/pro_m54_40_s250.hpp index dd973d59..139bb5dc 100644 --- a/src/dynamixel/servos/pro_m54_40_s250.hpp +++ b/src/dynamixel/servos/pro_m54_40_s250.hpp @@ -3,8 +3,8 @@ #include -#include "servo.hpp" #include "../protocols/protocol2.hpp" +#include "servo.hpp" namespace dynamixel { namespace servos { @@ -132,7 +132,7 @@ namespace dynamixel { READ_FIELD(present_current); READ_FIELD(hardware_error_status); }; - } -} + } // namespace servos +} // namespace dynamixel #endif diff --git a/src/dynamixel/servos/pro_m54_60_s250.hpp b/src/dynamixel/servos/pro_m54_60_s250.hpp index 6bf9c88a..ab7b17c7 100644 --- a/src/dynamixel/servos/pro_m54_60_s250.hpp +++ b/src/dynamixel/servos/pro_m54_60_s250.hpp @@ -3,8 +3,8 @@ #include -#include "servo.hpp" #include "../protocols/protocol2.hpp" +#include "servo.hpp" namespace dynamixel { namespace servos { @@ -132,7 +132,7 @@ namespace dynamixel { READ_FIELD(present_current); READ_FIELD(hardware_error_status); }; - } -} + } // namespace servos +} // namespace dynamixel #endif diff --git a/src/dynamixel/servos/servo.hpp b/src/dynamixel/servos/servo.hpp index 0b0aac16..e047bc2f 100644 --- a/src/dynamixel/servos/servo.hpp +++ b/src/dynamixel/servos/servo.hpp @@ -1,25 +1,28 @@ #ifndef DYNAMIXEL_SERVOS_SERVO_HPP_ #define DYNAMIXEL_SERVOS_SERVO_HPP_ -#include -#include - #include "../errors/error.hpp" #include "../errors/servo_limit_error.hpp" +#include "../errors/vector_size_errors.hpp" #include "../instruction_packet.hpp" #include "../instructions/action.hpp" #include "../instructions/bulk_read.hpp" +#include "../instructions/bulk_write.hpp" #include "../instructions/factory_reset.hpp" #include "../instructions/ping.hpp" #include "../instructions/read.hpp" #include "../instructions/reboot.hpp" #include "../instructions/reg_write.hpp" +#include "../instructions/sync_read.hpp" #include "../instructions/sync_write.hpp" #include "../instructions/write.hpp" #include "../status_packet.hpp" #include "base_servo.hpp" #include "model_traits.hpp" #include "protocol_specific_packets.hpp" +#include +#include +#include #define MODEL_NAME(Name) \ std::string model_name() const override \ @@ -99,8 +102,11 @@ namespace dynamixel { typedef instructions::RegWrite reg_write_t; typedef instructions::Action action_t; typedef instructions::FactoryReset factory_reset_t; + typedef instructions::Reboot reboot_t; + typedef instructions::SyncRead sync_read_t; typedef instructions::SyncWrite sync_write_t; typedef instructions::BulkRead bulk_read_t; + typedef instructions::BulkWrite bulk_write_t; long long int id() const override { @@ -144,7 +150,6 @@ namespace dynamixel { // ================================================================= // Position-specific - static inline InstructionPacket set_goal_position_angle(typename Servo::protocol_t::id_t id, double rad) { double deg = rad * 57.2958; @@ -158,6 +163,10 @@ namespace dynamixel { return set_goal_position(id, pos); } + InstructionPacket set_goal_position_angle(double rad) const override + { + return Model::set_goal_position_angle(this->_id, rad); + } static inline InstructionPacket reg_goal_position_angle(typename Servo::protocol_t::id_t id, double rad) { double deg = rad * 57.2958; @@ -172,16 +181,47 @@ namespace dynamixel { return reg_goal_position(id, pos); } - InstructionPacket set_goal_position_angle(double rad) const override + InstructionPacket reg_goal_position_angle(double rad) const override { - return Model::set_goal_position_angle(this->_id, rad); + return Model::reg_goal_position_angle(this->_id, rad); } + // template + static inline InstructionPacket sync_goal_position_angle(typename Servo::protocol_t::id_t id, const std::vector& ids, const std::vector& pos) + { + std::vector final_pos; + for (size_t j = 0; j < pos.size(); j++) + final_pos.push_back((((pos[j] * 57.2958) - ct_t::min_goal_angle_deg) * (ct_t::max_goal_position - ct_t::min_goal_position) / (ct_t::max_goal_angle_deg - ct_t::min_goal_angle_deg)) + ct_t::min_goal_position); + if (ids.size() != final_pos.size()) + throw errors::Error("Instruction: error when setting goal positions: \n\tMismatch in vector size for ids and positions"); + std::vector> packed(final_pos.size()); + for (size_t i = 0; i < final_pos.size(); i++) + packed[i] = protocol_t::pack_data((typename ct_t::goal_position_t)final_pos[i]); - InstructionPacket reg_goal_position_angle(double rad) const override + return sync_write_t(ct_t::goal_position, ids, packed); + } + + InstructionPacket sync_goal_position_angle(const std::vector& ids, const std::vector& pos) const override { - return Model::reg_goal_position_angle(this->_id, rad); + return Model::sync_goal_position_angle(this->_id, ids, pos); + } + + static inline InstructionPacket bulk_read_position_angle(typename Servo::protocol_t::id_t id, const std::vector& ids) + { + std::vector::protocol_t::address_t> address; + std::vector::protocol_t::length_t> data_length; + typename Servo::protocol_t::address_t present_position_address = ct_t::present_position; + typename ct_t::present_position_t type_address; + for (size_t i = 0; i < ids.size(); i++) { + address.push_back(present_position_address); + data_length.push_back(sizeof(type_address)); + } + return bulk_read_t(ids, address, data_length); } + InstructionPacket bulk_read_position_angle(const std::vector& ids) const override + { + return Model::bulk_read_position_angle(this->_id, ids); + } static InstructionPacket::protocol_t> get_present_position_angle(typename Servo::protocol_t::id_t id) { return get_present_position(id); @@ -206,35 +246,6 @@ namespace dynamixel { return Model::parse_present_position_angle(this->_id, st); } - // Sync operations. Only works if the models are known and they are all the same - template - static InstructionPacket set_goal_positions(const std::vector& ids, const std::vector& pos) - { - std::vector final_pos; - for (size_t j = 0; j < pos.size(); j++) - final_pos.push_back(((pos[j] * 57.2958 - ct_t::min_goal_angle_deg) * (ct_t::max_goal_position - ct_t::min_goal_position) / (ct_t::max_goal_angle_deg - ct_t::min_goal_angle_deg)) + ct_t::min_goal_position); - if (ids.size() != final_pos.size()) - throw errors::Error("Instruction: error when setting goal positions: \n\tMismatch in vector size for ids and positions"); - std::vector> packed(final_pos.size()); - for (size_t i = 0; i < final_pos.size(); i++) - packed[i] = protocol_t::pack_data((typename ct_t::goal_position_t)final_pos[i]); - - return sync_write_t(ct_t::goal_position, _get_typed(ids), packed); - } - - // Bulk operations. Only works for MX models with protocol 1. Only works if the models are known and they are all the same - template - static InstructionPacket get_current_positions(const std::vector& ids) - { - std::vector address; - std::vector data_length; - for (size_t i = 0; i < ids.size(); i++) { - address.push_back(0x24); // adress 36 on MX models (0x24) -- adress 37 on XL models (0x25) - data_length.push_back(0x02); - } - return bulk_read_t(address, _get_typed(ids), data_length); - } - // ================================================================= // Speed-specific @@ -259,6 +270,50 @@ namespace dynamixel { return Model::reg_moving_speed_angle(this->_id, rad_per_s, operating_mode); } + static inline InstructionPacket sync_moving_speed_angle(typename Servo::protocol_t::id_t id, const std::vector& ids, const std::vector& rad_per_s, OperatingMode operating_mode) + { + if (ids.size() != rad_per_s.size()) + throw errors::Error("Instruction: error when setting moving speeds: \n\tMismatch in vector size for ids and speeds"); + std::vector speed_ticks; + // convert radians per second to ticks + for (int i = 0; i < rad_per_s.size(); i++) + speed_ticks.push_back(round(60 * rad_per_s[i] / (two_pi * ct_t::rpm_per_tick))); + + for (int j = 0; j < speed_ticks.size(); j++) { + // The actuator is operated as a wheel (continuous rotation) + if (operating_mode == OperatingMode::wheel) { + // Check that desired speed is within the actuator's bounds + + if (!(abs(speed_ticks[j]) >= ct_t::min_goal_speed && abs(speed_ticks[j]) <= ct_t::max_goal_speed)) { + throw errors::Error("Desired speed is out actuator's bounds"); + } + + // Move negatives values in the range [ct_t::min_goal_speed, + // ct_t::2*max_goal_speed+1] + if (speed_ticks[j] < 0) { + speed_ticks[j] = -speed_ticks[j] + ct_t::max_goal_speed + 1; + } + } + // The actuator is operated as a joint (not continuous rotation) + else if (operating_mode == OperatingMode::joint) { + if (!(speed_ticks[j] >= ct_t::min_goal_speed && speed_ticks[j] <= ct_t::max_goal_speed)) { + throw errors::Error("Desired speed is out actuator's bounds"); + } + } + } + + std::vector> packed(speed_ticks.size()); + for (size_t i = 0; i < speed_ticks.size(); i++) + packed[i] = protocol_t::pack_data((typename ct_t::moving_speed_t)speed_ticks[i]); + + return sync_write_t(ct_t::moving_speed, ids, packed); + } + + InstructionPacket sync_moving_speed_angle(const std::vector& ids, const std::vector& rad_per_s, OperatingMode operating_mode = OperatingMode::joint) const override + { + return Model::sync_moving_speed_angle(this->_id, ids, rad_per_s, operating_mode); + } + // TODO: read speed from dynamixel pros to check that we do get negative values too // FIXME : replace the following by protocol specific methods ? static double parse_joint_speed(typename Servo::protocol_t::id_t id, const StatusPacket::protocol_t>& st) @@ -290,7 +345,7 @@ namespace dynamixel { static InstructionPacket set_moving_speeds(const std::vector& ids, const std::vector& speeds, OperatingMode operating_mode) { if (ids.size() != speeds.size()) - throw errors::Error("Instruction: error when setting moving speeds: \n\tMismatch in vector size for ids and speeds"); + throw errors::VectorSizesDifferError("set_moving_speeds", "ids", "speeds"); std::vector speed_ticks; // convert radians per second to ticks for (int i = 0; i < speeds.size(); i++) @@ -326,19 +381,22 @@ namespace dynamixel { return sync_write_t(ct_t::moving_speed, _get_typed(ids), packed); } - // Bulk operations. Only works for MX models with protocol 1. Only works if the models are known and they are all the same - template - static InstructionPacket get_current_speed(const std::vector& ids) + static inline InstructionPacket bulk_read_speed_angle(typename Servo::protocol_t::id_t id, const std::vector& ids) { - //std::vector address; - std::vector address; //uint8_t - - std::vector data_length; //uint8_t + std::vector::protocol_t::address_t> address; + std::vector::protocol_t::length_t> data_length; + typename Servo::protocol_t::address_t present_speed_address = ct_t::present_speed; + typename ct_t::present_speed_t type_address; for (size_t i = 0; i < ids.size(); i++) { - address.push_back(0x26); // 0x27 for XL and 0x26 for MX - data_length.push_back(0x02); + address.push_back(present_speed_address); + data_length.push_back(sizeof(type_address)); } - return bulk_read_t(address, _get_typed(ids), data_length); + return bulk_read_t(ids, address, data_length); + } + + InstructionPacket bulk_read_speed_angle(const std::vector& ids) const override + { + return Model::bulk_read_speed_angle(this->_id, ids); } // ================================================================= @@ -348,7 +406,7 @@ namespace dynamixel { static InstructionPacket set_torque_limits(const std::vector& ids, const std::vector& torque_limits) { if (ids.size() != torque_limits.size()) - throw errors::Error("Instruction: error when setting torque limits: \n\tMismatch in vector size for ids and torques"); + throw errors::VectorSizesDifferError("set_torque_limits", "ids", "torque_limits"); std::vector> packed(torque_limits.size()); for (size_t i = 0; i < torque_limits.size(); i++) packed[i] = protocol_t::pack_data((typename ct_t::torque_limit_t)torque_limits[i]); @@ -372,6 +430,7 @@ namespace dynamixel { } typename protocol_t::id_t _id; + }; // namespace servos } // namespace servos } // namespace dynamixel diff --git a/src/dynamixel/servos/xm430_w350.hpp b/src/dynamixel/servos/xm430_w350.hpp index 6cb25cd8..1cdff2a6 100644 --- a/src/dynamixel/servos/xm430_w350.hpp +++ b/src/dynamixel/servos/xm430_w350.hpp @@ -1,6 +1,11 @@ #ifndef DYNAMIXEL_SERVOS_XM430W350_HPP_ #define DYNAMIXEL_SERVOS_XM430W350_HPP_ +/* TODO : check why realtime_tick, profile_acceleration, profile_velocity didnt work. +READ_WRITE_FIELD(profile_acceleration); +READ_WRITE_FIELD(profile_velocity); +*/ + #include #include "../protocols/protocol2.hpp" @@ -18,8 +23,8 @@ namespace dynamixel { static const protocol_t::address_t model_number = 0; typedef uint16_t model_number_t; static const model_number_t model_number_value = 1020; - static const protocol_t::address_t model_information = 2; - typedef uint32_t model_information_t; + static const protocol_t::address_t model_info = 2; + typedef uint32_t model_info_t; static const protocol_t::address_t firmware_version = 6; typedef uint8_t firmware_version_t; static const protocol_t::address_t id = 7; @@ -40,12 +45,12 @@ namespace dynamixel { typedef uint32_t homing_offset_t; static const protocol_t::address_t moving_threshold = 24; typedef uint32_t moving_threshold_t; - static const protocol_t::address_t temperature_limit = 31; - typedef uint8_t temperature_limit_t; - static const protocol_t::address_t max_voltage_limit = 32; - typedef uint16_t max_voltage_limit_t; - static const protocol_t::address_t min_voltage_limit = 34; - typedef uint16_t min_voltage_limit_t; + static const protocol_t::address_t highest_temperature_limit = 31; // temperature_limit + typedef uint8_t highest_temperature_limit_t; + static const protocol_t::address_t highest_voltage_limit = 32; //max_voltage_limit + typedef uint16_t highest_voltage_limit_t; + static const protocol_t::address_t lowest_voltage_limit = 34; //min_voltage_limit + typedef uint16_t lowest_voltage_limit_t; static const protocol_t::address_t pwm_limit = 36; typedef uint16_t pwm_limit_t; static const protocol_t::address_t current_limit = 38; @@ -56,17 +61,17 @@ namespace dynamixel { typedef uint32_t max_position_limit_t; static const protocol_t::address_t min_position_limit = 52; typedef uint32_t min_position_limit_t; - static const protocol_t::address_t shutdown = 63; - typedef uint8_t shutdown_t; - static const shutdown_t shutdown_value = 52; + static const protocol_t::address_t alarm_shutdown = 63; //shutdown + typedef uint8_t alarm_shutdown_t; + static const alarm_shutdown_t alarm_shutdown_value = 52; static const protocol_t::address_t torque_enable = 64; typedef uint8_t torque_enable_t; static const protocol_t::address_t led = 65; typedef uint8_t led_t; static const protocol_t::address_t status_return_level = 68; typedef uint8_t status_return_level_t; - static const protocol_t::address_t registered_instruction = 69; - typedef uint8_t registered_instruction_t; + static const protocol_t::address_t registered = 69; // registered_instruction + typedef uint8_t registered_t; static const protocol_t::address_t hardware_error_status = 70; typedef uint8_t hardware_error_status_t; static const protocol_t::address_t velocity_i_gain = 76; @@ -87,14 +92,22 @@ namespace dynamixel { typedef uint8_t bus_watchdog_t; static const protocol_t::address_t goal_pwm = 100; typedef uint16_t goal_pwm_t; - static const protocol_t::address_t goal_velocity = 104; - typedef uint32_t goal_velocity_t; + static const protocol_t::address_t moving_speed = 104; // goal_velocity + typedef uint32_t moving_speed_t; + static const moving_speed_t min_goal_speed = 0; + static const moving_speed_t max_goal_speed = 1023; + static constexpr double rpm_per_tick = 0.111; + static const bool speed_sign_bit = true; static const protocol_t::address_t profile_acceleration = 108; typedef uint32_t profil_acceleration_t; static const protocol_t::address_t profile_velocity = 112; typedef uint32_t profil_velocity_t; static const protocol_t::address_t goal_position = 116; typedef uint32_t goal_position_t; + static const goal_position_t min_goal_position = 0; + static const goal_position_t max_goal_position = 4095; + static const uint16_t min_goal_angle_deg = 30; + static const uint16_t max_goal_angle_deg = 330; static const protocol_t::address_t realtime_tick = 120; typedef uint16_t realtime_tick_t; static const protocol_t::address_t moving = 122; @@ -105,16 +118,16 @@ namespace dynamixel { typedef uint16_t present_pwm_t; static const protocol_t::address_t present_load = 126; typedef uint16_t present_load_t; - static const protocol_t::address_t present_velocity = 128; - typedef uint32_t present_veclocity_t; + static const protocol_t::address_t present_speed = 128; // present_velocity + typedef uint32_t present_speed_t; static const protocol_t::address_t present_position = 132; typedef uint32_t present_position_t; static const protocol_t::address_t velocity_trajectory = 136; typedef uint32_t velocity_trajectory_t; static const protocol_t::address_t position_trajectory = 140; typedef uint32_t position_trajectory_t; - static const protocol_t::address_t present_input_voltage = 144; - typedef uint16_t present_input_voltage_t; + static const protocol_t::address_t present_voltage = 144; // present_input_voltage + typedef uint16_t present_voltage_t; static const protocol_t::address_t present_temperature = 146; typedef uint16_t present_temperature_t; }; @@ -132,21 +145,16 @@ namespace dynamixel { READ_WRITE_FIELD(drive_mode); READ_WRITE_FIELD(operating_mode); READ_WRITE_FIELD(secondary_id); - READ_WRITE_FIELD(protocol_version); READ_WRITE_FIELD(homing_offset); READ_WRITE_FIELD(moving_threshold); - READ_WRITE_FIELD(temperature_limit); - READ_WRITE_FIELD(max_voltage_limit); - READ_WRITE_FIELD(min_voltage_limit); + READ_WRITE_FIELD(pwm_limit); - READ_WRITE_FIELD(velocity_limit); + //READ_WRITE_FIELD(velocity_limit); READ_WRITE_FIELD(max_position_limit); READ_WRITE_FIELD(min_position_limit); - READ_WRITE_FIELD(shutdown); + READ_WRITE_FIELD(velocity_i_gain); READ_WRITE_FIELD(velocity_p_gain); - READ_WRITE_FIELD(position_d_gain); - READ_WRITE_FIELD(position_i_gain); READ_WRITE_FIELD(protocol_version); READ_WRITE_FIELD(led); READ_WRITE_FIELD(position_d_gain); @@ -156,21 +164,33 @@ namespace dynamixel { READ_WRITE_FIELD(feedforward_1st_gain); READ_WRITE_FIELD(bus_watchdog); READ_WRITE_FIELD(goal_pwm); - READ_WRITE_FIELD(goal_velocity); - READ_WRITE_FIELD(profile_acceleration); - READ_WRITE_FIELD(profile_velocity); - READ_FIELD(model_information); - READ_FIELD(registered_instruction); + + READ_FIELD(model_info); + READ_FIELD(hardware_error_status); - READ_FIELD(realtime_tick); + READ_FIELD(moving_status); READ_FIELD(present_pwm); READ_FIELD(present_load); - READ_FIELD(present_velocity); - READ_FIELD(velocity_trajectory); + + //READ_FIELD(velocity_trajectory); READ_FIELD(position_trajectory); - READ_FIELD(present_input_voltage); + + READ_FIELD(model_number); + READ_FIELD(firmware_version); + READ_WRITE_FIELD(id); + READ_WRITE_FIELD(baudrate); + READ_WRITE_FIELD(return_delay_time); + READ_WRITE_FIELD(status_return_level); + + READ_WRITE_FIELD(torque_enable); + READ_WRITE_FIELD(goal_position); + READ_FIELD(present_position); READ_FIELD(present_temperature); + READ_FIELD(moving); + READ_FIELD(realtime_tick); + //READ_WRITE_FIELD(profile_acceleration); + // READ_WRITE_FIELD(profile_velocity); }; } // namespace servos } // namespace dynamixel diff --git a/src/tools/command_line_utility.hpp b/src/tools/command_line_utility.hpp index 2aa9dac5..bbdb3fba 100644 --- a/src/tools/command_line_utility.hpp +++ b/src/tools/command_line_utility.hpp @@ -637,7 +637,7 @@ namespace dynamixel { _dyn_util.detect_servos(); std::pair, std::vector> speeds - = _dyn_util.get_speed(); + = _dyn_util.get_speed_bulk(); std::cout << "Goal angular velocities of the actuators (rad/s):" << std::endl; diff --git a/src/tools/utility.hpp b/src/tools/utility.hpp index 64ec0814..b3476132 100644 --- a/src/tools/utility.hpp +++ b/src/tools/utility.hpp @@ -433,9 +433,13 @@ namespace dynamixel { throw errors::UtilityError("set_position(vector, vector): the " "vectors of IDs and angles should have " "the same length"); - _serial_interface.send( - std::make_shared(0)->set_goal_positions(ids, angles)); - + std::vector ids_cast; + for (int i = 0; i < ids.size(); i++) { + ids_cast.push_back((uint8_t)ids[i]); + } + // _serial_interface.send( + // std::make_shared(0)->set_goal_positions(ids, angles)); //Mx28 + _serial_interface.send(_servos.at(ids[0])->sync_goal_position_angle(ids_cast, angles)); StatusPacket status; for (int i = 0; i < ids.size(); i++) { _serial_interface.recv(status); @@ -529,9 +533,17 @@ namespace dynamixel { ids.push_back(servo.first); } - _serial_interface.send( - std::make_shared(0)->get_current_positions(ids)); - + std::vector ids_cast; + for (int i = 0; i < ids.size(); i++) { + ids_cast.push_back((uint8_t)ids[i]); + } + // _servos.at(ids[0])-> + // _serial_interface.send(_servos.at(ids[0])->sync_goal_position_angle(ids_cast, angles)); + // for (auto servo : _servos) { + _serial_interface.send(_servos.at(ids[0])->bulk_read_position_angle(ids_cast)); + // std::make_shared(0)->get_current_positions_XL(ids)); + // break; + // } StatusPacket status; for (auto servo : _servos) { @@ -673,10 +685,13 @@ namespace dynamixel { throw errors::UtilityError("set_speed_sync(vector, vector): the " "vectors of IDs and speeds should have " "the same length"); - - _serial_interface.send( - std::make_shared(0)->set_moving_speeds(ids, speeds, OperatingMode::wheel)); - + std::vector ids_cast; + for (int i = 0; i < ids.size(); i++) { + ids_cast.push_back((uint8_t)ids[i]); + } + // _serial_interface.send( + // std::make_shared(0)->set_moving_speeds(ids, speeds, OperatingMode::wheel)); + _serial_interface.send(_servos.at(ids[0])->sync_moving_speed_angle(ids_cast, speeds, OperatingMode::joint)); StatusPacket status; for (int i = 0; i < ids.size(); i++) { _serial_interface.recv(status); @@ -759,6 +774,46 @@ namespace dynamixel { return std::make_pair(ids, speeds); } + std::pair, std::vector> + get_speed_bulk() const + { + check_scanned(); + + std::vector speeds; + std::vector ids; + + for (auto servo : _servos) { + ids.push_back(servo.first); + } + + std::vector ids_cast; + for (int i = 0; i < ids.size(); i++) { + ids_cast.push_back((uint8_t)ids[i]); + } + + _serial_interface.send(_servos.at(ids[0])->bulk_read_speed_angle(ids_cast)); + + StatusPacket status; + for (auto servo : _servos) { + + _serial_interface.recv(status); + + // parse response to get the speed + if (status.valid()) { + + speeds.push_back( + servo.second->parse_present_position_angle(status)); + } + else { + std::stringstream message; + message << (int)servo.first << " did not answer to the request for " + << "its speed"; + throw errors::Error(message.str()); + } + } + return std::make_pair(ids, speeds); + } + /** Enable (or disable) an actuator. By default, it will enable the actuator, unless one gives the `enable` argument with value false. Here, enabling an acuator means that it