Experiment with angled movements

examples/plotter/__init__.py

@@ -2,7 +2,7 @@ import logging
 import math
 import time
 
-from pylgbst import MoveHub, ColorDistanceSensor, COLORS, COLOR_RED, COLOR_YELLOW
+from pylgbst import MoveHub, ColorDistanceSensor, COLORS, COLOR_RED, COLOR_CYAN
 
 
 class Plotter(MoveHub):
@@ -10,7 +10,7 @@ class Plotter(MoveHub):
 
     def __init__(self, connection=None, base_speed=1.0):
         super(Plotter, self).__init__(connection)
-        self.base_speed = base_speed
+        self.base_speed = float(base_speed)
         self.field_width = 3.55 / self.base_speed
         self.xpos = 0
         self.ypos = 0
@@ -35,7 +35,7 @@ class Plotter(MoveHub):
             self.caret.constant(-self.base_speed)
             count = 0
             max_tries = 50
-            while self._marker_color not in (COLOR_RED, COLOR_YELLOW) and count < max_tries:
+            while self._marker_color not in (COLOR_RED, COLOR_CYAN) and count < max_tries:
                 time.sleep(30.0 / max_tries)
                 count += 1
             self.color_distance_sensor.unsubscribe(self._on_distance)
@@ -47,16 +47,27 @@ class Plotter(MoveHub):
             self.caret.stop()
             self.color_distance_sensor.unsubscribe(self._on_distance)
 
-        if self._marker_color != COLOR_YELLOW:
+        if self._marker_color != COLOR_CYAN:
             self.move(-self.field_width, 0)
 
     def _on_distance(self, color, distance):
         self._marker_color = None
         logging.debug("Color: %s, distance %s", COLORS[color], distance)
-        if color in (COLOR_RED, COLOR_YELLOW):
+        if color in (COLOR_RED, COLOR_CYAN):
             if distance <= 3:
                 self._marker_color = color
 
+    def _compensate_wheels_backlash(self, movy):
+        """
+        corrects backlash of wheels gear system
+        """
+        if not movy:
+            return
+        wheel_dir = movy / abs(movy)
+        if wheel_dir == -self.__last_wheel_dir:
+            self.wheels.angled(270, -wheel_dir)
+        self.__last_wheel_dir = wheel_dir
+
     def finalize(self):
         if self.is_tool_down:
             self._tool_up()
@@ -96,27 +107,19 @@ class Plotter(MoveHub):
             logging.warning("No movement, ignored")
             return
 
-        self._correct_wheels(movy)
+        self._compensate_wheels_backlash(movy)
 
         self.xpos += movx
         self.ypos += movy
 
-        length, speed_a, speed_b = self._calc_motor(movx, movy)
+        length, speed_a, speed_b = self._calc_motor_timed(movx, movy)
 
         self.motor_AB.timed(length, -speed_a * self.base_speed / self.MOTOR_RATIO, -speed_b * self.base_speed)
 
         # time.sleep(0.5)
 
-    def _correct_wheels(self, movy):
-        if not movy:
-            return
-        wheel_dir = movy / abs(movy)
-        if wheel_dir == -self.__last_wheel_dir:
-            self.wheels.angled(230, -wheel_dir)
-        self.__last_wheel_dir = wheel_dir
-
     @staticmethod
-    def _calc_motor(movx, movy):
+    def _calc_motor_timed(movx, movy):
         amovx = float(abs(movx))
         amovy = float(abs(movy))
 
@@ -136,6 +139,11 @@ class Plotter(MoveHub):
 
         return length, speed_a, speed_b
 
+    @staticmethod
+    def _calc_motor_angled(movx, movy):
+        pass
+        #return length, speed_a, speed_b
+
     def circle(self, radius):
         if not self.is_tool_down:
             self._tool_down()

examples/plotter/try.py

@@ -218,6 +218,25 @@ def snowflake():
         plotter.line(item[0] * zoom, -item[1] * zoom)
 
 
+def angles_experiment():
+    plotter._tool_down()
+
+    path = 1000
+
+    parts = 5
+    for x in range(1, parts):
+        spd_b = x * plotter.base_speed / parts
+        spd_a = plotter.base_speed - spd_b
+
+        angle = path * (1.0 + spd_b / spd_a)
+        logging.info("%s, %s, %s", angle, spd_a, spd_b)
+
+        plotter.motor_AB.angled(angle, spd_a, -spd_b)
+        plotter._compensate_wheels_backlash(-1)
+        plotter.motor_AB.angled(-angle, spd_a, -spd_b)
+        plotter._compensate_wheels_backlash(1)
+
+
 if __name__ == '__main__':
     logging.basicConfig(level=logging.INFO)
 
@@ -227,13 +246,13 @@ if __name__ == '__main__':
         logging.warning("Failed to use debug server: %s", traceback.format_exc())
         conn = BLEConnection().connect()
 
-    plotter = LaserPlotter(conn, 0.75)
+    plotter = LaserPlotter(conn, 1.0)
     FIELD_WIDTH = plotter.field_width
 
     try:
-        plotter.initialize()
+        # plotter.initialize()
 
-        # plotter._tool_down()
+        angles_experiment()
 
         # try_speeds()
 
@@ -249,7 +268,7 @@ if __name__ == '__main__':
         # square_spiral()
         # lego()
         # christmas_tree()
-        snowflake()
+        # snowflake()
         pass
     finally:
         plotter.finalize()

pylgbst/peripherals.py

@@ -138,7 +138,8 @@ class LED(Peripheral):
 
 
 class EncodedMotor(Peripheral):
-    TRAILER = b'\x64\x7f\x03'  # NOTE: \x64 is 100, might mean something
+    TRAILER = b'\x64\x7f\x03'  # NOTE: \x64 is 100, might mean something; also trailer might be a sequence terminator
+    # TODO: investigate sequence behavior, seen with zero values passed to angled mode
     # trailer is not required for all movement types
     MOVEMENT_TYPE = 0x11
 

tests.py

@@ -196,16 +196,16 @@ class GeneralTest(unittest.TestCase):
 
 class TestPlotter(unittest.TestCase):
     def test_calc1(self):
-        self.assertEqual((100, 1, 0.5), Plotter._calc_motor(100, 50))
+        self.assertEqual((100, 1, 0.5), Plotter._calc_motor_timed(100, 50))
 
     def test_calc2(self):
-        self.assertEqual((200, 0.5, 1), Plotter._calc_motor(100, 200))
+        self.assertEqual((200, 0.5, 1), Plotter._calc_motor_timed(100, 200))
 
     def test_calc_xoverflow(self):
-        self.assertEqual((800, 0.0125, 1), Plotter._calc_motor(10, 800))
+        self.assertEqual((800, 0.0125, 1), Plotter._calc_motor_timed(10, 800))
 
     def test_calc3(self):
-        self.assertEqual((100, 1, 0), Plotter._calc_motor(100, 0))
+        self.assertEqual((100, 1, 0), Plotter._calc_motor_timed(100, 0))
 
     def test_calc4(self):
-        self.assertEqual((200, 0, 1), Plotter._calc_motor(0, 200))
+        self.assertEqual((200, 0, 1), Plotter._calc_motor_timed(0, 200))