v/Lego-Boost-Python-Library
1
0
Fork 0
mirror of https://github.com/undera/pylgbst.git synced 2020-11-18 19:37:26 -08:00
Code Issues Releases Wiki Activity

Angled works fine

Browse Source
This commit is contained in:
Andrey Pohilko 2017-12-26 18:14:12 +03:00
parent d19cab08d8
commit 1dac915fbe
3 changed files with 67 additions and 35 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

46
examples/plotter/__init__.py
View File

@ -6,7 +6,8 @@ from pylgbst import ColorDistanceSensor, COLORS, COLOR_RED, COLOR_CYAN
class Plotter(object): class Plotter(object):
MOTOR_RATIO = 1.65 MOTOR_RATIO = 1.45
ROTATE_UNIT = 2100
def __init__(self, hub, base_speed=1.0): def __init__(self, hub, base_speed=1.0):
""" """
@ -19,7 +20,7 @@ class Plotter(object):
self.both = self._hub.motor_AB self.both = self._hub.motor_AB
self.base_speed = float(base_speed) 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.xpos = 0
self.ypos = 0 self.ypos = 0
self.is_tool_down = False self.is_tool_down = False
@ -28,6 +29,8 @@ class Plotter(object):
def initialize(self): def initialize(self):
self._reset_caret() self._reset_caret()
self._compensate_wheels_backlash(1)
self._compensate_wheels_backlash(-1)
self.xpos = 0 self.xpos = 0
self.ypos = 0 self.ypos = 0
self.is_tool_down = False self.is_tool_down = False
@ -37,9 +40,8 @@ class Plotter(object):
logging.warning("No color/distance sensor, cannot center caret") logging.warning("No color/distance sensor, cannot center caret")
return return
self._hub.color_distance_sensor.subscribe(self._on_distance, mode=ColorDistanceSensor.COLOR_DISTANCE_FLOAT)
self.caret.timed(0.5, self.base_speed) 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: try:
self.caret.constant(-self.base_speed) self.caret.constant(-self.base_speed)
count = 0 count = 0
@ -56,7 +58,9 @@ class Plotter(object):
self.caret.stop() self.caret.stop()
self._hub.color_distance_sensor.unsubscribe(self._on_distance) 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) self.move(-self.field_width, 0)
def _on_distance(self, color, distance): def _on_distance(self, color, distance):
@ -114,18 +118,18 @@ class Plotter(object):
# movx -= self.xpos - self.field_width # movx -= self.xpos - self.field_width
# self.xpos -= 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.xpos += movx
self.ypos += movy 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) logging.info("Motors: %.3f with %.3f/%.3f", length, speed_a, speed_b)
if not 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: 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: else:
self.both.angled(length, -speed_a * self.base_speed, -speed_b * self.base_speed) self.both.angled(length, -speed_a * self.base_speed, -speed_b * self.base_speed)
@ -155,21 +159,24 @@ class Plotter(object):
amovy = abs(movy) amovy = abs(movy)
if amovx >= amovy: if amovx >= amovy:
ax = amovy / (amovx + amovy) ax = amovy / (amovx + amovy)
spd_a = ax spd_b = ax
spd_b = (1.0 - spd_a) if spd_b < 0.05:
rotate = 1500 * amovx * (1.0 + spd_a / spd_b) 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) logging.info("A: movx=%s, movy=%s, ax=%s", movx, movy, ax)
else: else:
ax = amovx / (amovx + amovy) ax = amovx / (amovx + amovy)
spd_b = ax spd_a = ax
spd_a = (1.0 - spd_b) if spd_a < 0.05:
rotate = 1500 * amovy * (1.0 + spd_b / spd_a) 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) logging.info("B: movx=%s, movy=%s, ax=%s", movx, movy, ax)
assert 0 <= spd_a <= 1 assert 0 <= spd_a <= 1
assert 0 <= spd_b <= 1 assert 0 <= spd_b <= 1
assert rotate < 6000 # safety valve
spd_a *= (movx / amovx) if amovx else 0 spd_a *= (movx / amovx) if amovx else 0
spd_b *= (movy / amovy) if amovy else 0 spd_b *= (movy / amovy) if amovy else 0
return rotate, spd_a, spd_b return rotate, spd_a, spd_b
@ -237,3 +244,10 @@ class Plotter(object):
self.line(0, step) self.line(0, step)
self.line(width * flip, 0) self.line(width * flip, 0)
flip = -flip flip = -flip
class LaserPlotter(Plotter):
def _tool_down(self):
super(LaserPlotter, self)._tool_down()
time.sleep(1.0)

30
examples/plotter/try.py
View File

@ -2,7 +2,7 @@ import logging
import time import time
import traceback 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 import EncodedMotor, PORT_AB, PORT_C, PORT_A, PORT_B, MoveHub
from pylgbst.comms import DebugServerConnection, BLEConnection from pylgbst.comms import DebugServerConnection, BLEConnection
from tests import HubMock from tests import HubMock
@ -55,13 +55,6 @@ def circles():
plotter.circle(FIELD_WIDTH) plotter.circle(FIELD_WIDTH)
class LaserPlotter(Plotter):
def _tool_down(self):
super(LaserPlotter, self)._tool_down()
time.sleep(1)
def lego(): def lego():
t = FIELD_WIDTH / 5.0 t = FIELD_WIDTH / 5.0
h = t * 5.0 h = t * 5.0
@ -133,7 +126,7 @@ def lego():
def square_spiral(): def square_spiral():
rounds = 7 rounds = 7
step = FIELD_WIDTH / 4.0 / rounds step = plotter.base_speed / rounds / 3.0
for r in range(1, rounds): for r in range(1, rounds):
plotter.line(step * r * 4, 0) plotter.line(step * r * 4, 0)
plotter.line(0, step * (r * 4 + 1)) plotter.line(0, step * (r * 4 + 1))
@ -170,7 +163,7 @@ def try_speeds():
time.sleep(1) time.sleep(1)
def snowflake(): def snowflake(scale=1.0):
a = [300, 232, a = [300, 232,
351, 144, 351, 144,
307, 68, 307, 68,
@ -206,7 +199,7 @@ def snowflake():
vals = [(x[0] / float(maxval), x[1] / float(maxval)) for x in vals] vals = [(x[0] / float(maxval), x[1] / float(maxval)) for x in vals]
logging.info("Moves: %s", vals) logging.info("Moves: %s", vals)
zoom = FIELD_WIDTH * 2.0 zoom = FIELD_WIDTH * scale
for item in vals: for item in vals:
plotter.line(item[0] * zoom, item[1] * zoom) plotter.line(item[0] * zoom, item[1] * zoom)
@ -221,14 +214,14 @@ def snowflake():
def angles_experiment(): def angles_experiment():
parts = 5 parts = 2
for x in range(0, parts + 1): for x in range(0, parts + 1):
movy = x * 1.0 / parts movy = x * 1.0 / parts
plotter.line(1.0, movy) plotter.line(1.0, movy)
plotter.move(-1.0, -movy) plotter.move(-1.0, -movy)
logging.info("%s", movy) logging.info("%s", movy)
for x in range(0, parts + 1): for x in range(0, parts):
movx = x * 1.0 / parts movx = x * 1.0 / parts
plotter.line(movx, 1.0) plotter.line(movx, 1.0)
plotter.move(-movx, -1.0) plotter.move(-movx, -1.0)
@ -276,13 +269,14 @@ if __name__ == '__main__':
hub = MoveHub(conn) if 1 else get_hub_mock() hub = MoveHub(conn) if 1 else get_hub_mock()
plotter = LaserPlotter(hub, 1.0) plotter = LaserPlotter(hub, 0.4)
FIELD_WIDTH = plotter.field_width FIELD_WIDTH = plotter.field_width * 0.9
try: try:
# plotter._tool_up()
plotter.initialize() plotter.initialize()
# plotter._tool_down()
# angles_experiment() # angles_experiment()
# try_speeds() # try_speeds()
@ -296,10 +290,10 @@ if __name__ == '__main__':
# plotter.spiral(4, 0.02) # plotter.spiral(4, 0.02)
# plotter.rectangle(FIELD_WIDTH / 5.0, FIELD_WIDTH / 5.0, solid=True) # plotter.rectangle(FIELD_WIDTH / 5.0, FIELD_WIDTH / 5.0, solid=True)
# square_spiral() square_spiral()
# lego() # lego()
# christmas_tree() # christmas_tree()
# snowflake() # snowflake(0.5)
pass pass
finally: finally:
plotter.finalize() plotter.finalize()

24
tests.py
View File

@ -209,3 +209,27 @@ class TestPlotter(unittest.TestCase):
def test_calc4(self): def test_calc4(self):
self.assertEqual((200, 0, 1), Plotter._calc_motor_timed(0, 200)) 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)