diff --git a/examples/plotter/__init__.py b/examples/plotter/__init__.py index 929c766..3894107 100644 --- a/examples/plotter/__init__.py +++ b/examples/plotter/__init__.py @@ -1,10 +1,14 @@ import logging +import sys +import time import traceback from pylgbst import MoveHub, BLEConnection from pylgbst.comms import DebugServerConnection +from pylgbst.peripherals import EncodedMotor BASE_SPEED = 0.5 +CARET_WIDTH = -940 class Plotter(MoveHub): @@ -13,35 +17,53 @@ class Plotter(MoveHub): self.xpos = 0 self.ypos = 0 self.is_tool_down = False + self.__last_rotation_value = sys.maxsize 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.ypos = 0 self.is_tool_down = False + def _on_rotate(self, value): + logging.info("Rotation: %s", value) + self.__last_rotation_value = value + def _tool_down(self): - self.motor_external.angled(270) + self.motor_external.angled(270, BASE_SPEED) self.is_tool_down = True def _tool_up(self): - self.motor_external.angled(-270) + self.motor_external.angled(-270, BASE_SPEED) self.is_tool_down = False - def move_to(self, movx, movy): + def move(self, movx, movy): if self.is_tool_down: self._tool_up() self._transfer_to(movx, movy) - def line_to(self, movx, movy): + def line(self, movx, movy): if not self.is_tool_down: self._tool_down() self._transfer_to(movx, movy) def _transfer_to(self, movx, movy): - angle = max(movy, movx) + angle = max(abs(movy), abs(movx)) 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) def finalize(self): @@ -53,15 +75,15 @@ if __name__ == '__main__': logging.basicConfig(level=logging.INFO) try: - connection = DebugServerConnection() + conn = DebugServerConnection() except BaseException: 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.move_to(100, 100) - # plotter.move_to(100, -100) - # plotter.move_to(-100, -100) - # plotter.move_to(-100, 100) + plotter.line(100, 100) + plotter.line(100, -100) + plotter.line(-100, -100) + plotter.line(-100, 100) plotter.finalize()