mirror of
https://github.com/undera/pylgbst.git
synced 2020-11-18 19:37:26 -08:00
68 lines
1.7 KiB
Python
68 lines
1.7 KiB
Python
import logging
|
|
import traceback
|
|
|
|
from pylgbst import MoveHub, BLEConnection
|
|
from pylgbst.comms import DebugServerConnection
|
|
|
|
BASE_SPEED = 0.5
|
|
|
|
|
|
class Plotter(MoveHub):
|
|
def __init__(self, connection=None):
|
|
super(Plotter, self).__init__(connection)
|
|
self.xpos = 0
|
|
self.ypos = 0
|
|
self.is_tool_down = False
|
|
|
|
def initialize(self):
|
|
# TODO: ensure caret is in the middle
|
|
self.xpos = 0
|
|
self.ypos = 0
|
|
self.is_tool_down = False
|
|
|
|
def _tool_down(self):
|
|
self.motor_external.angled(270)
|
|
self.is_tool_down = True
|
|
|
|
def _tool_up(self):
|
|
self.motor_external.angled(-270)
|
|
self.is_tool_down = False
|
|
|
|
def move_to(self, movx, movy):
|
|
if self.is_tool_down:
|
|
self._tool_up()
|
|
self._transfer_to(movx, movy)
|
|
|
|
def line_to(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)
|
|
speed_a = BASE_SPEED
|
|
speed_b = BASE_SPEED * 1.5
|
|
self.motor_AB.angled(angle, speed_a, speed_b)
|
|
|
|
def finalize(self):
|
|
if self.is_tool_down:
|
|
self._tool_up()
|
|
|
|
|
|
if __name__ == '__main__':
|
|
logging.basicConfig(level=logging.INFO)
|
|
|
|
try:
|
|
connection = DebugServerConnection()
|
|
except BaseException:
|
|
logging.warning("Failed to use debug server: %s", traceback.format_exc())
|
|
connection = BLEConnection().connect()
|
|
|
|
plotter = Plotter(connection)
|
|
plotter.initialize()
|
|
# plotter.move_to(100, 100)
|
|
# plotter.move_to(100, -100)
|
|
# plotter.move_to(-100, -100)
|
|
# plotter.move_to(-100, 100)
|
|
plotter.finalize()
|