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

REctangle and romb

Browse Source
This commit is contained in:
Andrey Pohilko 2017-12-19 19:31:30 +03:00
parent b600cc93c3
commit 184950a1c5
4 changed files with 76 additions and 86 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

64
examples/plotter/__init__.py
View File

@ -5,8 +5,8 @@ import time
from pylgbst import MoveHub from pylgbst import MoveHub
from pylgbst.peripherals import EncodedMotor from pylgbst.peripherals import EncodedMotor
BASE_SPEED = 0.5 BASE_SPEED = 0.75
CARET_WIDTH = 940 FIELD_WIDTH = 1.1
class Plotter(MoveHub): class Plotter(MoveHub):
@ -24,9 +24,9 @@ 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.3) self.motor_A.timed(0.5, -0.2)
self.motor_A.subscribe(self._on_rotate, mode=EncodedMotor.SENSOR_SPEED) self.motor_A.subscribe(self._on_rotate, mode=EncodedMotor.SENSOR_SPEED)
self.motor_A.constant(0.3) self.motor_A.constant(0.2)
count = 0 count = 0
max_tries = 50 max_tries = 50
while abs(self.__last_rotation_value) > 5 and count < max_tries: while abs(self.__last_rotation_value) > 5 and count < max_tries:
@ -38,18 +38,18 @@ class Plotter(MoveHub):
self.motor_A.stop() self.motor_A.stop()
if count >= max_tries: if count >= max_tries:
raise RuntimeError("Failed to center caret") raise RuntimeError("Failed to center caret")
self.motor_A.angled(-CARET_WIDTH, BASE_SPEED) self.motor_A.timed(FIELD_WIDTH, -BASE_SPEED)
def _on_rotate(self, value): def _on_rotate(self, value):
logging.debug("Rotation: %s", value) logging.debug("Rotation: %s", value)
self.__last_rotation_value = value self.__last_rotation_value = value
def _tool_down(self): def _tool_down(self):
self.motor_external.angled(270, BASE_SPEED) self.motor_external.angled(270, 1)
self.is_tool_down = True self.is_tool_down = True
def _tool_up(self): def _tool_up(self):
self.motor_external.angled(-270, BASE_SPEED) self.motor_external.angled(-270, 1)
self.is_tool_down = False self.is_tool_down = False
def finalize(self): def finalize(self):
@ -69,14 +69,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 < -CARET_WIDTH: if self.xpos + movx < -FIELD_WIDTH:
logging.warning("Invalid xpos: %s", self.xpos) logging.warning("Invalid xpos: %s", self.xpos)
movx += self.xpos - CARET_WIDTH movx += self.xpos - FIELD_WIDTH
if self.xpos + movx > CARET_WIDTH: if self.xpos + movx > FIELD_WIDTH:
logging.warning("Invalid xpos: %s", self.xpos) logging.warning("Invalid xpos: %s", self.xpos)
movx -= self.xpos - CARET_WIDTH movx -= self.xpos - FIELD_WIDTH
self.xpos -= self.xpos - CARET_WIDTH self.xpos -= self.xpos - FIELD_WIDTH
if not movy and not movx: if not movy and not movx:
logging.warning("No movement, ignored") logging.warning("No movement, ignored")
@ -85,42 +85,30 @@ class Plotter(MoveHub):
self.xpos += movx self.xpos += movx
self.ypos += movy self.ypos += movy
angle, speed_a, speed_b = calc_motor(movx, movy) length, speed_a, speed_b = self.calc_motor(movx, movy)
if not speed_b: self.motor_AB.timed(length, -speed_a * BASE_SPEED, -speed_b * BASE_SPEED)
self.motor_A.angled(angle, speed_a * BASE_SPEED)
elif not speed_a:
self.motor_B.angled(angle, speed_b * BASE_SPEED)
else:
self.motor_AB.angled(angle, speed_a * BASE_SPEED, speed_b * BASE_SPEED)
# time.sleep(0.5) # time.sleep(0.5)
@staticmethod
def calc_motor(movx, movy):
motor_ratio = 1.15
amovx = float(abs(movx))
amovy = float(abs(movy))
def calc_motor(movx, movy): length = max(amovx, amovy)
amovy = abs(movy)
amovx = abs(movx)
angle = max(amovx, amovy)
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:
speed_b = (movy / float(amovx)) if movx else 0 if amovx >= amovy * motor_ratio:
speed_b = movy / amovx * motor_ratio
else: else:
speed_a = (movx / float(amovy)) if movy else 0 speed_a = movx / amovy / motor_ratio
if speed_a: logging.info("Motor: %s with %s/%s", length, speed_a, speed_b)
speed_b *= 2.75
else:
angle *= 1.5
norm = max(abs(speed_a), abs(speed_b))
speed_a /= norm
speed_b /= norm
angle *= speed_a
logging.info("Motor: %s with %s/%s", angle, speed_a, speed_b)
assert -1 <= speed_a <= 1 assert -1 <= speed_a <= 1
assert -1 <= speed_b <= 1 assert -1 <= speed_b <= 1
return angle, speed_a, speed_b return length, speed_a, speed_b

63
examples/plotter/try.py
View File

@ -1,49 +1,49 @@
import logging import logging
import traceback import traceback
from examples.plotter import Plotter, CARET_WIDTH from examples.plotter import Plotter, FIELD_WIDTH
from pylgbst.comms import DebugServerConnection, BLEConnection from pylgbst.comms import DebugServerConnection, BLEConnection
def cross():
plotter.line(CARET_WIDTH, CARET_WIDTH)
plotter.move(-CARET_WIDTH, 0)
plotter.line(CARET_WIDTH, -CARET_WIDTH)
def moves(): def moves():
plotter.move(CARET_WIDTH, CARET_WIDTH) plotter.move(FIELD_WIDTH, FIELD_WIDTH)
plotter.move(-CARET_WIDTH, -CARET_WIDTH) plotter.move(-FIELD_WIDTH, -FIELD_WIDTH)
plotter.move(CARET_WIDTH, 0) plotter.move(FIELD_WIDTH, 0)
plotter.move(-CARET_WIDTH, 0) plotter.move(-FIELD_WIDTH, 0)
plotter.move(0, CARET_WIDTH) plotter.move(0, FIELD_WIDTH)
plotter.move(0, -CARET_WIDTH) plotter.move(0, -FIELD_WIDTH)
def cross():
plotter.line(FIELD_WIDTH, FIELD_WIDTH)
plotter.move(-FIELD_WIDTH, 0)
plotter.line(FIELD_WIDTH, -FIELD_WIDTH)
def square(): def square():
plotter.line(CARET_WIDTH, 0) plotter.line(FIELD_WIDTH, 0)
plotter.line(0, CARET_WIDTH) plotter.line(0, FIELD_WIDTH)
plotter.line(-CARET_WIDTH, 0) plotter.line(-FIELD_WIDTH, 0)
plotter.line(0, -CARET_WIDTH) plotter.line(0, -FIELD_WIDTH)
def triangle(): def triangle():
plotter.line(CARET_WIDTH, 0) plotter.line(FIELD_WIDTH, 0)
plotter.line(0, CARET_WIDTH) plotter.line(0, FIELD_WIDTH)
plotter.line(-CARET_WIDTH, -CARET_WIDTH) plotter.line(-FIELD_WIDTH, -FIELD_WIDTH)
def romb(): def romb():
plotter.move(-CARET_WIDTH * 2, 0) plotter.move(-FIELD_WIDTH, 0)
plotter.line(CARET_WIDTH, CARET_WIDTH) plotter.line(FIELD_WIDTH, FIELD_WIDTH)
plotter.line(CARET_WIDTH, -CARET_WIDTH) plotter.line(FIELD_WIDTH, -FIELD_WIDTH)
plotter.line(-CARET_WIDTH, -CARET_WIDTH) plotter.line(-FIELD_WIDTH, -FIELD_WIDTH)
plotter.line(-CARET_WIDTH, CARET_WIDTH) plotter.line(-FIELD_WIDTH, FIELD_WIDTH)
if __name__ == '__main__': if __name__ == '__main__':
logging.basicConfig(level=logging.INFO) logging.basicConfig(level=logging.DEBUG)
try: try:
conn = DebugServerConnection() conn = DebugServerConnection()
@ -52,13 +52,12 @@ if __name__ == '__main__':
conn = BLEConnection().connect() conn = BLEConnection().connect()
plotter = Plotter(conn) plotter = Plotter(conn)
#plotter.initialize() plotter.initialize()
# plotter._tool_up() # and plotter._tool_up()
triangle()
# moves() # moves()
# square() # triangle()
# cross() square()
# romb() cross()
#romb()
plotter.finalize() plotter.finalize()

2
pylgbst/movehub.py
View File

@ -81,8 +81,6 @@ class MoveHub(object):
raise RuntimeError("Failed to obtain all builtin devices") raise RuntimeError("Failed to obtain all builtin devices")
def _notify(self, handle, data): def _notify(self, handle, data):
# 1b0e000500823701
orig = data orig = data
if handle != MOVE_HUB_HARDWARE_HANDLE: if handle != MOVE_HUB_HARDWARE_HANDLE:

25
tests.py
View File

@ -1,7 +1,7 @@
import unittest import unittest
from binascii import unhexlify from binascii import unhexlify
from examples.plotter import calc_motor from examples.plotter import Plotter
from pylgbst import * from pylgbst import *
from pylgbst.comms import Connection from pylgbst.comms import Connection
from pylgbst.movehub import MoveHub from pylgbst.movehub import MoveHub
@ -195,12 +195,17 @@ class GeneralTest(unittest.TestCase):
class TestPlotter(unittest.TestCase): class TestPlotter(unittest.TestCase):
def test_calc(self): def test_calc1(self):
self.assertEqual((100, 0, 0.24), calc_motor(100, 100)) self.assertEqual((100, 1, 1), Plotter.calc_motor(100, 50))
self.assertEqual((100, 0, 0.24), calc_motor(-100, -100))
self.assertEqual((100, 0, 0.24), calc_motor(0, 100)) def test_calc2(self):
self.assertEqual((100, 0.75, 0), calc_motor(100, 0)) self.assertEqual((400, 0.25, 1), Plotter.calc_motor(100, 200))
self.assertEqual((100, -0.75, 0), calc_motor(-100, 0))
self.assertEqual((100, 0, -0.24), calc_motor(0, -100)) def test_calc_xoverflow(self):
self.assertEqual((100, 0.75, 0.12), calc_motor(100, 50)) self.assertEqual((400, 0.25, 1), Plotter.calc_motor(10, 900))
self.assertEqual((100, 0.375, 0.24), calc_motor(50, 100))
def test_calc3(self):
self.assertEqual((100, 1, 0), Plotter.calc_motor(100, 0))
def test_calc4(self):
self.assertEqual((400, 0, 1), Plotter.calc_motor(0, 200))