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

Reworked hw part

Browse Source
This commit is contained in:
Andrey Pohilko 2017-12-23 19:12:51 +03:00
parent ab9e829d97
commit 4a5880de20
2 changed files with 38 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

30
examples/plotter/__init__.py
View File

@ -6,16 +6,19 @@ from pylgbst import MoveHub, ColorDistanceSensor, COLORS, COLOR_RED, COLOR_CYAN
class Plotter(MoveHub): class Plotter(MoveHub):
MOTOR_RATIO = 1.15 MOTOR_RATIO = 1.65
def __init__(self, connection=None, base_speed=1.0): 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.base_speed = base_speed
self.field_width = 0.925 / self.base_speed self.field_width = 2.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
self._marker_color = False self._marker_color = False
self.caret = self.motor_A
self.wheels = self.motor_B
self.both = self.motor_AB
def initialize(self): def initialize(self):
self._reset_caret() self._reset_caret()
@ -24,25 +27,25 @@ 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.5, 0.4) self.caret.timed(0.5, 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=1) granularity=1)
try: try:
self.motor_A.constant(-0.4) self.caret.constant(-self.base_speed)
count = 0 count = 0
max_tries = 50 max_tries = 50
while self._marker_color != COLOR_RED and count < max_tries: while self._marker_color != COLOR_RED and count < max_tries:
time.sleep(7.0 / max_tries) time.sleep(30.0 / 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)
if count >= max_tries: if count >= max_tries:
raise RuntimeError("Failed to center caret") raise RuntimeError("Failed to center caret")
finally: finally:
self.motor_A.stop() self.caret.stop()
self.color_distance_sensor.unsubscribe(self._on_distance) self.color_distance_sensor.unsubscribe(self._on_distance)
self.motor_A.timed(0.925 / 0.4, 0.5) self.caret.timed(0.925 / 0.4, self.base_speed)
def _on_distance(self, color, distance): def _on_distance(self, color, distance):
self._marker_color = None self._marker_color = None
@ -52,10 +55,11 @@ class Plotter(MoveHub):
self._marker_color = color self._marker_color = color
def finalize(self): def finalize(self):
self.motor_AB.stop()
self.motor_external.stop()
if self.is_tool_down: if self.is_tool_down:
self._tool_up() self._tool_up()
self.motor_AB.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.motor_external.angled(-270, 1)
@ -94,7 +98,7 @@ class Plotter(MoveHub):
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 * self.base_speed, -speed_b * self.base_speed) self.motor_AB.timed(length, -speed_a * self.base_speed / self.MOTOR_RATIO, -speed_b * self.base_speed)
# time.sleep(0.5) # time.sleep(0.5)
@ -108,10 +112,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 * Plotter.MOTOR_RATIO: if amovx >= amovy:
speed_b = movy / amovx * Plotter.MOTOR_RATIO speed_b = movy / amovx
else: else:
speed_a = movx / amovy / Plotter.MOTOR_RATIO speed_a = movx / amovy
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

26
examples/plotter/try.py
View File

@ -152,6 +152,18 @@ def christmas_tree():
plotter.line(t, -t) plotter.line(t, -t)
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)
time.sleep(1)
for s in reversed(speeds):
logging.info("%s", s)
plotter.motor_AB.constant(-s, s)
time.sleep(1)
if __name__ == '__main__': if __name__ == '__main__':
logging.basicConfig(level=logging.INFO) logging.basicConfig(level=logging.INFO)
@ -161,25 +173,29 @@ 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, 0.15) plotter = LaserPlotter(conn, 0.5)
FIELD_WIDTH = plotter.field_width FIELD_WIDTH = plotter.field_width
try: try:
#plotter.initialize()
# plotter._tool_down() # plotter._tool_down()
# plotter._tool_up()
plotter.initialize() # try_speeds()
# moves() # moves()
# triangle() triangle()
# square() # square()
# cross() # cross()
# romb() # romb()
# circles() # circles()
# 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)
# lego() # lego()
# square_spiral() # square_spiral()
christmas_tree() # christmas_tree()
pass pass
finally: finally:
plotter.finalize() plotter.finalize()