Struggle with math

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

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()

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:

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))