From 0496bb6dbdce3306e63962ec6c33c9e4659177b7 Mon Sep 17 00:00:00 2001 From: Andrey Pohilko Date: Wed, 20 Dec 2017 21:28:16 +0300 Subject: [PATCH] Lego word demo --- examples/plotter/__init__.py | 50 ++++++++++--------- examples/plotter/imgtracer.py | 2 +- examples/plotter/try.py | 90 ++++++++++++++++++++++++++++++----- tests.py | 10 ++-- 4 files changed, 113 insertions(+), 39 deletions(-) diff --git a/examples/plotter/__init__.py b/examples/plotter/__init__.py index 3b4570e..273f42b 100644 --- a/examples/plotter/__init__.py +++ b/examples/plotter/__init__.py @@ -4,14 +4,14 @@ import time from pylgbst import MoveHub, ColorDistanceSensor, COLORS, COLOR_RED, COLOR_CYAN -BASE_SPEED = 0.4 -FIELD_WIDTH = 0.925 / BASE_SPEED -MOTOR_RATIO = 1.15 - class Plotter(MoveHub): - def __init__(self, connection=None): + MOTOR_RATIO = 1.15 + + def __init__(self, connection=None, base_speed=1.0): super(Plotter, self).__init__(connection) + self.base_speed = base_speed + self.field_width = 0.925 / self.base_speed self.xpos = 0 self.ypos = 0 self.is_tool_down = False @@ -24,15 +24,15 @@ class Plotter(MoveHub): self.is_tool_down = False def _reset_caret(self): - self.motor_A.timed(0.2, BASE_SPEED) + self.motor_A.timed(0.2, self.base_speed) self.color_distance_sensor.subscribe(self._on_distance, mode=ColorDistanceSensor.COLOR_DISTANCE_FLOAT, granularity=5) try: - self.motor_A.constant(-BASE_SPEED) + self.motor_A.constant(-self.base_speed) count = 0 max_tries = 50 while not self._marker_color and count < max_tries: - time.sleep(5.0 / BASE_SPEED / max_tries) + time.sleep(5.0 / self.base_speed / max_tries) count += 1 logging.info("Centering tries: %s, color #%s", count, COLORS[self._marker_color] if self._marker_color else None) @@ -42,7 +42,7 @@ class Plotter(MoveHub): self.motor_A.stop() self.color_distance_sensor.unsubscribe(self._on_distance) - self.motor_A.timed(FIELD_WIDTH, BASE_SPEED) + self.motor_A.timed(self.field_width, self.base_speed) def _on_distance(self, color, distance): self._marker_color = None @@ -76,14 +76,14 @@ class Plotter(MoveHub): self._transfer_to(movx, movy) def _transfer_to(self, movx, movy): - if self.xpos + movx < -FIELD_WIDTH: + if self.xpos + movx < -self.field_width: logging.warning("Invalid xpos: %s", self.xpos) - movx += self.xpos - FIELD_WIDTH + movx += self.xpos - self.field_width - if self.xpos + movx > FIELD_WIDTH: + if self.xpos + movx > self.field_width: logging.warning("Invalid xpos: %s", self.xpos) - movx -= self.xpos - FIELD_WIDTH - self.xpos -= self.xpos - FIELD_WIDTH + movx -= self.xpos - self.field_width + self.xpos -= self.xpos - self.field_width if not movy and not movx: logging.warning("No movement, ignored") @@ -92,14 +92,14 @@ class Plotter(MoveHub): self.xpos += movx self.ypos += movy - length, speed_a, speed_b = self.calc_motor(movx, movy) + length, speed_a, speed_b = self._calc_motor(movx, movy) - self.motor_AB.timed(length, -speed_a * BASE_SPEED, -speed_b * BASE_SPEED) + self.motor_AB.timed(length * 4.0, -speed_a * self.base_speed / 4.0, -speed_b * self.base_speed / 4.0) # time.sleep(0.5) @staticmethod - def calc_motor(movx, movy): + def _calc_motor(movx, movy): amovx = float(abs(movx)) amovy = float(abs(movy)) @@ -108,10 +108,10 @@ class Plotter(MoveHub): speed_a = (movx / float(amovx)) if amovx else 0.0 speed_b = (movy / float(amovy)) if amovy else 0.0 - if amovx >= amovy * MOTOR_RATIO: - speed_b = movy / amovx * MOTOR_RATIO + if amovx >= amovy * Plotter.MOTOR_RATIO: + speed_b = movy / amovx * Plotter.MOTOR_RATIO else: - speed_a = movx / amovy / MOTOR_RATIO + speed_a = movx / amovy / Plotter.MOTOR_RATIO logging.info("Motor: %s with %s/%s", length, speed_a, speed_b) assert -1 <= speed_a <= 1 @@ -135,7 +135,10 @@ class Plotter(MoveHub): speeds.append((0, 0)) for speed_a, speed_b in speeds: - self.motor_AB.constant(speed_a * BASE_SPEED, -speed_b * BASE_SPEED * MOTOR_RATIO) + spa = speed_a * self.base_speed + spb = -speed_b * self.base_speed * self.MOTOR_RATIO + logging.info("Motor speeds: %.3f / %.3f", spa, spb) + self.motor_AB.constant(spa, spb) time.sleep(dur) def spiral(self, rounds, growth): @@ -157,5 +160,8 @@ class Plotter(MoveHub): speeds.append((0, 0, 0)) for speed_a, speed_b, dur in speeds: - self.motor_AB.constant(speed_a * BASE_SPEED, -speed_b * BASE_SPEED * MOTOR_RATIO) + spa = speed_a * self.base_speed + spb = -speed_b * self.base_speed * self.MOTOR_RATIO + self.motor_AB.constant(spa, spb) + logging.info("Motor speeds: %.3f / %.3f", spa, spb) time.sleep(dur) diff --git a/examples/plotter/imgtracer.py b/examples/plotter/imgtracer.py index f7c4914..a2e6f4d 100644 --- a/examples/plotter/imgtracer.py +++ b/examples/plotter/imgtracer.py @@ -21,7 +21,7 @@ class Tracer(object): self.dst.fill(False) self.mark = numpy.copy(self.dst) # start in center - self.height, self.width = self.dst.shanape[0:2] + self.height, self.width = self.dst.shape[0:2] self.posy = self.height / 2 self.posx = self.width / 2 self.lines = [] diff --git a/examples/plotter/try.py b/examples/plotter/try.py index 0ec0988..f9f0106 100644 --- a/examples/plotter/try.py +++ b/examples/plotter/try.py @@ -1,9 +1,8 @@ import logging +import time import traceback -import time - -from examples.plotter import Plotter, FIELD_WIDTH +from examples.plotter import Plotter from pylgbst.comms import DebugServerConnection, BLEConnection @@ -45,10 +44,10 @@ def romb(): def circles(): - plotter.move(FIELD_WIDTH / 2.0, 0) + plotter.move(FIELD_WIDTH / 4.0, 0) plotter.circle(FIELD_WIDTH / 2.0) - plotter.move(FIELD_WIDTH / 2.0, 0) + plotter.move(FIELD_WIDTH / 4.0, 0) plotter.circle(FIELD_WIDTH) @@ -59,6 +58,73 @@ class LaserPlotter(Plotter): time.sleep(1) +def lego(): + t = FIELD_WIDTH / 10.0 + h = t * 5.0 + w = t * 3.0 + + plotter.line(h, 0) + plotter.line(0, t) + plotter.line(-(h - t), 0) + plotter.line(0, 2 * t) + plotter.line(-t, 0) + plotter.line(0, -w) + + plotter.move(0, w + t) + + plotter.line(h, 0) + plotter.line(0, w) + plotter.line(-t, 0) + plotter.line(0, -2 * t) + plotter.line(-t, 0) + plotter.line(0, t) + plotter.line(-t, 0) + plotter.line(0, -t) + plotter.line(-t, 0) + plotter.line(0, 2 * t) + plotter.line(-t, 0) + plotter.line(0, -w) + + plotter.move(0, w + t) + + plotter.move(t, 0) + plotter.line(3 * t, 0) + plotter.line(t, t) + plotter.line(0, t) + plotter.line(-t, t) + plotter.line(-t, 0) + plotter.line(0, -t) + plotter.line(t, 0) + plotter.line(0, -t) + plotter.line(-3 * t, 0) + plotter.line(0, t) + plotter.line(t, 0) + plotter.line(0, t) + plotter.line(-3 * t, 0) + plotter.line(0, -t) + plotter.line(t, 0) + plotter.line(0, -t) + plotter.line(t, -t) + plotter.move(-t, 0) + + plotter.move(0, w + t) + + plotter.move(t, 0) + plotter.line(3 * t, 0) + plotter.line(t, t) + plotter.line(0, t) + plotter.line(-t, t) + plotter.line(-3 * t, 0) + plotter.line(-t, -t) + plotter.line(0, -t) + plotter.line(t, -t) + plotter.move(0, t) + plotter.line(3 * t, 0) + plotter.line(0, t) + plotter.line(-3 * t, 0) + plotter.line(0, -t) + + if __name__ == '__main__': logging.basicConfig(level=logging.INFO) @@ -68,7 +134,8 @@ if __name__ == '__main__': logging.warning("Failed to use debug server: %s", traceback.format_exc()) conn = BLEConnection().connect() - plotter = LaserPlotter(conn) + plotter = LaserPlotter(conn, 0.4) + FIELD_WIDTH = plotter.field_width try: # plotter._tool_down() @@ -76,12 +143,13 @@ if __name__ == '__main__': plotter.initialize() # moves() - #triangle() - #square() - #cross() + # triangle() + # square() + # cross() # romb() - #circles() - plotter.spiral(4, 0.02) + # circles() + # plotter.spiral(4, 0.02) + lego() pass finally: plotter.finalize() diff --git a/tests.py b/tests.py index 2110055..6e494ea 100644 --- a/tests.py +++ b/tests.py @@ -196,16 +196,16 @@ class GeneralTest(unittest.TestCase): class TestPlotter(unittest.TestCase): def test_calc1(self): - self.assertEqual((100, 1, 1), Plotter.calc_motor(100, 50)) + self.assertEqual((100, 1, 1), Plotter._calc_motor(100, 50)) def test_calc2(self): - self.assertEqual((400, 0.25, 1), Plotter.calc_motor(100, 200)) + self.assertEqual((400, 0.25, 1), Plotter._calc_motor(100, 200)) def test_calc_xoverflow(self): - self.assertEqual((400, 0.25, 1), Plotter.calc_motor(10, 900)) + self.assertEqual((400, 0.25, 1), Plotter._calc_motor(10, 900)) def test_calc3(self): - self.assertEqual((100, 1, 0), Plotter.calc_motor(100, 0)) + self.assertEqual((100, 1, 0), Plotter._calc_motor(100, 0)) def test_calc4(self): - self.assertEqual((400, 0, 1), Plotter.calc_motor(0, 200)) + self.assertEqual((400, 0, 1), Plotter._calc_motor(0, 200))