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):
MOTOR_RATIO = 1.65
MOTOR_RATIO = 1.45
ROTATE_UNIT = 2100
def __init__(self, hub, base_speed=1.0):
"""
@ -19,7 +20,7 @@ class Plotter(object):
self.both = self._hub.motor_AB
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.ypos = 0
self.is_tool_down = False
@ -28,6 +29,8 @@ class Plotter(object):
def initialize(self):
self._reset_caret()
self._compensate_wheels_backlash(1)
self._compensate_wheels_backlash(-1)
self.xpos = 0
self.ypos = 0
self.is_tool_down = False
@ -37,9 +40,8 @@ class Plotter(object):
logging.warning("No color/distance sensor, cannot center caret")
return
self._hub.color_distance_sensor.subscribe(self._on_distance, mode=ColorDistanceSensor.COLOR_DISTANCE_FLOAT)
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:
self.caret.constant(-self.base_speed)
count = 0
@ -56,7 +58,9 @@ class Plotter(object):
self.caret.stop()
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)
def _on_distance(self, color, distance):
@ -114,18 +118,18 @@ class Plotter(object):
# movx -= 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.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)
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:
self.wheels.angled(length, -speed_b * self.base_speed)
self.wheels.angled(length * 2.0, -speed_b * self.base_speed)
else:
self.both.angled(length, -speed_a * self.base_speed, -speed_b * self.base_speed)
@ -155,21 +159,24 @@ class Plotter(object):
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)
spd_b = ax
if spd_b < 0.05:
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)
else:
ax = amovx / (amovx + amovy)
spd_b = ax
spd_a = (1.0 - spd_b)
rotate = 1500 * amovy * (1.0 + spd_b / spd_a)
spd_a = ax
if spd_a < 0.05:
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)
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
@ -237,3 +244,10 @@ class Plotter(object):
self.line(0, step)
self.line(width * flip, 0)
flip = -flip
class LaserPlotter(Plotter):
def _tool_down(self):
super(LaserPlotter, self)._tool_down()
time.sleep(1.0)

32
examples/plotter/try.py
View File

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

24
tests.py
View File

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