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

We're close to success

Browse Source
This commit is contained in:
Andrey Pohilko 2017-12-25 22:19:45 +03:00
parent c6582103c9
commit 16fabe0e28
2 changed files with 107 additions and 46 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

98
examples/plotter/__init__.py
View File

@ -2,23 +2,28 @@ import logging
import math import math
import time import time
from pylgbst import MoveHub, ColorDistanceSensor, COLORS, COLOR_RED, COLOR_CYAN from pylgbst import ColorDistanceSensor, COLORS, COLOR_RED, COLOR_CYAN
class Plotter(MoveHub): class Plotter(object):
MOTOR_RATIO = 1.65 MOTOR_RATIO = 1.65
def __init__(self, connection=None, base_speed=1.0): def __init__(self, hub, base_speed=1.0):
super(Plotter, self).__init__(connection) """
:type hub: pylgbst.movehub.MoveHub
"""
self._hub = hub
self.caret = self._hub.motor_A
self.wheels = self._hub.motor_B
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 = 3.55 / 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
self._marker_color = False self._marker_color = False
self.caret = self.motor_A
self.wheels = self.motor_B
self.both = self.motor_AB
self.__last_wheel_dir = 1 self.__last_wheel_dir = 1
def initialize(self): def initialize(self):
@ -28,8 +33,12 @@ class Plotter(MoveHub):
self.is_tool_down = False self.is_tool_down = False
def _reset_caret(self): def _reset_caret(self):
if not self._hub.color_distance_sensor:
logging.warning("No color/distance sensor, cannot center caret")
return
self.caret.timed(0.5, self.base_speed) self.caret.timed(0.5, self.base_speed)
self.color_distance_sensor.subscribe(self._on_distance, mode=ColorDistanceSensor.COLOR_DISTANCE_FLOAT, self._hub.color_distance_sensor.subscribe(self._on_distance, mode=ColorDistanceSensor.COLOR_DISTANCE_FLOAT,
granularity=1) granularity=1)
try: try:
self.caret.constant(-self.base_speed) self.caret.constant(-self.base_speed)
@ -38,14 +47,14 @@ class Plotter(MoveHub):
while self._marker_color not in (COLOR_RED, COLOR_CYAN) and count < max_tries: while self._marker_color not in (COLOR_RED, COLOR_CYAN) and count < max_tries:
time.sleep(30.0 / max_tries) time.sleep(30.0 / max_tries)
count += 1 count += 1
self.color_distance_sensor.unsubscribe(self._on_distance) self._hub.color_distance_sensor.unsubscribe(self._on_distance)
clr = COLORS[self._marker_color] if self._marker_color else None clr = COLORS[self._marker_color] if self._marker_color else None
logging.info("Centering tries: %s, color #%s", count, clr) logging.info("Centering tries: %s, color #%s", count, clr)
if count >= max_tries: if count >= max_tries:
raise RuntimeError("Failed to center caret") raise RuntimeError("Failed to center caret")
finally: finally:
self.caret.stop() self.caret.stop()
self.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) self.move(-self.field_width, 0)
@ -71,16 +80,14 @@ class Plotter(MoveHub):
def finalize(self): def finalize(self):
if self.is_tool_down: if self.is_tool_down:
self._tool_up() self._tool_up()
self.motor_AB.stop(async=True) self.both.stop(async=True)
if self.motor_external:
self.motor_external.stop(async=True)
def _tool_down(self): def _tool_down(self):
self.motor_external.angled(-270, 1)
self.is_tool_down = True self.is_tool_down = True
self._hub.motor_external.angled(-270, 1)
def _tool_up(self): def _tool_up(self):
self.motor_external.angled(270, 1) self._hub.motor_external.angled(270, 1)
self.is_tool_down = False self.is_tool_down = False
def move(self, movx, movy): def move(self, movx, movy):
@ -94,29 +101,33 @@ 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 < -self.field_width:
logging.warning("Invalid xpos: %s", self.xpos)
movx += self.xpos - self.field_width
if self.xpos + movx > self.field_width:
logging.warning("Invalid xpos: %s", self.xpos)
movx -= self.xpos - self.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")
return return
self._compensate_wheels_backlash(movy) if self.xpos + movx < -self.field_width:
logging.warning("Invalid xpos: %s", self.xpos)
# movx += self.xpos - self.field_width
if self.xpos + movx > self.field_width:
logging.warning("Invalid xpos: %s", self.xpos)
# movx -= self.xpos - self.field_width
# self.xpos -= self.xpos - self.field_width
# 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_timed(movx, movy) length, speed_a, speed_b = self._calc_motor_angled(movx, movy)
logging.info("Motors: %.3f with %.3f/%.3f", length, speed_a, speed_b)
self.motor_AB.timed(length, -speed_a * self.base_speed / self.MOTOR_RATIO, -speed_b * self.base_speed) if not speed_b:
self.caret.angled(length, -speed_a * self.base_speed)
# time.sleep(0.5) elif not speed_a:
self.wheels.angled(length, -speed_b * self.base_speed)
else:
self.both.angled(length, -speed_a * self.base_speed, -speed_b * self.base_speed)
@staticmethod @staticmethod
def _calc_motor_timed(movx, movy): def _calc_motor_timed(movx, movy):
@ -133,7 +144,6 @@ class Plotter(MoveHub):
else: else:
speed_a = movx / amovy speed_a = movx / amovy
logging.info("Motor: %s with %s/%s", length, speed_a, speed_b)
assert -1 <= speed_a <= 1 assert -1 <= speed_a <= 1
assert -1 <= speed_b <= 1 assert -1 <= speed_b <= 1
@ -141,8 +151,28 @@ class Plotter(MoveHub):
@staticmethod @staticmethod
def _calc_motor_angled(movx, movy): def _calc_motor_angled(movx, movy):
pass amovx = abs(movx)
#return length, speed_a, speed_b 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)
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)
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
def circle(self, radius): def circle(self, radius):
if not self.is_tool_down: if not self.is_tool_down:
@ -163,7 +193,7 @@ class Plotter(MoveHub):
spa = speed_a * self.base_speed spa = speed_a * self.base_speed
spb = -speed_b * self.base_speed * self.MOTOR_RATIO spb = -speed_b * self.base_speed * self.MOTOR_RATIO
logging.info("Motor speeds: %.3f / %.3f", spa, spb) logging.info("Motor speeds: %.3f / %.3f", spa, spb)
self.motor_AB.constant(spa, spb) self.both.constant(spa, spb)
time.sleep(dur) time.sleep(dur)
def spiral(self, rounds, growth): def spiral(self, rounds, growth):
@ -187,7 +217,7 @@ class Plotter(MoveHub):
for speed_a, speed_b, dur in speeds: for speed_a, speed_b, dur in speeds:
spa = speed_a * self.base_speed spa = speed_a * self.base_speed
spb = -speed_b * self.base_speed * self.MOTOR_RATIO spb = -speed_b * self.base_speed * self.MOTOR_RATIO
self.motor_AB.constant(spa, spb) self.both.constant(spa, spb)
logging.info("Motor speeds: %.3f / %.3f", spa, spb) logging.info("Motor speeds: %.3f / %.3f", spa, spb)
time.sleep(dur) time.sleep(dur)

49
examples/plotter/try.py
View File

@ -3,7 +3,9 @@ import time
import traceback import traceback
from examples.plotter import Plotter from examples.plotter import Plotter
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
def moves(): def moves():
@ -160,11 +162,11 @@ def try_speeds():
speeds = [x * 1.0 / 10.0 for x in range(1, 11)] speeds = [x * 1.0 / 10.0 for x in range(1, 11)]
for s in speeds: for s in speeds:
logging.info("%s", s) logging.info("%s", s)
plotter.motor_AB.constant(s, -s) plotter.both.constant(s, -s)
time.sleep(1) time.sleep(1)
for s in reversed(speeds): for s in reversed(speeds):
logging.info("%s", s) logging.info("%s", s)
plotter.motor_AB.constant(-s, s) plotter.both.constant(-s, s)
time.sleep(1) time.sleep(1)
@ -219,12 +221,22 @@ def snowflake():
def angles_experiment(): def angles_experiment():
plotter._tool_down() parts = 5
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):
movx = x * 1.0 / parts
plotter.line(movx, 1.0)
plotter.move(-movx, -1.0)
logging.info("%s", movx)
"""
path = 1000 path = 1000
parts = 5
for x in range(1, parts):
spd_b = x * plotter.base_speed / parts spd_b = x * plotter.base_speed / parts
spd_a = plotter.base_speed - spd_b spd_a = plotter.base_speed - spd_b
@ -235,10 +247,26 @@ def angles_experiment():
plotter._compensate_wheels_backlash(-1) plotter._compensate_wheels_backlash(-1)
plotter.motor_AB.angled(-angle, spd_a, -spd_b) plotter.motor_AB.angled(-angle, spd_a, -spd_b)
plotter._compensate_wheels_backlash(1) plotter._compensate_wheels_backlash(1)
"""
class MotorMock(EncodedMotor):
def _wait_sync(self, async):
super(MotorMock, self)._wait_sync(True)
def get_hub_mock():
hub = HubMock()
hub.motor_A = MotorMock(hub, PORT_A)
hub.motor_B = MotorMock(hub, PORT_B)
hub.motor_AB = MotorMock(hub, PORT_AB)
hub.motor_external = MotorMock(hub, PORT_C)
return hub
if __name__ == '__main__': if __name__ == '__main__':
logging.basicConfig(level=logging.INFO) logging.basicConfig(level=logging.DEBUG)
logging.getLogger('').setLevel(logging.INFO)
try: try:
conn = DebugServerConnection() conn = DebugServerConnection()
@ -246,13 +274,16 @@ 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, 1.0) hub = MoveHub(conn) if 1 else get_hub_mock()
plotter = LaserPlotter(hub, 1.0)
FIELD_WIDTH = plotter.field_width FIELD_WIDTH = plotter.field_width
try: try:
# plotter.initialize() # plotter._tool_up()
plotter.initialize()
angles_experiment() #angles_experiment()
# try_speeds() # try_speeds()