From c6582103c9df2c21797195772709df0ca4740196 Mon Sep 17 00:00:00 2001 From: Andrey Pohilko Date: Sun, 24 Dec 2017 12:06:42 +0300 Subject: [PATCH] Experiment with angled movements --- examples/plotter/__init__.py | 40 +++++++++++++++++++++--------------- examples/plotter/try.py | 27 ++++++++++++++++++++---- pylgbst/peripherals.py | 3 ++- tests.py | 10 ++++----- 4 files changed, 54 insertions(+), 26 deletions(-) diff --git a/examples/plotter/__init__.py b/examples/plotter/__init__.py index 6e32c19..f927640 100644 --- a/examples/plotter/__init__.py +++ b/examples/plotter/__init__.py @@ -2,7 +2,7 @@ import logging import math import time -from pylgbst import MoveHub, ColorDistanceSensor, COLORS, COLOR_RED, COLOR_YELLOW +from pylgbst import MoveHub, ColorDistanceSensor, COLORS, COLOR_RED, COLOR_CYAN class Plotter(MoveHub): @@ -10,7 +10,7 @@ class Plotter(MoveHub): def __init__(self, connection=None, base_speed=1.0): super(Plotter, self).__init__(connection) - self.base_speed = base_speed + self.base_speed = float(base_speed) self.field_width = 3.55 / self.base_speed self.xpos = 0 self.ypos = 0 @@ -35,7 +35,7 @@ class Plotter(MoveHub): self.caret.constant(-self.base_speed) count = 0 max_tries = 50 - while self._marker_color not in (COLOR_RED, COLOR_YELLOW) and count < max_tries: + while self._marker_color not in (COLOR_RED, COLOR_CYAN) and count < max_tries: time.sleep(30.0 / max_tries) count += 1 self.color_distance_sensor.unsubscribe(self._on_distance) @@ -47,16 +47,27 @@ class Plotter(MoveHub): self.caret.stop() self.color_distance_sensor.unsubscribe(self._on_distance) - if self._marker_color != COLOR_YELLOW: + if self._marker_color != COLOR_CYAN: self.move(-self.field_width, 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_YELLOW): + 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() @@ -96,27 +107,19 @@ class Plotter(MoveHub): logging.warning("No movement, ignored") return - self._correct_wheels(movy) + self._compensate_wheels_backlash(movy) self.xpos += movx self.ypos += movy - length, speed_a, speed_b = self._calc_motor(movx, movy) + length, speed_a, speed_b = self._calc_motor_timed(movx, movy) self.motor_AB.timed(length, -speed_a * self.base_speed / self.MOTOR_RATIO, -speed_b * self.base_speed) # time.sleep(0.5) - def _correct_wheels(self, movy): - if not movy: - return - wheel_dir = movy / abs(movy) - if wheel_dir == -self.__last_wheel_dir: - self.wheels.angled(230, -wheel_dir) - self.__last_wheel_dir = wheel_dir - @staticmethod - def _calc_motor(movx, movy): + def _calc_motor_timed(movx, movy): amovx = float(abs(movx)) amovy = float(abs(movy)) @@ -136,6 +139,11 @@ class Plotter(MoveHub): return length, speed_a, speed_b + @staticmethod + def _calc_motor_angled(movx, movy): + pass + #return length, speed_a, speed_b + def circle(self, radius): if not self.is_tool_down: self._tool_down() diff --git a/examples/plotter/try.py b/examples/plotter/try.py index f8041c7..1fc2405 100644 --- a/examples/plotter/try.py +++ b/examples/plotter/try.py @@ -218,6 +218,25 @@ def snowflake(): plotter.line(item[0] * zoom, -item[1] * zoom) +def angles_experiment(): + plotter._tool_down() + + path = 1000 + + parts = 5 + for x in range(1, parts): + spd_b = x * plotter.base_speed / parts + spd_a = plotter.base_speed - spd_b + + angle = path * (1.0 + spd_b / spd_a) + logging.info("%s, %s, %s", angle, spd_a, spd_b) + + plotter.motor_AB.angled(angle, spd_a, -spd_b) + plotter._compensate_wheels_backlash(-1) + plotter.motor_AB.angled(-angle, spd_a, -spd_b) + plotter._compensate_wheels_backlash(1) + + if __name__ == '__main__': logging.basicConfig(level=logging.INFO) @@ -227,13 +246,13 @@ if __name__ == '__main__': logging.warning("Failed to use debug server: %s", traceback.format_exc()) conn = BLEConnection().connect() - plotter = LaserPlotter(conn, 0.75) + plotter = LaserPlotter(conn, 1.0) FIELD_WIDTH = plotter.field_width try: - plotter.initialize() + # plotter.initialize() - # plotter._tool_down() + angles_experiment() # try_speeds() @@ -249,7 +268,7 @@ if __name__ == '__main__': # square_spiral() # lego() # christmas_tree() - snowflake() + # snowflake() pass finally: plotter.finalize() diff --git a/pylgbst/peripherals.py b/pylgbst/peripherals.py index 0cfdfbc..88eb87f 100644 --- a/pylgbst/peripherals.py +++ b/pylgbst/peripherals.py @@ -138,7 +138,8 @@ class LED(Peripheral): class EncodedMotor(Peripheral): - TRAILER = b'\x64\x7f\x03' # NOTE: \x64 is 100, might mean something + TRAILER = b'\x64\x7f\x03' # NOTE: \x64 is 100, might mean something; also trailer might be a sequence terminator + # TODO: investigate sequence behavior, seen with zero values passed to angled mode # trailer is not required for all movement types MOVEMENT_TYPE = 0x11 diff --git a/tests.py b/tests.py index defaaf8..5b29aa4 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, 0.5), Plotter._calc_motor(100, 50)) + self.assertEqual((100, 1, 0.5), Plotter._calc_motor_timed(100, 50)) def test_calc2(self): - self.assertEqual((200, 0.5, 1), Plotter._calc_motor(100, 200)) + self.assertEqual((200, 0.5, 1), Plotter._calc_motor_timed(100, 200)) def test_calc_xoverflow(self): - self.assertEqual((800, 0.0125, 1), Plotter._calc_motor(10, 800)) + self.assertEqual((800, 0.0125, 1), Plotter._calc_motor_timed(10, 800)) def test_calc3(self): - self.assertEqual((100, 1, 0), Plotter._calc_motor(100, 0)) + self.assertEqual((100, 1, 0), Plotter._calc_motor_timed(100, 0)) def test_calc4(self): - self.assertEqual((200, 0, 1), Plotter._calc_motor(0, 200)) + self.assertEqual((200, 0, 1), Plotter._calc_motor_timed(0, 200))