import logging import math import time from pylgbst.constants import COLOR_RED, COLOR_CYAN, COLORS from pylgbst.peripherals import ColorDistanceSensor class Plotter(object): MOTOR_RATIO = 1.425 ROTATE_UNIT = 2100 def __init__(self, hub, base_speed=1.0): """ :type hub: pylgbst.movehub.MoveHub """ self._hub = hub self.caret = self._hub.motor_A self.wheels = self._hub.motor_B self.both = self._hub.motor_AB self.base_speed = float(base_speed) self.xpos = 0 self.ypos = 0 self.is_tool_down = False self._marker_color = False self.__last_wheel_dir = 1 def initialize(self): self._reset_caret() self._compensate_wheels_backlash(1) self._compensate_wheels_backlash(-1) self.xpos = 0 self.ypos = 0 self.is_tool_down = False def _reset_caret(self): if not self._hub.color_distance_sensor: logging.warning("No color/distance sensor, cannot center caret") return self._hub.color_distance_sensor.subscribe(self._on_distance, mode=ColorDistanceSensor.COLOR_DISTANCE_FLOAT) self.caret.timed(0.5, 1) try: self.caret.constant(-1) count = 0 max_tries = 50 while self._marker_color not in (COLOR_RED, COLOR_CYAN) and count < max_tries: time.sleep(30.0 / max_tries) count += 1 self._hub.color_distance_sensor.unsubscribe(self._on_distance) clr = COLORS[self._marker_color] if self._marker_color else None logging.info("Centering tries: %s, color #%s", count, clr) if count >= max_tries: raise RuntimeError("Failed to center caret") finally: self.caret.stop() self._hub.color_distance_sensor.unsubscribe(self._on_distance) if self._marker_color == COLOR_CYAN: self.move(-0.1, 0) else: self.move(-1.0, 0) def _on_distance(self, color, distance): self._marker_color = None logging.debug("Color: %s, distance %s", COLORS[color], distance) if color in (COLOR_RED, COLOR_CYAN): if distance <= 3: self._marker_color = color def _compensate_wheels_backlash(self, movy): """ corrects backlash of wheels gear system """ if not movy: return wheel_dir = movy / abs(movy) if wheel_dir == -self.__last_wheel_dir: self.wheels.angled(270, -wheel_dir) self.__last_wheel_dir = wheel_dir def finalize(self): if self.is_tool_down: self._tool_up() self.both.stop(is_async=True) def _tool_down(self): self.is_tool_down = True self._hub.motor_external.angled(-270, 1) time.sleep(1.0) # for laser to heat up def _tool_up(self): self._hub.motor_external.angled(270, 1) self.is_tool_down = False def move(self, movx, movy): if self.is_tool_down: self._tool_up() self._transfer_to(movx, movy) def line(self, movx, movy): if not self.is_tool_down: self._tool_down() self._transfer_to(movx, movy) def _transfer_to(self, movx, movy): if not movy and not movx: logging.warning("No movement, ignored") return self._compensate_wheels_backlash(movy) self.xpos += movx self.ypos += movy length, speed_a, speed_b = self._calc_motor_angled(movx, movy * self.MOTOR_RATIO) logging.info("Motors: %.3f with %.3f/%.3f", length, speed_a, speed_b) if not speed_b: self.caret.angled(length * 2.0, -speed_a * self.base_speed * 0.75) # slow down to cut better elif not speed_a: self.wheels.angled(length * 2.0, -speed_b * self.base_speed * 0.75) # slow down to cut better else: self.both.angled(length, -speed_a * self.base_speed, -speed_b * self.base_speed) @staticmethod def _calc_motor_timed(movx, movy): amovx = float(abs(movx)) amovy = float(abs(movy)) length = max(amovx, amovy) speed_a = (movx / float(amovx)) if amovx else 0.0 speed_b = (movy / float(amovy)) if amovy else 0.0 if amovx >= amovy: speed_b = movy / amovx else: speed_a = movx / amovy assert -1 <= speed_a <= 1 assert -1 <= speed_b <= 1 return length, speed_a, speed_b @staticmethod def _calc_motor_angled(movx, movy): amovx = abs(movx) amovy = abs(movy) if amovx >= amovy: ax = amovy / (amovx + amovy) spd_b = ax if spd_b < 0.05: spd_b = 0 spd_a = (1.0 - spd_b) rotate = Plotter.ROTATE_UNIT * amovx * (1.0 + spd_b / spd_a) logging.info("A: movx=%s, movy=%s, ax=%s", movx, movy, ax) else: ax = amovx / (amovx + amovy) spd_a = ax if spd_a < 0.05: spd_a = 0 spd_b = (1.0 - spd_a) rotate = Plotter.ROTATE_UNIT * amovy * (1.0 + spd_a / spd_b) logging.info("B: movx=%s, movy=%s, ax=%s", movx, movy, ax) assert 0 <= spd_a <= 1 assert 0 <= spd_b <= 1 spd_a *= (movx / amovx) if amovx else 0 spd_b *= (movy / amovy) if amovy else 0 return rotate, spd_a, spd_b def circle(self, radius): if not self.is_tool_down: self._tool_down() parts = int(2 * math.pi * radius * 7) dur = 0.025 logging.info("Circle of radius %s, %s parts with %s time", radius, parts, dur) speeds = [] for x in range(0, parts): speed_a = math.sin(x * 2.0 * math.pi / float(parts)) speed_b = math.cos(x * 2.0 * math.pi / float(parts)) speeds.append((speed_a, speed_b)) logging.debug("A: %s, B: %s", speed_a, speed_b) speeds.append((0, 0)) for speed_a, speed_b in speeds: spa = speed_a * self.base_speed spb = -speed_b * self.base_speed * self.MOTOR_RATIO logging.info("Motor speeds: %.3f / %.3f", spa, spb) self.both.constant(spa, spb) time.sleep(dur) def spiral(self, rounds, growth): if not self.is_tool_down: self._tool_down() dur = 0.00 parts = 16 speeds = [] for r in range(0, rounds): logging.info("Round: %s", r) for x in range(0, parts): speed_a = math.sin(x * 2.0 * math.pi / float(parts)) speed_b = math.cos(x * 2.0 * math.pi / float(parts)) dur += growth speeds.append((speed_a, speed_b, dur)) logging.debug("A: %s, B: %s", speed_a, speed_b) speeds.append((0, 0, 0)) for speed_a, speed_b, dur in speeds: spa = speed_a * self.base_speed spb = -speed_b * self.base_speed * self.MOTOR_RATIO self.both.constant(spa, spb) logging.info("Motor speeds: %.3f / %.3f", spa, spb) time.sleep(dur) def rectangle(self, width, height, solid=False): self.line(width, 0) self.line(0, height) self.line(-width, 0) self.line(0, -height) if solid: max_step = 0.01 rounds = int(math.ceil(height / max_step)) step = height / rounds flip = 1 for r in range(1, rounds): self.line(0, step) self.line(width * flip, 0) flip = -flip