def test_mul(self): v1 = Vector(2.0, 4.0, 6.0) res = v1 * 2.0 self.assertEqual(res, Vector(4.0, 8.0, 12.0)) res = v1 * Vector(-1.0, -1.0, -1.0) self.assertEqual(res, Vector(-2.0, -4.0, -6.0))
def test_div(self): v1 = Vector(2.0, 4.0, 6.0) res = v1 / 2.0 self.assertEqual(res, Vector(1.0, 2.0, 3.0)) res = v1 / Vector(2.0, 4.0, 6.0) self.assertEqual(res, Vector(1.0, 1.0, 1.0))
def test_init(self): v1 = Vector(1, 2, 3) v2 = Vector((1, 2, 3)) v3 = Vector({'x': 1, 'y': 2, 'z': 3}) v4 = Vector({'x': 1}) self.assertEqual(v1, (1, 2, 3)) self.assertEqual(v2, (1, 2, 3)) self.assertEqual(v3, (1, 2, 3)) self.assertEqual(v4, Vector(1, 0, 0))
def add(self, child, name=None, coordinates=Vector(0, 0, 0)): if not name: name = str(child) if name in self.children_by_name: raise Exception( 'Child with the name {} already exists'.format(name)) child._coordinates = Vector(coordinates) child.parent = self self.children_by_name[name] = child self.children_by_reference[child] = name
def coordinates(self, reference=None): if reference == self: return Vector(0, 0, 0) if not self.parent: return Vector(0, 0, 0) if not reference: return self.parent.get_child_coordinates(self) return self.parent.coordinates(reference) + self.coordinates()
def convert(self, placeable, coordinates=Vector(0, 0, 0)): coordinates = Vector(coordinates) path = placeable.get_trace() adjusted_coordinates = Vector(0, 0, 0) for item in path: c = self.calibrated_coordinates.get(item, item._coordinates) adjusted_coordinates += c return coordinates + adjusted_coordinates
def test_break_down_travel(self): # with 3-dimensional points p1 = Vector(0, 0, 0) p2 = Vector(10, -12, 14) res = break_down_travel(p1, p2, increment=5, mode='absolute') self.assertEquals(res[-1], p2) self.assertEquals(len(res), 5) p1 = Vector(10, -12, 14) res = break_down_travel(Vector(0, 0, 0), p1, mode='relative') expected = Vector(0.46537410754407676, -0.5584489290528921, 0.6515237505617075) self.assertEquals(res[-1], expected) self.assertEquals(len(res), 5)
def test_top_bottom(self): deck = Deck() slot = Slot() plate = self.generate_plate(wells=4, cols=2, spacing=(10, 10), offset=(0, 0), radius=5, height=10) deck.add(slot, 'A1', (0, 0, 0)) slot.add(plate) self.assertEqual(plate['A1'].bottom(10), (plate['A1'], Vector(5, 5, 10))) self.assertEqual(plate['A1'].top(10), (plate['A1'], Vector(5, 5, 20)))
def __init__(self, parent=None, properties=None): self.children_by_name = OrderedDict() self.children_by_reference = OrderedDict() self._coordinates = Vector(0, 0, 0) self._max_dimensions = {} self.parent = parent if properties is None: properties = {} self.properties = properties if 'radius' in properties: properties['width'] = properties['radius'] * 2 properties['length'] = properties['radius'] * 2 if 'diameter' in properties: properties['width'] = properties['diameter'] properties['length'] = properties['diameter'] if 'depth' in properties: properties['height'] = properties['depth'] for dimension in ['length', 'width', 'height']: if dimension not in properties: properties[dimension] = 0
def from_polar(self, r, theta, h): center = self.size() / 2.0 r = r * center['x'] return center + Vector(r * math.cos(theta), r * math.sin(theta), center['z'] * h)
def test_dispense_move_to(self): x, y, z = (161.0, 116.7, 3.0) well = self.plate[0] pos = well.from_center(x=0, y=0, z=-1, reference=self.plate) location = (self.plate, pos) self.robot._driver.move_head(x=x, y=y, z=z) self.p200.calibrate_position(location) self.p200.aspirate(100, location) self.p200.dispense(100, location) self.robot.run(mode='live') driver = self.robot._driver current_plunger_pos = driver.get_plunger_positions()['current'] current_head_pos = driver.get_head_position()['current'] self.assertEqual( current_head_pos, Vector({'x': 161.0, 'y': 116.7, 'z': 3.0}) ) self.assertDictEqual( current_plunger_pos, {'a': 0, 'b': 10.0} )
def calibrate(self, calibration_data, location, actual): actual = Vector(actual) placeable, expected = unpack_location(location) coordinates_to_deck = placeable.coordinates(placeable.get_deck()) expected_to_deck = expected + coordinates_to_deck delta = actual - expected_to_deck path = placeable.get_path() calibration_data = copy.deepcopy(calibration_data) current = {'children': calibration_data} for i, name in enumerate(path): children = current['children'] if name not in children: if i == len(path) - 1: children[name] = {} else: children[name] = {'children': {}} current = children[name] current['delta'] = delta self.calibration_data = calibration_data self._apply_calibration(calibration_data, self.placeable) return calibration_data
def break_down_travel(p1, target, increment=5, mode='absolute'): """ given two points p1 and target, this returns a list of incremental positions or relative steps """ heading = target - p1 if mode == 'relative': heading = target length = heading.length() length_steps = length / increment length_remainder = length % increment vector_step = Vector(0, 0, 0) if length_steps > 0: vector_step = heading / length_steps vector_remainder = vector_step * (length_remainder / increment) res = [] if mode is 'absolute': for i in range(int(length_steps)): p1 = p1 + vector_step res.append(p1) p1 = p1 + vector_remainder res.append(p1) else: for i in range(int(length_steps)): res.append(vector_step) res.append(vector_remainder) return res
def test_blow_out_move_to(self): x, y, z = (161.0, 116.7, 3.0) well = self.plate[0] pos = well.from_center(x=0, y=0, z=-1, reference=self.plate) location = (self.plate, pos) self.robot._driver.move_head(x=x, y=y, z=z) self.p200.calibrate_position(location) self.p200.blow_out(location) self.robot.run() current_pos = self.robot._driver.get_head_position()['current'] self.assertEqual( current_pos, Vector({'x': 161.0, 'y': 116.7, 'z': 3.0}) ) current_pos = self.robot._driver.get_plunger_positions()['current'] self.assertDictEqual( current_pos, {'a': 0, 'b': 12.0} )
def move_head(self, mode='absolute', **kwargs): self.set_coordinate_system(mode) current = self.get_head_position()['target'] log.debug('Current Head Position: {}'.format(current)) target_point = { axis: kwargs.get(axis, 0 if mode == 'relative' else current[axis]) for axis in 'xyz' } log.debug('Destination: {}'.format(target_point)) time_interval = 0.5 # convert mm/min -> mm/sec, # multiply by time interval to get increment in mm increment = self.head_speed / 60 * time_interval vector_list = break_down_travel(current, Vector(target_point), mode=mode, increment=increment) # turn the vector list into axis args args_list = [] for vector in vector_list: flipped_vector = self.flip_coordinates(vector, mode) args_list.append({ axis.upper(): flipped_vector[axis] for axis in 'xyz' if axis in kwargs }) return self.consume_move_commands(args_list, increment)
def unpack_location(location): coordinates = None placeable = None if isinstance(location, Placeable): coordinates = location.from_center(x=0, y=0, z=1) placeable = location elif isinstance(location, tuple): placeable, coordinates = location else: raise ValueError( 'Location should be (Placeable, (x, y, z)) or Placeable') return (placeable, Vector(coordinates))
def from_center(self, x=None, y=None, z=None, r=None, theta=None, h=None, reference=None): coords_to_endpoint = None if all([isinstance(i, numbers.Number) for i in (x, y, z)]): coords_to_endpoint = self.from_cartesian(x, y, z) if all([isinstance(i, numbers.Number) for i in (r, theta, h)]): coords_to_endpoint = self.from_polar(r, theta, h) coords_to_reference = Vector(0, 0, 0) if reference: coords_to_reference = self.coordinates(reference) return coords_to_reference + coords_to_endpoint
def position_for_aspirate(self, location=None): if location: self.move_to_top(location, now=True) if self.current_volume == 0: self.plunger.move(self.positions['bottom']) if location: if isinstance(location, Placeable): # go all the way to the bottom bottom = location.from_center(x=0, y=0, z=-1) # go up 1mm to give space to aspirate bottom += Vector(0, 0, 1) location = (location, bottom) self.go_to(location, now=True)
def test_drop_tip_move_to(self): x, y, z = (161.0, 116.7, 3.0) well = self.plate[0] pos = well.from_center(x=0, y=0, z=-1, reference=self.plate) location = (self.plate, pos) self.robot._driver.move_head(x=x, y=y, z=z) self.p200.calibrate_position(location) self.p200.drop_tip(location) self.robot.run() current_pos = self.robot._driver.get_head_position()['current'] self.assertEqual( current_pos, Vector({'x': 144.3, 'y': 97.0, 'z': 3.0}) )
def test_calibrate_placeable(self): well = self.plate[0] pos = well.from_center(x=0, y=0, z=0, reference=self.plate) location = (self.plate, pos) well_deck_coordinates = well.center(well.get_deck()) dest = well_deck_coordinates + Vector(1, 2, 3) self.robot._driver.move_head(x=dest['x'], y=dest['y'], z=dest['z']) self.p200.calibrate_position(location) expected_calibration_data = { 'A2': { 'children': { '96-flat': { 'delta': (1.0, 2.0, 3.0) }}}} self.assertDictEqual( self.p200.calibration_data, expected_calibration_data)
def size(self): return Vector(self.x_size(), self.y_size(), self.z_size())
def from_cartesian(self, x, y, z): center = self.size() / 2.0 return center + center * Vector(x, y, z)
def test_iterator(self): v1 = Vector(1, 2, 3) res = tuple([x for x in v1]) self.assertEqual(res, (1, 2, 3))
def test_index(self): v1 = Vector(1, 2, 3) self.assertEqual(v1['x'], 1) self.assertEqual(v1[0], 1) self.assertEqual(tuple(v1[:-1]), (1, 2))
def test_substract(self): v1 = Vector(1, 2, 3) v2 = Vector(4, 5, 6) res = v2 - v1 self.assertEqual(res, Vector(3.0, 3.0, 3.0))
def test_add(self): v1 = Vector(1, 2, 3) v2 = Vector(4, 5, 6) res = v1 + v2 self.assertEqual(res, Vector(5, 7, 9))
def get_head_position(self): coords = self.get_position() coords['current'] = self.flip_coordinates(Vector(coords['current'])) coords['target'] = self.flip_coordinates(Vector(coords['target'])) return coords
def flip_coordinates(self, coordinates, mode='absolute'): coordinates = Vector(coordinates) * Vector(1, -1, -1) if mode == 'absolute': offset = Vector(0, 1, 1) * self.ot_one_dimensions[self.ot_version] coordinates += offset return coordinates
class CNCDriver(object): """ This object outputs raw GCode commands to perform high-level tasks. """ MOVE = 'G0' DWELL = 'G4' HOME = 'G28' SET_POSITION = 'G92' GET_POSITION = 'M114' GET_ENDSTOPS = 'M119' SET_SPEED = 'G0' HALT = 'M112' CALM_DOWN = 'M999' ACCELERATION = 'M204' MOTORS_ON = 'M17' MOTORS_OFF = 'M18' DISENGAGE_FEEDBACK = 'M63' ABSOLUTE_POSITIONING = 'G90' RELATIVE_POSITIONING = 'G91' GET_OT_VERSION = 'config-get sd ot_version' GET_FIRMWARE_VERSION = 'version' GET_CONFIG_VERSION = 'config-get sd version' GET_STEPS_PER_MM = { 'x': 'config-get sd alpha_steps_per_mm', 'y': 'config-get sd beta_steps_per_mm' } SET_STEPS_PER_MM = { 'x': 'config-set sd alpha_steps_per_mm ', 'y': 'config-set sd beta_steps_per_mm ' } MOSFET = [{ True: 'M41', False: 'M40' }, { True: 'M43', False: 'M42' }, { True: 'M45', False: 'M44' }, { True: 'M47', False: 'M46' }, { True: 'M49', False: 'M48' }, { True: 'M51', False: 'M50' }] """ Serial port connection to talk to the device. """ connection = None serial_timeout = 0.1 # TODO: move to config ot_version = 'hood' ot_one_dimensions = { 'hood': Vector(300, 120, 120), 'one_pro': Vector(300, 250, 120), 'one_standard': Vector(300, 250, 120) } def __init__(self): self.stopped = Event() self.can_move = Event() self.resume() self.head_speed = 3000 # smoothie's default speed in mm/minute self.current_commands = [] self.axis_homed = { 'x': False, 'y': False, 'z': False, 'a': False, 'b': False } self.SMOOTHIE_SUCCESS = 'Succes' self.SMOOTHIE_ERROR = 'Received unexpected response from Smoothie' self.STOPPED = 'Received a STOP signal and exited from movements' def get_connected_port(self): """ Returns the port the driver is currently connected to :return: """ if not self.connection: return return self.connection.port def get_dimensions(self): return self.ot_one_dimensions[self.ot_version] def get_serial_ports_list(self): """ Lists serial port names :raises EnvironmentError: On unsupported or unknown platforms :returns: A list of the serial ports available on the system """ if sys.platform.startswith('win'): ports = ['COM%s' % (i + 1) for i in range(256)] elif (sys.platform.startswith('linux') or sys.platform.startswith('cygwin')): # this excludes your current terminal "/dev/tty" ports = glob.glob('/dev/tty[A-Za-z]*') elif sys.platform.startswith('darwin'): ports = glob.glob('/dev/tty.*') else: raise EnvironmentError('Unsupported platform') result = [] for port in ports: try: if 'usbmodem' in port or 'COM' in port: s = serial.Serial(port) s.close() result.append(port) except Exception as e: log.debug('Exception in testing port {}'.format(port)) log.debug(e) return result def disconnect(self): if self.is_connected(): self.connection.close() self.connection = None def connect(self, device): self.connection = device self.reset_port() log.debug("Connected to {}".format(device)) return self.calm_down() def is_connected(self): return self.connection and self.connection.isOpen() def reset_port(self): for axis in 'xyzab': self.axis_homed[axis.lower()] = False self.connection.close() self.connection.open() self.flush_port() self.turn_off_feedback() self.get_ot_version() def pause(self): self.can_move.clear() def resume(self): self.can_move.set() self.stopped.clear() def stop(self): if self.current_commands: self.stopped.set() self.can_move.set() else: self.resume() def send_command(self, command, **kwargs): """ Sends a GCode command. Keyword arguments will be automatically converted to GCode syntax. Returns a string with the Smoothie board's response Empty string if no response from Smoothie >>> send_command(self.MOVE, x=100 y=100) G0 X100 Y100 """ args = ' '.join(['{}{}'.format(k, v) for k, v in kwargs.items()]) command = '{} {}\r\n'.format(command, args) response = self.write_to_serial(command) return response def write_to_serial(self, data, max_tries=10, try_interval=0.2): log.debug("Write: {}".format(str(data).encode())) if self.connection is None: log.warn("No connection found.") return if self.is_connected(): self.connection.write(str(data).encode()) return self.wait_for_response() elif max_tries > 0: self.reset_port() return self.write_to_serial(data, max_tries=max_tries - 1, try_interval=try_interval) else: log.error("Cannot connect to serial port.") return b'' def wait_for_response(self, timeout=20.0): count = 0 max_retries = int(timeout / self.serial_timeout) while count < max_retries: count = count + 1 out = self.readline_from_serial() if out: log.debug("Waited {} lines for response {}.".format( count, out)) return out else: if count == 1 or count % 10 == 0: # Don't log all the time; gets spammy. log.debug("Waiting {} lines for response.".format(count)) raise RuntimeWarning('no response after {} seconds'.format(timeout)) def flush_port(self): # if we are running a virtual smoothie # we don't need a timeout for flush if isinstance(self.connection, VirtualSmoothie): self.readline_from_serial() else: time.sleep(self.serial_timeout) while self.readline_from_serial(): time.sleep(self.serial_timeout) def readline_from_serial(self): msg = b'' if self.is_connected(): # serial.readline() returns an empty byte string if it times out msg = self.connection.readline().strip() if msg: log.debug("Read: {}".format(msg)) # detect if it hit a home switch if b'!!' in msg or b'limit' in msg: # TODO (andy): allow this to bubble up so UI is notified log.debug('home switch hit') self.flush_port() self.calm_down() raise RuntimeWarning('limit switch hit') return msg def set_coordinate_system(self, mode): if mode == 'absolute': self.send_command(self.ABSOLUTE_POSITIONING) elif mode == 'relative': self.send_command(self.RELATIVE_POSITIONING) else: raise ValueError('Invalid coordinate mode: ' + mode) def move_plunger(self, mode='absolute', **kwargs): self.set_coordinate_system(mode) args = { axis.upper(): kwargs.get(axis) for axis in 'ab' if axis in kwargs } return self.consume_move_commands([args], 0.1) def move_head(self, mode='absolute', **kwargs): self.set_coordinate_system(mode) current = self.get_head_position()['target'] log.debug('Current Head Position: {}'.format(current)) target_point = { axis: kwargs.get(axis, 0 if mode == 'relative' else current[axis]) for axis in 'xyz' } log.debug('Destination: {}'.format(target_point)) time_interval = 0.5 # convert mm/min -> mm/sec, # multiply by time interval to get increment in mm increment = self.head_speed / 60 * time_interval vector_list = break_down_travel(current, Vector(target_point), mode=mode, increment=increment) # turn the vector list into axis args args_list = [] for vector in vector_list: flipped_vector = self.flip_coordinates(vector, mode) args_list.append({ axis.upper(): flipped_vector[axis] for axis in 'xyz' if axis in kwargs }) return self.consume_move_commands(args_list, increment) def consume_move_commands(self, args_list, step): tolerance = step * 0.5 self.current_commands = list(args_list) while self.can_move.wait(): if self.stopped.is_set(): self.resume() return (False, self.STOPPED) if self.current_commands: args = self.current_commands.pop(0) else: self.wait_for_arrival() break self.wait_for_arrival(tolerance) log.debug("Moving head: {}".format(args)) res = self.send_command(self.MOVE, **args) if res != b'ok': return (False, self.SMOOTHIE_ERROR) return (True, self.SMOOTHIE_SUCCESS) def flip_coordinates(self, coordinates, mode='absolute'): coordinates = Vector(coordinates) * Vector(1, -1, -1) if mode == 'absolute': offset = Vector(0, 1, 1) * self.ot_one_dimensions[self.ot_version] coordinates += offset return coordinates def wait_for_arrival(self, tolerance=0.1): arrived = False coords = self.get_position() while not arrived: coords = self.get_position() diff = {} for axis in coords.get('target', {}): diff[axis] = coords['current'][axis] - coords['target'][axis] dist = pow(diff['x'], 2) + pow(diff['y'], 2) + pow(diff['z'], 2) dist_head = math.sqrt(dist) """ smoothie not guaranteed to be EXACTLY where it's target is but seems to be about +-0.05 mm from the target coordinate the robot's physical resolution is found with: 1mm / config_steps_per_mm """ if dist_head < tolerance: if abs(diff['a']) < tolerance and abs(diff['b']) < tolerance: arrived = True else: arrived = False return arrived def home(self, *axis): axis_to_home = '' for a in axis: ax = ''.join(sorted(a)).upper() if ax in 'ABXYZ': axis_to_home += ax if not axis_to_home: axis_to_home = 'ABXYZ' res = self.send_command(self.HOME + axis_to_home) if res == b'ok': # the axis aren't necessarily set to 0.0 # values after homing, so force it pos_args = {} for l in axis_to_home: self.axis_homed[l.lower()] = True pos_args[l] = 0 return self.set_position(**pos_args) else: return False def wait(self, sec): ms = int((sec % 1.0) * 1000) s = int(sec) res = self.send_command(self.DWELL, S=s, P=ms) return res == b'ok' def calm_down(self): res = self.send_command(self.CALM_DOWN) return res == b'ok' def set_position(self, **kwargs): uppercase_args = {} for key in kwargs: uppercase_args[key.upper()] = kwargs[key] res = self.send_command(self.SET_POSITION, **uppercase_args) return res == b'ok' def get_head_position(self): coords = self.get_position() coords['current'] = self.flip_coordinates(Vector(coords['current'])) coords['target'] = self.flip_coordinates(Vector(coords['target'])) return coords def get_plunger_positions(self): coords = self.get_position() plunger_coords = {} for state in ['current', 'target']: plunger_coords[state] = { axis: coords[state][axis] for axis in 'ab' } return plunger_coords def get_position(self): res = self.send_command(self.GET_POSITION) # remove the "ok " from beginning of response res = res.decode('utf-8')[3:] coords = {} try: response_dict = json.loads(res).get(self.GET_POSITION) coords = {'target': {}, 'current': {}} for letter in 'xyzab': # the lowercase axis are the "real-time" values coords['current'][letter] = response_dict.get(letter, 0) # the uppercase axis are the "target" values coords['target'][letter] = response_dict.get(letter.upper(), 0) except JSON_ERROR: log.debug("Error parsing JSON string:") log.debug(res) return coords def turn_off_feedback(self): res = self.send_command(self.DISENGAGE_FEEDBACK) if res == b'feedback disengaged': res = self.wait_for_response() return res == b'ok' else: return False def calibrate_steps_per_mm(self, axis, expected_travel, actual_travel): current_steps_per_mm = self.get_steps_per_mm(axis) current_steps_per_mm *= (expected_travel / actual_travel) current_steps_per_mm = round(current_steps_per_mm, 2) return self.set_steps_per_mm(axis, current_steps_per_mm) def set_head_speed(self, rate): self.head_speed = rate kwargs = {"F": rate} res = self.send_command(self.SET_SPEED, **kwargs) return res == b'ok' def set_plunger_speed(self, rate, axis): if axis.lower() not in 'ab': raise ValueError('Axis {} not supported'.format(axis)) kwargs = {axis.lower(): rate} res = self.send_command(self.SET_SPEED, **kwargs) return res == b'ok' def get_ot_version(self): res = self.send_command(self.GET_OT_VERSION) res = res.decode().split(' ')[-1] if res not in self.ot_one_dimensions: raise ValueError('{} is not an ot_version'.format(res)) self.ot_version = res return self.ot_version def get_firmware_version(self): res = self.send_command(self.GET_FIRMWARE_VERSION) res = res.decode().split(' ')[-1] # the version is returned as a JSON dict, the version is a string # but not wrapped in double-quotes as JSON requires... # aka --> {"version":v1.0.5} self.firmware_version = res.split(':')[-1][:-1] return self.firmware_version def get_config_version(self): res = self.send_command(self.GET_CONFIG_VERSION) res = res.decode().split(' ')[-1] self.config_version = res return self.config_version def get_steps_per_mm(self, axis): if axis not in self.GET_STEPS_PER_MM: raise ValueError('Axis {} not supported'.format(axis)) res = self.send_command(self.GET_STEPS_PER_MM[axis]) return float(res.decode().split(' ')[-1]) def set_steps_per_mm(self, axis, value): if axis not in self.SET_STEPS_PER_MM: raise ValueError('Axis {} not supported'.format(axis)) command = self.SET_STEPS_PER_MM[axis] command += str(value) res = self.send_command(command) return res.decode().split(' ')[-1] == str(value) def get_endstop_switches(self): first_line = self.send_command(self.GET_ENDSTOPS) second_line = self.wait_for_response() if second_line == b'ok': res = json.loads(first_line.decode()) res = res.get(self.GET_ENDSTOPS) obj = {} for axis in 'xyzab': obj[axis] = bool(res.get('min_' + axis)) return obj else: return False def set_mosfet(self, mosfet_index, state): try: command = self.MOSFET[mosfet_index][bool(state)] res = self.send_command(command) return res == b'ok' except IndexError: raise IndexError( "Smoothie mosfet not at index {}".format(mosfet_index)) def power_on(self): res = self.send_command(self.MOTORS_ON) return res == b'ok' def power_off(self): res = self.send_command(self.MOTORS_OFF) return res == b'ok'