diff --git a/dynamixel_driver/scripts/info_dump.py b/dynamixel_driver/scripts/info_dump.py index c104f00..6f614e6 100755 --- a/dynamixel_driver/scripts/info_dump.py +++ b/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'] diff --git a/dynamixel_driver/src/dynamixel_driver/dynamixel_const.py b/dynamixel_driver/src/dynamixel_driver/dynamixel_const.py index 850c964..c2035ad 100755 --- a/dynamixel_driver/src/dynamixel_driver/dynamixel_const.py +++ b/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 diff --git a/dynamixel_driver/src/dynamixel_driver/dynamixel_io.py b/dynamixel_driver/src/dynamixel_driver/dynamixel_io.py index a3a5a55..46e5466 100755 --- a/dynamixel_driver/src/dynamixel_driver/dynamixel_io.py +++ b/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