From b3dc409e3364d5486091001dd484e2fb4cebc224 Mon Sep 17 00:00:00 2001
From: Andrey Pohilko <apc4@ya.ru>
Date: Wed, 20 Dec 2017 14:17:20 +0300
Subject: [PATCH] Spiral works fine

---
 examples/plotter/__init__.py | 55 ++++++++++++++++++++++++------------
 examples/plotter/try.py      |  5 ++--
 2 files changed, 40 insertions(+), 20 deletions(-)

diff --git a/examples/plotter/__init__.py b/examples/plotter/__init__.py
index 0f86a07..4debbb0 100644
--- a/examples/plotter/__init__.py
+++ b/examples/plotter/__init__.py
@@ -16,11 +16,8 @@ class Plotter(MoveHub):
         self.ypos = 0
         self.is_tool_down = False
         self._marker_color = False
-        self._stop_on_marker = False
 
     def initialize(self):
-        self.color_distance_sensor.subscribe(self._on_distance, mode=ColorDistanceSensor.COLOR_DISTANCE_FLOAT,
-                                             granularity=10)
         self._reset_caret()
         self.xpos = 0
         self.ypos = 0
@@ -28,17 +25,22 @@ class Plotter(MoveHub):
 
     def _reset_caret(self):
         self.motor_A.timed(0.2, BASE_SPEED)
-        self.motor_A.constant(-BASE_SPEED)
-        count = 0
-        max_tries = 50
-        while not self._marker_color and count < max_tries:
-            time.sleep(5.0 / max_tries)
-            count += 1
+        self.color_distance_sensor.subscribe(self._on_distance, mode=ColorDistanceSensor.COLOR_DISTANCE_FLOAT,
+                                             granularity=5)
+        try:
+            self.motor_A.constant(-BASE_SPEED)
+            count = 0
+            max_tries = 50
+            while not self._marker_color and count < max_tries:
+                time.sleep(5.0 / max_tries)
+                count += 1
+            logging.debug("Centering tries: %s", count)
+            if count >= max_tries:
+                raise RuntimeError("Failed to center caret")
+        finally:
+            self.motor_A.stop()
+            self.color_distance_sensor.unsubscribe(self._on_distance)
 
-        logging.debug("Centering tries: %s", count)
-        self.motor_A.stop()
-        if count >= max_tries:
-            raise RuntimeError("Failed to center caret")
         self.motor_A.timed(FIELD_WIDTH, BASE_SPEED)
 
     def _on_distance(self, color, distance):
@@ -47,9 +49,6 @@ class Plotter(MoveHub):
         if color in (COLOR_RED, COLOR_CYAN):
             if distance <= 3:
                 self._marker_color = color
-                if self._stop_on_marker:
-                    logging.info("Stopping motor because of marker: %s", COLORS[color])
-                    self.motor_AB.stop()
 
     def finalize(self):
         self.motor_AB.stop()
@@ -57,8 +56,6 @@ class Plotter(MoveHub):
         if self.is_tool_down:
             self._tool_up()
 
-        self.color_distance_sensor.unsubscribe(self._on_distance)
-
     def _tool_down(self):
         self.motor_external.angled(270, 1)
         self.is_tool_down = True
@@ -139,3 +136,25 @@ class Plotter(MoveHub):
         for speed_a, speed_b in speeds:
             self.motor_AB.constant(speed_a * BASE_SPEED, -speed_b * BASE_SPEED * MOTOR_RATIO)
             time.sleep(dur)
+
+    def spiral(self, rounds, growth):
+        if not self.is_tool_down:
+            self._tool_down()
+
+        dur = 0.00
+        parts = 12
+        speeds = []
+        for r in range(0, rounds):
+            logging.info("Round: %s", r)
+
+            for x in range(0, parts):
+                speed_a = math.sin(x * 2.0 * math.pi / float(parts))
+                speed_b = math.cos(x * 2.0 * math.pi / float(parts))
+                dur += growth
+                speeds.append((speed_a, speed_b, dur))
+                logging.debug("A: %s, B: %s", speed_a, speed_b)
+        speeds.append((0, 0, 0))
+
+        for speed_a, speed_b, dur in speeds:
+            self.motor_AB.constant(speed_a * BASE_SPEED, -speed_b * BASE_SPEED * MOTOR_RATIO)
+            time.sleep(dur)
diff --git a/examples/plotter/try.py b/examples/plotter/try.py
index 74cafaf..7457f9e 100644
--- a/examples/plotter/try.py
+++ b/examples/plotter/try.py
@@ -62,7 +62,7 @@ if __name__ == '__main__':
     plotter = Plotter(conn)
 
     try:
-        #plotter._tool_up()
+        # plotter._tool_up()
         plotter.initialize()
 
         # moves()
@@ -70,7 +70,8 @@ if __name__ == '__main__':
         # square()
         # cross()
         # romb()
-        circles()
+        # circles()
+        plotter.spiral(4, 0.01)
         pass
     finally:
         plotter.finalize()