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

Lego word demo

Browse Source
This commit is contained in:
Andrey Pohilko 2017-12-20 21:28:16 +03:00
parent 6d691b988f
commit 0496bb6dbd
4 changed files with 113 additions and 39 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
examples/plotter/__init__.py
View File

@ -4,14 +4,14 @@ import time
from pylgbst import MoveHub, ColorDistanceSensor, COLORS, COLOR_RED, COLOR_CYAN 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): 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) super(Plotter, self).__init__(connection)
self.base_speed = base_speed
self.field_width = 0.925 / 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
@ -24,15 +24,15 @@ class Plotter(MoveHub):
self.is_tool_down = False self.is_tool_down = False
def _reset_caret(self): 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, self.color_distance_sensor.subscribe(self._on_distance, mode=ColorDistanceSensor.COLOR_DISTANCE_FLOAT,
granularity=5) granularity=5)
try: try:
self.motor_A.constant(-BASE_SPEED) self.motor_A.constant(-self.base_speed)
count = 0 count = 0
max_tries = 50 max_tries = 50
while not self._marker_color and count < max_tries: 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 count += 1
logging.info("Centering tries: %s, color #%s", count, logging.info("Centering tries: %s, color #%s", count,
COLORS[self._marker_color] if self._marker_color else None) COLORS[self._marker_color] if self._marker_color else None)
@ -42,7 +42,7 @@ class Plotter(MoveHub):
self.motor_A.stop() self.motor_A.stop()
self.color_distance_sensor.unsubscribe(self._on_distance) 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): def _on_distance(self, color, distance):
self._marker_color = None self._marker_color = None
@ -76,14 +76,14 @@ class Plotter(MoveHub):
self._transfer_to(movx, movy) self._transfer_to(movx, movy)
def _transfer_to(self, 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) 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) logging.warning("Invalid xpos: %s", self.xpos)
movx -= self.xpos - FIELD_WIDTH movx -= self.xpos - self.field_width
self.xpos -= self.xpos - FIELD_WIDTH self.xpos -= self.xpos - self.field_width
if not movy and not movx: if not movy and not movx:
logging.warning("No movement, ignored") logging.warning("No movement, ignored")
@ -92,14 +92,14 @@ class Plotter(MoveHub):
self.xpos += movx self.xpos += movx
self.ypos += movy 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) # time.sleep(0.5)
@staticmethod @staticmethod
def calc_motor(movx, movy): def _calc_motor(movx, movy):
amovx = float(abs(movx)) amovx = float(abs(movx))
amovy = float(abs(movy)) amovy = float(abs(movy))
@ -108,10 +108,10 @@ class Plotter(MoveHub):
speed_a = (movx / float(amovx)) if amovx else 0.0 speed_a = (movx / float(amovx)) if amovx else 0.0
speed_b = (movy / float(amovy)) if amovy else 0.0 speed_b = (movy / float(amovy)) if amovy else 0.0
if amovx >= amovy * MOTOR_RATIO: if amovx >= amovy * Plotter.MOTOR_RATIO:
speed_b = movy / amovx * MOTOR_RATIO speed_b = movy / amovx * Plotter.MOTOR_RATIO
else: 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) logging.info("Motor: %s with %s/%s", length, speed_a, speed_b)
assert -1 <= speed_a <= 1 assert -1 <= speed_a <= 1
@ -135,7 +135,10 @@ class Plotter(MoveHub):
speeds.append((0, 0)) speeds.append((0, 0))
for speed_a, speed_b in speeds: 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) time.sleep(dur)
def spiral(self, rounds, growth): def spiral(self, rounds, growth):
@ -157,5 +160,8 @@ class Plotter(MoveHub):
speeds.append((0, 0, 0)) speeds.append((0, 0, 0))
for speed_a, speed_b, dur in speeds: 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) time.sleep(dur)

2
examples/plotter/imgtracer.py
View File

@ -21,7 +21,7 @@ class Tracer(object):
self.dst.fill(False) self.dst.fill(False)
self.mark = numpy.copy(self.dst) self.mark = numpy.copy(self.dst)
# start in center # 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.posy = self.height / 2
self.posx = self.width / 2 self.posx = self.width / 2
self.lines = [] self.lines = []

90
examples/plotter/try.py
View File

@ -1,9 +1,8 @@
import logging import logging
import time
import traceback import traceback
import time from examples.plotter import Plotter
from examples.plotter import Plotter, FIELD_WIDTH
from pylgbst.comms import DebugServerConnection, BLEConnection from pylgbst.comms import DebugServerConnection, BLEConnection
@ -45,10 +44,10 @@ def romb():
def circles(): def circles():
plotter.move(FIELD_WIDTH / 2.0, 0) plotter.move(FIELD_WIDTH / 4.0, 0)
plotter.circle(FIELD_WIDTH / 2.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) plotter.circle(FIELD_WIDTH)
@ -59,6 +58,73 @@ class LaserPlotter(Plotter):
time.sleep(1) 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__': if __name__ == '__main__':
logging.basicConfig(level=logging.INFO) logging.basicConfig(level=logging.INFO)
@ -68,7 +134,8 @@ if __name__ == '__main__':
logging.warning("Failed to use debug server: %s", traceback.format_exc()) logging.warning("Failed to use debug server: %s", traceback.format_exc())
conn = BLEConnection().connect() conn = BLEConnection().connect()
plotter = LaserPlotter(conn) plotter = LaserPlotter(conn, 0.4)
FIELD_WIDTH = plotter.field_width
try: try:
# plotter._tool_down() # plotter._tool_down()
@ -76,12 +143,13 @@ if __name__ == '__main__':
plotter.initialize() plotter.initialize()
# moves() # moves()
#triangle() # triangle()
#square() # square()
#cross() # cross()
# romb() # romb()
#circles() # circles()
plotter.spiral(4, 0.02) # plotter.spiral(4, 0.02)
lego()
pass pass
finally: finally:
plotter.finalize() plotter.finalize()

10
tests.py
View File

@ -196,16 +196,16 @@ class GeneralTest(unittest.TestCase):
class TestPlotter(unittest.TestCase): class TestPlotter(unittest.TestCase):
def test_calc1(self): 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): 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): 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): 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): def test_calc4(self):
self.assertEqual((400, 0, 1), Plotter.calc_motor(0, 200)) self.assertEqual((400, 0, 1), Plotter._calc_motor(0, 200))