[WIP] Sync and bulk operations & new actuators

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
 

src/demos/operating_mode.cpp

@@ -1,9 +1,9 @@
 
 #include <dynamixel/dynamixel.hpp>
 
+#include <iostream>
 #include <string.h>
 #include <string>
-#include <iostream>
 
 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;
-}
+}

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=". ..")

src/dynamixel/auto_detect.hpp

@@ -1,8 +1,8 @@
 #ifndef DYNAMIXEL_AUTO_DETECT_HPP_
 #define DYNAMIXEL_AUTO_DETECT_HPP_
 
-#include <memory>
 #include <map>
+#include <memory>
 
 #include "errors/error.hpp"
 #include "servos.hpp"
@@ -35,6 +35,8 @@ namespace dynamixel {
     inline std::shared_ptr<servos::BaseServo<protocols::Protocol2>> 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<servos::Xm430W350>(id);
         case servos::Xl320::ct_t::model_number_value:
             return std::make_shared<servos::Xl320>(id);
         case servos::ProL4210S300::ct_t::model_number_value:

src/dynamixel/baudrate.hpp

@@ -1,11 +1,11 @@
 #ifndef DYNAMIXEL_BAUDRATE_HPP_
 #define DYNAMIXEL_BAUDRATE_HPP_
 
-#include <stdint.h>
 #include <memory>
+#include <stdint.h>
 
-#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

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<uint8_t> 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;
             }

src/dynamixel/errors/bad_packet.hpp

@@ -1,8 +1,8 @@
 #ifndef DYNAMIXEL_BAD_PACKET_ERROR_HPP_
 #define DYNAMIXEL_BAD_PACKET_ERROR_HPP_
 
-#include <string>
 #include <stdint.h>
+#include <string>
 
 #include "error.hpp"
 
@@ -39,7 +39,7 @@ namespace dynamixel {
         private:
             const std::vector<uint8_t> _packet;
         };
-    }
-}
+    } // namespace errors
+} // namespace dynamixel
 
 #endif

src/dynamixel/errors/crc_error.hpp

@@ -1,17 +1,18 @@
 #ifndef DYNAMIXEL_ERRORS_CRC_ERROR_HPP_
 #define DYNAMIXEL_ERRORS_CRC_ERROR_HPP_
 
-#include <string>
-#include <stdint.h>
 #include <sstream>
+#include <stdint.h>
+#include <string>
 
 #include "error.hpp"
 
 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

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

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 <string>
-#include <stdint.h>
 #include <sstream>
+#include <stdint.h>
+#include <string>
 
 #include "error.hpp"
 
@@ -46,7 +46,7 @@ namespace dynamixel {
             int _id;
             double _max, _min, _value;
         };
-    }
-}
+    } // namespace errors
+} // namespace dynamixel
 
 #endif

src/dynamixel/errors/status_error.hpp

@@ -1,8 +1,8 @@
 #ifndef DYNAMIXEL_STATUS_ERROR_HPP_
 #define DYNAMIXEL_STATUS_ERROR_HPP_
 
-#include <string>
 #include <stdint.h>
+#include <string>
 
 #include "error.hpp"
 
@@ -38,7 +38,7 @@ namespace dynamixel {
         private:
             uint8_t _id, _protocol, _error_byte;
         };
-    }
-}
+    } // namespace errors
+} // namespace dynamixel
 
 #endif

src/dynamixel/errors/unpack_error.hpp

@@ -1,9 +1,9 @@
 #ifndef DYNAMIXEL_ERRORS_UNPACK_ERROR_HPP_
 #define DYNAMIXEL_ERRORS_UNPACK_ERROR_HPP_
 
-#include <string>
-#include <stdint.h>
 #include <sstream>
+#include <stdint.h>
+#include <string>
 
 #include "error.hpp"
 

src/dynamixel/errors/vector_size_errors.hpp

@@ -0,0 +1,55 @@
+#ifndef DYNAMIXEL_VECTOR_SIZE_ERROR_HPP_
+#define DYNAMIXEL_VECTOR_SIZE_ERROR_HPP_
+
+#include <string>
+
+#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