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

Init works

Browse Source
This commit is contained in:
Andrey Pohilko 2017-12-18 19:40:53 +03:00
parent 258c259042
commit a6cb301688
1 changed files with 36 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

50
examples/plotter/__init__.py
View File

@ -1,10 +1,14 @@
import logging import logging
import sys
import time
import traceback import traceback
from pylgbst import MoveHub, BLEConnection from pylgbst import MoveHub, BLEConnection
from pylgbst.comms import DebugServerConnection from pylgbst.comms import DebugServerConnection
from pylgbst.peripherals import EncodedMotor
BASE_SPEED = 0.5 BASE_SPEED = 0.5
CARET_WIDTH = -940
class Plotter(MoveHub): class Plotter(MoveHub):
@ -13,35 +17,53 @@ class Plotter(MoveHub):
self.xpos = 0 self.xpos = 0
self.ypos = 0 self.ypos = 0
self.is_tool_down = False self.is_tool_down = False
self.__last_rotation_value = sys.maxsize
def initialize(self): def initialize(self):
# TODO: ensure caret is in the middle self.motor_A.subscribe(self._on_rotate, mode=EncodedMotor.SENSOR_SPEED)
self.motor_A.constant(0.4)
count = 0
max_tries = 50
while self.__last_rotation_value > 5 and count < max_tries:
logging.info("Last rot: %s", self.__last_rotation_value)
time.sleep(5.0 / max_tries)
count += 1
logging.info("Tries: %s, last: %s", count, self.__last_rotation_value)
self.motor_A.unsubscribe(self._on_rotate)
self.motor_A.stop()
if count >= max_tries:
raise RuntimeError("Failed to center caret")
self.motor_A.angled(CARET_WIDTH, BASE_SPEED)
self.xpos = 0 self.xpos = 0
self.ypos = 0 self.ypos = 0
self.is_tool_down = False self.is_tool_down = False
def _on_rotate(self, value):
logging.info("Rotation: %s", value)
self.__last_rotation_value = value
def _tool_down(self): def _tool_down(self):
self.motor_external.angled(270) self.motor_external.angled(270, BASE_SPEED)
self.is_tool_down = True self.is_tool_down = True
def _tool_up(self): def _tool_up(self):
self.motor_external.angled(-270) self.motor_external.angled(-270, BASE_SPEED)
self.is_tool_down = False self.is_tool_down = False
def move_to(self, movx, movy): def move(self, movx, movy):
if self.is_tool_down: if self.is_tool_down:
self._tool_up() self._tool_up()
self._transfer_to(movx, movy) self._transfer_to(movx, movy)
def line_to(self, movx, movy): def line(self, movx, movy):
if not self.is_tool_down: if not self.is_tool_down:
self._tool_down() self._tool_down()
self._transfer_to(movx, movy) self._transfer_to(movx, movy)
def _transfer_to(self, movx, movy): def _transfer_to(self, movx, movy):
angle = max(movy, movx) angle = max(abs(movy), abs(movx))
speed_a = BASE_SPEED speed_a = BASE_SPEED
speed_b = BASE_SPEED * 1.5 speed_b = BASE_SPEED * 0.5
self.motor_AB.angled(angle, speed_a, speed_b) self.motor_AB.angled(angle, speed_a, speed_b)
def finalize(self): def finalize(self):
@ -53,15 +75,15 @@ if __name__ == '__main__':
logging.basicConfig(level=logging.INFO) logging.basicConfig(level=logging.INFO)
try: try:
connection = DebugServerConnection() conn = DebugServerConnection()
except BaseException: except BaseException:
logging.warning("Failed to use debug server: %s", traceback.format_exc()) logging.warning("Failed to use debug server: %s", traceback.format_exc())
connection = BLEConnection().connect() conn = BLEConnection().connect()
plotter = Plotter(connection) plotter = Plotter(conn)
plotter.initialize() plotter.initialize()
# plotter.move_to(100, 100) plotter.line(100, 100)
# plotter.move_to(100, -100) plotter.line(100, -100)
# plotter.move_to(-100, -100) plotter.line(-100, -100)
# plotter.move_to(-100, 100) plotter.line(-100, 100)
plotter.finalize() plotter.finalize()