From b600cc93c35aea9d1458e76f92788177ac23ad1e Mon Sep 17 00:00:00 2001
From: Andrey Pohilko <apc4@ya.ru>
Date: Tue, 19 Dec 2017 13:11:35 +0300
Subject: [PATCH] Struggle with math
---
examples/plotter/__init__.py | 117 +++++++++++++++++++++++------------
examples/plotter/try.py | 64 +++++++++++++++++++
pylgbst/movehub.py | 2 +
tests.py | 13 ++++
4 files changed, 156 insertions(+), 40 deletions(-)
create mode 100644 examples/plotter/try.py
diff --git a/examples/plotter/__init__.py b/examples/plotter/__init__.py
index 3894107..1336593 100644
--- a/examples/plotter/__init__.py
+++ b/examples/plotter/__init__.py
@@ -1,14 +1,12 @@
import logging
import sys
import time
-import traceback
-from pylgbst import MoveHub, BLEConnection
-from pylgbst.comms import DebugServerConnection
+from pylgbst import MoveHub
from pylgbst.peripherals import EncodedMotor
BASE_SPEED = 0.5
-CARET_WIDTH = -940
+CARET_WIDTH = 940
class Plotter(MoveHub):
@@ -20,26 +18,30 @@ class Plotter(MoveHub):
self.__last_rotation_value = sys.maxsize
def initialize(self):
- 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._reset_caret()
self.xpos = 0
self.ypos = 0
self.is_tool_down = False
+ def _reset_caret(self):
+ self.motor_A.timed(0.5, -0.3)
+ self.motor_A.subscribe(self._on_rotate, mode=EncodedMotor.SENSOR_SPEED)
+ self.motor_A.constant(0.3)
+ count = 0
+ max_tries = 50
+ while abs(self.__last_rotation_value) > 5 and count < max_tries:
+ logging.debug("Last rot: %s", self.__last_rotation_value)
+ time.sleep(10.0 / max_tries)
+ count += 1
+ logging.debug("Centering tries: %s, last value: %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)
+
def _on_rotate(self, value):
- logging.info("Rotation: %s", value)
+ logging.debug("Rotation: %s", value)
self.__last_rotation_value = value
def _tool_down(self):
@@ -50,6 +52,12 @@ class Plotter(MoveHub):
self.motor_external.angled(-270, BASE_SPEED)
self.is_tool_down = False
+ def finalize(self):
+ if self.is_tool_down:
+ self._tool_up()
+
+ self.move(-self.xpos, -self.ypos)
+
def move(self, movx, movy):
if self.is_tool_down:
self._tool_up()
@@ -61,29 +69,58 @@ class Plotter(MoveHub):
self._transfer_to(movx, movy)
def _transfer_to(self, movx, movy):
- angle = max(abs(movy), abs(movx))
- speed_a = BASE_SPEED
- speed_b = BASE_SPEED * 0.5
- self.motor_AB.angled(angle, speed_a, speed_b)
+ if self.xpos + movx < -CARET_WIDTH:
+ logging.warning("Invalid xpos: %s", self.xpos)
+ movx += self.xpos - CARET_WIDTH
- def finalize(self):
- if self.is_tool_down:
- self._tool_up()
+ if self.xpos + movx > CARET_WIDTH:
+ logging.warning("Invalid xpos: %s", self.xpos)
+ movx -= self.xpos - CARET_WIDTH
+ self.xpos -= self.xpos - CARET_WIDTH
+
+ if not movy and not movx:
+ logging.warning("No movement, ignored")
+ return
+
+ self.xpos += movx
+ self.ypos += movy
+
+ angle, speed_a, speed_b = calc_motor(movx, movy)
+
+ if not speed_b:
+ 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)
-if __name__ == '__main__':
- logging.basicConfig(level=logging.INFO)
+def calc_motor(movx, movy):
+ amovy = abs(movy)
+ amovx = abs(movx)
+ angle = max(amovx, amovy)
- try:
- conn = DebugServerConnection()
- except BaseException:
- logging.warning("Failed to use debug server: %s", traceback.format_exc())
- conn = BLEConnection().connect()
+ speed_a = (movx / float(amovx)) if amovx 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
+ else:
+ speed_a = (movx / float(amovy)) if movy else 0
- plotter = Plotter(conn)
- plotter.initialize()
- plotter.line(100, 100)
- plotter.line(100, -100)
- plotter.line(-100, -100)
- plotter.line(-100, 100)
- plotter.finalize()
+ if speed_a:
+ 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_b <= 1
+
+ return angle, speed_a, speed_b
diff --git a/examples/plotter/try.py b/examples/plotter/try.py
new file mode 100644
index 0000000..3b39edf
--- /dev/null
+++ b/examples/plotter/try.py
@@ -0,0 +1,64 @@
+import logging
+import traceback
+
+from examples.plotter import Plotter, CARET_WIDTH
+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():
+ plotter.move(CARET_WIDTH, CARET_WIDTH)
+ plotter.move(-CARET_WIDTH, -CARET_WIDTH)
+
+ plotter.move(CARET_WIDTH, 0)
+ plotter.move(-CARET_WIDTH, 0)
+ plotter.move(0, CARET_WIDTH)
+ plotter.move(0, -CARET_WIDTH)
+
+
+def square():
+ plotter.line(CARET_WIDTH, 0)
+ plotter.line(0, CARET_WIDTH)
+ plotter.line(-CARET_WIDTH, 0)
+ plotter.line(0, -CARET_WIDTH)
+
+
+def triangle():
+ plotter.line(CARET_WIDTH, 0)
+ plotter.line(0, CARET_WIDTH)
+ plotter.line(-CARET_WIDTH, -CARET_WIDTH)
+
+
+def romb():
+ plotter.move(-CARET_WIDTH * 2, 0)
+ plotter.line(CARET_WIDTH, CARET_WIDTH)
+ plotter.line(CARET_WIDTH, -CARET_WIDTH)
+ plotter.line(-CARET_WIDTH, -CARET_WIDTH)
+ plotter.line(-CARET_WIDTH, CARET_WIDTH)
+
+
+if __name__ == '__main__':
+ logging.basicConfig(level=logging.INFO)
+
+ try:
+ conn = DebugServerConnection()
+ except BaseException:
+ logging.warning("Failed to use debug server: %s", traceback.format_exc())
+ conn = BLEConnection().connect()
+
+ plotter = Plotter(conn)
+ #plotter.initialize()
+ # plotter._tool_up() # and plotter._tool_up()
+
+ triangle()
+ # moves()
+ # square()
+ # cross()
+ # romb()
+
+ plotter.finalize()
diff --git a/pylgbst/movehub.py b/pylgbst/movehub.py
index fd3142b..2b2b00f 100644
--- a/pylgbst/movehub.py
+++ b/pylgbst/movehub.py
@@ -81,6 +81,8 @@ class MoveHub(object):
raise RuntimeError("Failed to obtain all builtin devices")
def _notify(self, handle, data):
+ # 1b0e000500823701
+
orig = data
if handle != MOVE_HUB_HARDWARE_HANDLE:
diff --git a/tests.py b/tests.py
index b5d7cd6..0bf0835 100644
--- a/tests.py
+++ b/tests.py
@@ -1,6 +1,7 @@
import unittest
from binascii import unhexlify
+from examples.plotter import calc_motor
from pylgbst import *
from pylgbst.comms import Connection
from pylgbst.movehub import MoveHub
@@ -191,3 +192,15 @@ class GeneralTest(unittest.TestCase):
hub.notify_mock.append((HANDLE, notify))
Thread(target=inject).start()
+
+
+class TestPlotter(unittest.TestCase):
+ def test_calc(self):
+ self.assertEqual((100, 0, 0.24), calc_motor(100, 100))
+ self.assertEqual((100, 0, 0.24), calc_motor(-100, -100))
+ self.assertEqual((100, 0, 0.24), calc_motor(0, 100))
+ self.assertEqual((100, 0.75, 0), calc_motor(100, 0))
+ self.assertEqual((100, -0.75, 0), calc_motor(-100, 0))
+ self.assertEqual((100, 0, -0.24), calc_motor(0, -100))
+ self.assertEqual((100, 0.75, 0.12), calc_motor(100, 50))
+ self.assertEqual((100, 0.375, 0.24), calc_motor(50, 100))