Support for features: multiturn mode, get_load, get_moving; Baudrate bug on USB2AX

dynamixel_driver/scripts/info_dump.py

@@ -118,6 +118,7 @@ def print_data(values):
                 angles = dxl_io.get_angle_limits(motor_id)
                 model = dxl_io.get_model_number(motor_id)
                 firmware = dxl_io.get_firmware_version(motor_id)
+                print "Model: ", model
                 values['model'] = '%s (firmware version: %d)' % (DXL_MODEL_TO_PARAMS[model]['name'], firmware)
                 values['degree_symbol'] = u"\u00B0"
                 values['min'] = angles['min']

dynamixel_driver/src/dynamixel_driver/dynamixel_const.py

@@ -69,6 +69,10 @@
 DXL_DOWN_CALIBRATION_H = 21
 DXL_UP_CALIBRATION_L = 22
 DXL_UP_CALIBRATION_H = 23
+DXL_MULTI_TURN_OFFSET_L = 20  # MX-series
+DXL_MULTI_TURN_OFFSET_H = 21  # MX-series
+DXL_RESOLUTION_DIVIDER = 22  # MX-series
+# RAM:
 DXL_TORQUE_ENABLE = 24
 DXL_LED = 25
 DXL_CW_COMPLIANCE_MARGIN = 26

dynamixel_driver/src/dynamixel_driver/dynamixel_io.py

@@ -59,14 +59,12 @@ class DynamixelIO(object):
     multi-servo instruction packet.
     """
 
-    def __init__(self, port, baudrate, readback_echo=False):
+    def __init__(self, port, baudrate, readback_echo=False, timeout=0.015):
         """ Constructor takes serial port and baudrate as arguments. """
         try:
             self.serial_mutex = Lock()
             self.ser = None
-            self.ser = serial.Serial(port)
-            self.ser.setTimeout(0.015)
-            self.ser.baudrate = baudrate
+            self.ser = serial.Serial(port, baudrate=baudrate, timeout=timeout)
             self.port_name = port
             self.readback_echo = readback_echo
         except SerialOpenError:
@@ -328,6 +326,18 @@ def set_angle_limits(self, servo_id, min_angle, max_angle):
             self.exception_on_error(response[4], servo_id, 'setting CW and CCW angle limits to %d and %d' %(min_angle, max_angle))
         return response
 
+    def set_mode_wheel(self, servo_id):
+        """
+        Set wheel mode. Use set_angle_limits for joint mode.
+        """
+        return self.set_angle_limits(servo_id, 0, 0)
+
+    def set_mode_multiturn(self, servo_id):
+        """
+        Set multi-turn mode. Use set_angle_limits for joint mode.
+        """
+        return self.set_angle_limits(servo_id, 0x0fff, 0x0fff)
+
     def set_drive_mode(self, servo_id, is_slave=False, is_reverse=False):
         """
         Sets the drive mode for EX-106 motors
@@ -339,6 +349,31 @@ def set_drive_mode(self, servo_id, is_slave=False, is_reverse=False):
             self.exception_on_error(response[4], servo_id, 'setting drive mode to %d' % drive_mode)
         return response
 
+    def set_resolution_divider(self, servo_id, divider):
+        """
+        Set resolution divider. Valid range: 1 to 4
+        """
+        response = self.write(servo_id, DXL_RESOLUTION_DIVIDER, [divider])
+        if response:
+            self.exception_on_error(response[4], servo_id, 'setting resolution divider to %d' % divider)
+        return response
+
+
+
+    def set_multiturn_offset(self, servo_id, offset):
+        """
+        Set the multiturn offset
+        """
+
+        offset &= 0xffff
+
+        response = self.write(servo_id, DXL_MULTI_TURN_OFFSET_L, (int(offset % 256), int(offset >> 8)))
+        if response:
+            self.exception_on_error(response[4], servo_id, 'setting multiturn offset to %d' % offset)
+        return response
+
+
+
     def set_voltage_limit_min(self, servo_id, min_voltage):
         """
         Set the minimum voltage limit.
@@ -530,6 +565,7 @@ def set_position(self, servo_id, position):
         Set the servo with servo_id to the specified goal position.
         Position value must be positive.
         """
+        position &= 0xffff
         loVal = int(position % 256)
         hiVal = int(position >> 8)
 
@@ -614,6 +650,7 @@ def set_position_and_speed(self, servo_id, position, speed):
             hiSpeedVal = int((1023 - speed) >> 8)
 
         # split position into 2 bytes
+        position &= 0xffff
         loPositionVal = int(position % 256)
         hiPositionVal = int(position >> 8)
 
@@ -718,6 +755,7 @@ def set_multi_position(self, valueTuples):
             sid = vals[0]
             position = vals[1]
             # split position into 2 bytes
+            position &= 0xffff
             loVal = int(position % 256)
             hiVal = int(position >> 8)
             writeableVals.append( (sid, loVal, hiVal) )
@@ -792,6 +830,7 @@ def set_multi_position_and_speed(self, valueTuples):
                 hiSpeedVal = int((1023 - speed) >> 8)
 
             # split position into 2 bytes
+            position &= 0xffff
             loPositionVal = int(position % 256)
             hiPositionVal = int(position >> 8)
             writeableVals.append( (sid, loPositionVal, hiPositionVal, loSpeedVal, hiSpeedVal) )
@@ -867,6 +906,8 @@ def get_position(self, servo_id):
         if response:
             self.exception_on_error(response[4], servo_id, 'fetching present position')
         position = response[5] + (response[6] << 8)
+        if position & 0x8000:
+            position += -0x10000
         return position
 
     def get_speed(self, servo_id):
@@ -924,6 +965,8 @@ def get_feedback(self, servo_id):
             # extract data values from the raw data
             goal = response[5] + (response[6] << 8)
             position = response[11] + (response[12] << 8)
+            if position & 0x8000:
+                position += -0x10000
             error = position - goal
             speed = response[13] + ( response[14] << 8)
             if speed > 1023: speed = 1023 - speed
@@ -948,6 +991,31 @@ def get_feedback(self, servo_id):
                      'temperature': temperature,
                      'moving': bool(moving) }
 
+    def get_load(self, servo_id):
+        """
+        Returns the present load value (raw, -1023 .. 1023) from the specified servo.
+        """
+        response = self.read(servo_id, DXL_PRESENT_LOAD_L, 2)
+        if response:
+            self.exception_on_error(response[4], servo_id, 'fetching moving status')
+
+        load_raw = response[5] + (response[6] << 8)
+        load = load_raw & 0x03ff
+        if self.test_bit(load_raw, 10): load *= -1
+
+        return load
+
+
+    def get_moving(self, servo_id):
+        """
+        Returns moving status
+        """
+        response = self.read(servo_id, DXL_MOVING, 1)
+
+        if response:
+            self.exception_on_error(response[4], servo_id, 'fetching moving status')
+        return bool(response[5])
+
     def exception_on_error(self, error_code, servo_id, command_failed):
         global exception
         exception = None