From 184950a1c5350cbda0143f598609f580d5054c1c Mon Sep 17 00:00:00 2001 From: Andrey Pohilko Date: Tue, 19 Dec 2017 19:31:30 +0300 Subject: [PATCH] REctangle and romb --- examples/plotter/__init__.py | 72 +++++++++++++++--------------------- examples/plotter/try.py | 63 ++++++++++++++++--------------- pylgbst/movehub.py | 2 - tests.py | 25 ++++++++----- 4 files changed, 76 insertions(+), 86 deletions(-) diff --git a/examples/plotter/__init__.py b/examples/plotter/__init__.py index 1336593..d6c8efb 100644 --- a/examples/plotter/__init__.py +++ b/examples/plotter/__init__.py @@ -5,8 +5,8 @@ import time from pylgbst import MoveHub from pylgbst.peripherals import EncodedMotor -BASE_SPEED = 0.5 -CARET_WIDTH = 940 +BASE_SPEED = 0.75 +FIELD_WIDTH = 1.1 class Plotter(MoveHub): @@ -24,9 +24,9 @@ class Plotter(MoveHub): self.is_tool_down = False def _reset_caret(self): - self.motor_A.timed(0.5, -0.3) + self.motor_A.timed(0.5, -0.2) self.motor_A.subscribe(self._on_rotate, mode=EncodedMotor.SENSOR_SPEED) - self.motor_A.constant(0.3) + self.motor_A.constant(0.2) count = 0 max_tries = 50 while abs(self.__last_rotation_value) > 5 and count < max_tries: @@ -38,18 +38,18 @@ class Plotter(MoveHub): self.motor_A.stop() if count >= max_tries: raise RuntimeError("Failed to center caret") - self.motor_A.angled(-CARET_WIDTH, BASE_SPEED) + self.motor_A.timed(FIELD_WIDTH, -BASE_SPEED) def _on_rotate(self, value): logging.debug("Rotation: %s", value) self.__last_rotation_value = value def _tool_down(self): - self.motor_external.angled(270, BASE_SPEED) + self.motor_external.angled(270, 1) self.is_tool_down = True def _tool_up(self): - self.motor_external.angled(-270, BASE_SPEED) + self.motor_external.angled(-270, 1) self.is_tool_down = False def finalize(self): @@ -69,14 +69,14 @@ class Plotter(MoveHub): self._transfer_to(movx, movy) def _transfer_to(self, movx, movy): - if self.xpos + movx < -CARET_WIDTH: + if self.xpos + movx < -FIELD_WIDTH: logging.warning("Invalid xpos: %s", self.xpos) - movx += self.xpos - CARET_WIDTH + movx += self.xpos - FIELD_WIDTH - if self.xpos + movx > CARET_WIDTH: + if self.xpos + movx > FIELD_WIDTH: logging.warning("Invalid xpos: %s", self.xpos) - movx -= self.xpos - CARET_WIDTH - self.xpos -= self.xpos - CARET_WIDTH + movx -= self.xpos - FIELD_WIDTH + self.xpos -= self.xpos - FIELD_WIDTH if not movy and not movx: logging.warning("No movement, ignored") @@ -85,42 +85,30 @@ class Plotter(MoveHub): self.xpos += movx self.ypos += movy - angle, speed_a, speed_b = calc_motor(movx, movy) + length, speed_a, speed_b = self.calc_motor(movx, movy) - if not speed_b: - self.motor_A.angled(angle, speed_a * BASE_SPEED) - elif not speed_a: - self.motor_B.angled(angle, speed_b * BASE_SPEED) - else: - self.motor_AB.angled(angle, speed_a * BASE_SPEED, speed_b * BASE_SPEED) + self.motor_AB.timed(length, -speed_a * BASE_SPEED, -speed_b * BASE_SPEED) # time.sleep(0.5) + @staticmethod + def calc_motor(movx, movy): + motor_ratio = 1.15 + amovx = float(abs(movx)) + amovy = float(abs(movy)) -def calc_motor(movx, movy): - amovy = abs(movy) - amovx = abs(movx) - angle = max(amovx, amovy) + 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 / float(amovx)) if movx else 0 - else: - speed_a = (movx / float(amovy)) if movy else 0 + speed_a = (movx / float(amovx)) if amovx else 0.0 + speed_b = (movy / float(amovy)) if amovy else 0.0 - if speed_a: - speed_b *= 2.75 - else: - angle *= 1.5 + if amovx >= amovy * motor_ratio: + speed_b = movy / amovx * motor_ratio + else: + speed_a = movx / amovy / motor_ratio - norm = max(abs(speed_a), abs(speed_b)) - speed_a /= norm - speed_b /= norm - angle *= speed_a + logging.info("Motor: %s with %s/%s", length, speed_a, speed_b) + assert -1 <= speed_a <= 1 + assert -1 <= speed_b <= 1 - logging.info("Motor: %s with %s/%s", angle, speed_a, speed_b) - assert -1 <= speed_a <= 1 - assert -1 <= speed_b <= 1 - - return angle, speed_a, speed_b + return length, speed_a, speed_b diff --git a/examples/plotter/try.py b/examples/plotter/try.py index 3b39edf..2849038 100644 --- a/examples/plotter/try.py +++ b/examples/plotter/try.py @@ -1,49 +1,49 @@ import logging import traceback -from examples.plotter import Plotter, CARET_WIDTH +from examples.plotter import Plotter, FIELD_WIDTH from pylgbst.comms import DebugServerConnection, BLEConnection -def cross(): - plotter.line(CARET_WIDTH, CARET_WIDTH) - plotter.move(-CARET_WIDTH, 0) - plotter.line(CARET_WIDTH, -CARET_WIDTH) - - def moves(): - plotter.move(CARET_WIDTH, CARET_WIDTH) - plotter.move(-CARET_WIDTH, -CARET_WIDTH) + plotter.move(FIELD_WIDTH, FIELD_WIDTH) + plotter.move(-FIELD_WIDTH, -FIELD_WIDTH) - plotter.move(CARET_WIDTH, 0) - plotter.move(-CARET_WIDTH, 0) - plotter.move(0, CARET_WIDTH) - plotter.move(0, -CARET_WIDTH) + plotter.move(FIELD_WIDTH, 0) + plotter.move(-FIELD_WIDTH, 0) + plotter.move(0, FIELD_WIDTH) + plotter.move(0, -FIELD_WIDTH) + + +def cross(): + plotter.line(FIELD_WIDTH, FIELD_WIDTH) + plotter.move(-FIELD_WIDTH, 0) + plotter.line(FIELD_WIDTH, -FIELD_WIDTH) def square(): - plotter.line(CARET_WIDTH, 0) - plotter.line(0, CARET_WIDTH) - plotter.line(-CARET_WIDTH, 0) - plotter.line(0, -CARET_WIDTH) + plotter.line(FIELD_WIDTH, 0) + plotter.line(0, FIELD_WIDTH) + plotter.line(-FIELD_WIDTH, 0) + plotter.line(0, -FIELD_WIDTH) def triangle(): - plotter.line(CARET_WIDTH, 0) - plotter.line(0, CARET_WIDTH) - plotter.line(-CARET_WIDTH, -CARET_WIDTH) + plotter.line(FIELD_WIDTH, 0) + plotter.line(0, FIELD_WIDTH) + plotter.line(-FIELD_WIDTH, -FIELD_WIDTH) def romb(): - plotter.move(-CARET_WIDTH * 2, 0) - plotter.line(CARET_WIDTH, CARET_WIDTH) - plotter.line(CARET_WIDTH, -CARET_WIDTH) - plotter.line(-CARET_WIDTH, -CARET_WIDTH) - plotter.line(-CARET_WIDTH, CARET_WIDTH) + plotter.move(-FIELD_WIDTH, 0) + plotter.line(FIELD_WIDTH, FIELD_WIDTH) + plotter.line(FIELD_WIDTH, -FIELD_WIDTH) + plotter.line(-FIELD_WIDTH, -FIELD_WIDTH) + plotter.line(-FIELD_WIDTH, FIELD_WIDTH) if __name__ == '__main__': - logging.basicConfig(level=logging.INFO) + logging.basicConfig(level=logging.DEBUG) try: conn = DebugServerConnection() @@ -52,13 +52,12 @@ if __name__ == '__main__': conn = BLEConnection().connect() plotter = Plotter(conn) - #plotter.initialize() - # plotter._tool_up() # and plotter._tool_up() + plotter.initialize() - triangle() # moves() - # square() - # cross() - # romb() + # triangle() + square() + cross() + #romb() plotter.finalize() diff --git a/pylgbst/movehub.py b/pylgbst/movehub.py index 2b2b00f..fd3142b 100644 --- a/pylgbst/movehub.py +++ b/pylgbst/movehub.py @@ -81,8 +81,6 @@ class MoveHub(object): raise RuntimeError("Failed to obtain all builtin devices") def _notify(self, handle, data): - # 1b0e000500823701 - orig = data if handle != MOVE_HUB_HARDWARE_HANDLE: diff --git a/tests.py b/tests.py index 0bf0835..2110055 100644 --- a/tests.py +++ b/tests.py @@ -1,7 +1,7 @@ import unittest from binascii import unhexlify -from examples.plotter import calc_motor +from examples.plotter import Plotter from pylgbst import * from pylgbst.comms import Connection from pylgbst.movehub import MoveHub @@ -195,12 +195,17 @@ class GeneralTest(unittest.TestCase): class TestPlotter(unittest.TestCase): - def test_calc(self): - self.assertEqual((100, 0, 0.24), calc_motor(100, 100)) - self.assertEqual((100, 0, 0.24), calc_motor(-100, -100)) - self.assertEqual((100, 0, 0.24), calc_motor(0, 100)) - self.assertEqual((100, 0.75, 0), calc_motor(100, 0)) - self.assertEqual((100, -0.75, 0), calc_motor(-100, 0)) - self.assertEqual((100, 0, -0.24), calc_motor(0, -100)) - self.assertEqual((100, 0.75, 0.12), calc_motor(100, 50)) - self.assertEqual((100, 0.375, 0.24), calc_motor(50, 100)) + def test_calc1(self): + 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)) + + def test_calc_xoverflow(self): + 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)) + + def test_calc4(self): + self.assertEqual((400, 0, 1), Plotter.calc_motor(0, 200))