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

100
examples/plotter/__init__.py
View File

@ -2,23 +2,28 @@ import logging
import math
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
def __init__(self, connection=None, base_speed=1.0):
super(Plotter, self).__init__(connection)
def __init__(self, hub, base_speed=1.0):
"""
: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.field_width = 3.55 / self.base_speed
self.xpos = 0
self.ypos = 0
self.is_tool_down = 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
def initialize(self):
@ -28,9 +33,13 @@ class Plotter(MoveHub):
self.is_tool_down = False
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.color_distance_sensor.subscribe(self._on_distance, mode=ColorDistanceSensor.COLOR_DISTANCE_FLOAT,
granularity=1)
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
@ -38,14 +47,14 @@ class Plotter(MoveHub):
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)
self._hub.color_distance_sensor.unsubscribe(self._on_distance)
clr = COLORS[self._marker_color] if self._marker_color else None
logging.info("Centering tries: %s, color #%s", count, clr)
if count >= max_tries:
raise RuntimeError("Failed to center caret")
finally:
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:
self.move(-self.field_width, 0)
@ -71,16 +80,14 @@ class Plotter(MoveHub):
def finalize(self):
if self.is_tool_down:
self._tool_up()
self.motor_AB.stop(async=True)
if self.motor_external:
self.motor_external.stop(async=True)
self.both.stop(async=True)
def _tool_down(self):
self.motor_external.angled(-270, 1)
self.is_tool_down = True
self._hub.motor_external.angled(-270, 1)
def _tool_up(self):
self.motor_external.angled(270, 1)
self._hub.motor_external.angled(270, 1)
self.is_tool_down = False
def move(self, movx, movy):
@ -94,29 +101,33 @@ class Plotter(MoveHub):
self._transfer_to(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:
logging.warning("No movement, ignored")
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.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)
# time.sleep(0.5)
if not speed_b:
self.caret.angled(length, -speed_a * self.base_speed)
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
def _calc_motor_timed(movx, movy):
@ -133,7 +144,6 @@ class Plotter(MoveHub):
else:
speed_a = movx / amovy
logging.info("Motor: %s with %s/%s", length, speed_a, speed_b)
assert -1 <= speed_a <= 1
assert -1 <= speed_b <= 1
@ -141,8 +151,28 @@ class Plotter(MoveHub):
@staticmethod
def _calc_motor_angled(movx, movy):
pass
#return length, speed_a, speed_b
amovx = abs(movx)
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):
if not self.is_tool_down:
@ -163,7 +193,7 @@ class Plotter(MoveHub):
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)
self.both.constant(spa, spb)
time.sleep(dur)
def spiral(self, rounds, growth):
@ -187,7 +217,7 @@ class Plotter(MoveHub):
for speed_a, speed_b, dur in speeds:
spa = speed_a * self.base_speed
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)
time.sleep(dur)

53
examples/plotter/try.py
View File

@ -3,7 +3,9 @@ import time
import traceback
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 tests import HubMock
def moves():
@ -160,11 +162,11 @@ def try_speeds():
speeds = [x * 1.0 / 10.0 for x in range(1, 11)]
for s in speeds:
logging.info("%s", s)
plotter.motor_AB.constant(s, -s)
plotter.both.constant(s, -s)
time.sleep(1)
for s in reversed(speeds):
logging.info("%s", s)
plotter.motor_AB.constant(-s, s)
plotter.both.constant(-s, s)
time.sleep(1)
@ -219,12 +221,22 @@ def snowflake():
def angles_experiment():
plotter._tool_down()
path = 1000
parts = 5
for x in range(1, parts):
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
spd_b = x * plotter.base_speed / parts
spd_a = plotter.base_speed - spd_b
@ -235,10 +247,26 @@ def angles_experiment():
plotter._compensate_wheels_backlash(-1)
plotter.motor_AB.angled(-angle, spd_a, -spd_b)
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__':
logging.basicConfig(level=logging.INFO)
logging.basicConfig(level=logging.DEBUG)
logging.getLogger('').setLevel(logging.INFO)
try:
conn = DebugServerConnection()
@ -246,13 +274,16 @@ if __name__ == '__main__':
logging.warning("Failed to use debug server: %s", traceback.format_exc())
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
try:
# plotter.initialize()
# plotter._tool_up()
plotter.initialize()
angles_experiment()
#angles_experiment()
# try_speeds()