diff --git a/examples/plotter/__init__.py b/examples/plotter/__init__.py index ce40308..527ae55 100644 --- a/examples/plotter/__init__.py +++ b/examples/plotter/__init__.py @@ -6,7 +6,8 @@ from pylgbst import ColorDistanceSensor, COLORS, COLOR_RED, COLOR_CYAN class Plotter(object): - MOTOR_RATIO = 1.65 + MOTOR_RATIO = 1.45 + ROTATE_UNIT = 2100 def __init__(self, hub, base_speed=1.0): """ @@ -19,7 +20,7 @@ class Plotter(object): self.both = self._hub.motor_AB self.base_speed = float(base_speed) - self.field_width = 3.55 / self.base_speed + self.field_width = 1.0 / self.base_speed self.xpos = 0 self.ypos = 0 self.is_tool_down = False @@ -28,6 +29,8 @@ class Plotter(object): 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 @@ -37,9 +40,8 @@ class Plotter(object): 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, self.base_speed) - self._hub.color_distance_sensor.subscribe(self._on_distance, mode=ColorDistanceSensor.COLOR_DISTANCE_FLOAT, - granularity=1) try: self.caret.constant(-self.base_speed) count = 0 @@ -56,7 +58,9 @@ class Plotter(object): self.caret.stop() self._hub.color_distance_sensor.unsubscribe(self._on_distance) - if self._marker_color != COLOR_CYAN: + if self._marker_color == COLOR_CYAN: + self.move(-self.field_width * 0.1, 0) + else: self.move(-self.field_width, 0) def _on_distance(self, color, distance): @@ -114,18 +118,18 @@ class Plotter(object): # movx -= self.xpos - self.field_width # self.xpos -= self.xpos - self.field_width - # self._compensate_wheels_backlash(movy) + self._compensate_wheels_backlash(movy) self.xpos += movx self.ypos += movy - length, speed_a, speed_b = self._calc_motor_angled(movx, 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, -speed_a * self.base_speed) + self.caret.angled(length * 2.0, -speed_a * self.base_speed) elif not speed_a: - self.wheels.angled(length, -speed_b * self.base_speed) + self.wheels.angled(length * 2.0, -speed_b * self.base_speed) else: self.both.angled(length, -speed_a * self.base_speed, -speed_b * self.base_speed) @@ -155,21 +159,24 @@ class Plotter(object): amovy = abs(movy) if amovx >= amovy: ax = amovy / (amovx + amovy) - spd_a = ax - spd_b = (1.0 - spd_a) - rotate = 1500 * amovx * (1.0 + spd_a / spd_b) + 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_b = ax - spd_a = (1.0 - spd_b) - rotate = 1500 * amovy * (1.0 + spd_b / spd_a) + 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 - assert rotate < 6000 # safety valve spd_a *= (movx / amovx) if amovx else 0 spd_b *= (movy / amovy) if amovy else 0 return rotate, spd_a, spd_b @@ -237,3 +244,10 @@ class Plotter(object): self.line(0, step) self.line(width * flip, 0) flip = -flip + + +class LaserPlotter(Plotter): + + def _tool_down(self): + super(LaserPlotter, self)._tool_down() + time.sleep(1.0) diff --git a/examples/plotter/try.py b/examples/plotter/try.py index 25e3499..68ca46c 100644 --- a/examples/plotter/try.py +++ b/examples/plotter/try.py @@ -2,7 +2,7 @@ import logging import time import traceback -from examples.plotter import Plotter +from examples.plotter import LaserPlotter from pylgbst import EncodedMotor, PORT_AB, PORT_C, PORT_A, PORT_B, MoveHub from pylgbst.comms import DebugServerConnection, BLEConnection from tests import HubMock @@ -55,13 +55,6 @@ def circles(): plotter.circle(FIELD_WIDTH) -class LaserPlotter(Plotter): - - def _tool_down(self): - super(LaserPlotter, self)._tool_down() - time.sleep(1) - - def lego(): t = FIELD_WIDTH / 5.0 h = t * 5.0 @@ -133,7 +126,7 @@ def lego(): def square_spiral(): rounds = 7 - step = FIELD_WIDTH / 4.0 / rounds + step = plotter.base_speed / rounds / 3.0 for r in range(1, rounds): plotter.line(step * r * 4, 0) plotter.line(0, step * (r * 4 + 1)) @@ -170,7 +163,7 @@ def try_speeds(): time.sleep(1) -def snowflake(): +def snowflake(scale=1.0): a = [300, 232, 351, 144, 307, 68, @@ -206,7 +199,7 @@ def snowflake(): vals = [(x[0] / float(maxval), x[1] / float(maxval)) for x in vals] logging.info("Moves: %s", vals) - zoom = FIELD_WIDTH * 2.0 + zoom = FIELD_WIDTH * scale for item in vals: plotter.line(item[0] * zoom, item[1] * zoom) @@ -221,14 +214,14 @@ def snowflake(): def angles_experiment(): - parts = 5 + parts = 2 for x in range(0, parts + 1): movy = x * 1.0 / parts plotter.line(1.0, movy) plotter.move(-1.0, -movy) logging.info("%s", movy) - for x in range(0, parts + 1): + for x in range(0, parts): movx = x * 1.0 / parts plotter.line(movx, 1.0) plotter.move(-movx, -1.0) @@ -276,14 +269,15 @@ if __name__ == '__main__': hub = MoveHub(conn) if 1 else get_hub_mock() - plotter = LaserPlotter(hub, 1.0) - FIELD_WIDTH = plotter.field_width + plotter = LaserPlotter(hub, 0.4) + FIELD_WIDTH = plotter.field_width * 0.9 try: - # plotter._tool_up() plotter.initialize() - #angles_experiment() + # plotter._tool_down() + + # angles_experiment() # try_speeds() @@ -296,10 +290,10 @@ if __name__ == '__main__': # plotter.spiral(4, 0.02) # plotter.rectangle(FIELD_WIDTH / 5.0, FIELD_WIDTH / 5.0, solid=True) - # square_spiral() + square_spiral() # lego() # christmas_tree() - # snowflake() + # snowflake(0.5) pass finally: plotter.finalize() diff --git a/tests.py b/tests.py index 5b29aa4..da15a9f 100644 --- a/tests.py +++ b/tests.py @@ -209,3 +209,27 @@ class TestPlotter(unittest.TestCase): def test_calc4(self): self.assertEqual((200, 0, 1), Plotter._calc_motor_timed(0, 200)) + + def test_calc5(self): + parts = 2 + for x in range(0, parts + 1): + res = Plotter._calc_motor_angled(1.0, x * 1.0 / parts) + logging.debug("%s", res) + + for x in range(0, parts + 1): + res = Plotter._calc_motor_angled(x * 1.0 / parts, 1.0) + logging.debug("%s", res) + + def test_zeroes(self): + res = Plotter._calc_motor_angled(1.0, 0.0) + self.assertNotEqual(0, res[1]) + res = Plotter._calc_motor_angled(0.0, 1.0) + self.assertNotEqual(0, res[2]) + + def test_calc6(self): + res = Plotter._calc_motor_angled(1.0, 0.2) + logging.debug("%s", res) + res = Plotter._calc_motor_angled(1.0, 0.5) + logging.debug("%s", res) + res = Plotter._calc_motor_angled(1.0, 0.8) + logging.debug("%s", res)