From e5f74a64ed814bec1bdff11a50566d8f40229216 Mon Sep 17 00:00:00 2001
From: Kevin O'Connor <kevin@koconnor.net>
Date: Thu, 20 Jun 2019 15:44:45 -0400
Subject: [PATCH] polar: Add initial support for multiple bed rotations

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
---
 config/example-polar.cfg   |  4 +---
 klippy/chelper/kin_polar.c | 10 ++++++---
 klippy/kinematics/polar.py | 11 ++++++++--
 test/klippy/polar.test     | 42 ++++++++++++++++++++++++++++++++++++++
 4 files changed, 59 insertions(+), 8 deletions(-)

diff --git a/config/example-polar.cfg b/config/example-polar.cfg
index 3e5c1f62..3c68c8dd 100644
--- a/config/example-polar.cfg
+++ b/config/example-polar.cfg
@@ -3,9 +3,7 @@
 # polar printer.
 
 # POLAR KINEMATICS ARE A WORK IN PROGRESS. Moves around the 0,0
-# position are known to not work properly. Moves to a negative Y
-# coordinate from a positive Y coordinate (and vice-versa) when the
-# head is at a negative X coordinate also do not work properly.
+# position are known to not work properly.
 
 # Only parameters unique to polar printers are described here - see
 # the "example.cfg" file for description of common config parameters.
diff --git a/klippy/chelper/kin_polar.c b/klippy/chelper/kin_polar.c
index cc1f2eab..d1a00273 100644
--- a/klippy/chelper/kin_polar.c
+++ b/klippy/chelper/kin_polar.c
@@ -1,6 +1,6 @@
 // Polar kinematics stepper pulse time generation
 //
-// Copyright (C) 2018  Kevin O'Connor <kevin@koconnor.net>
+// Copyright (C) 2018-2019  Kevin O'Connor <kevin@koconnor.net>
 //
 // This file may be distributed under the terms of the GNU GPLv3 license.
 
@@ -24,8 +24,12 @@ polar_stepper_angle_calc_position(struct stepper_kinematics *sk, struct move *m
 {
     struct coord c = move_get_coord(m, move_time);
     // XXX - handle x==y==0
-    // XXX - handle angle wrapping
-    return atan2(c.y, c.x);
+    double angle = atan2(c.y, c.x);
+    if (angle - sk->commanded_pos > M_PI)
+        angle -= 2. * M_PI;
+    else if (angle - sk->commanded_pos < -M_PI)
+        angle += 2. * M_PI;
+    return angle;
 }
 
 struct stepper_kinematics * __visible
diff --git a/klippy/kinematics/polar.py b/klippy/kinematics/polar.py
index 2930152c..70b314eb 100644
--- a/klippy/kinematics/polar.py
+++ b/klippy/kinematics/polar.py
@@ -1,6 +1,6 @@
 # Code for handling the kinematics of polar robots
 #
-# Copyright (C) 2018  Kevin O'Connor <kevin@koconnor.net>
+# Copyright (C) 2018-2019  Kevin O'Connor <kevin@koconnor.net>
 #
 # This file may be distributed under the terms of the GNU GPLv3 license.
 import logging, math
@@ -122,8 +122,15 @@ class PolarKinematics:
         axes_d = move.axes_d
         cmove = move.cmove
         if axes_d[0] or axes_d[1]:
-            self.steppers[0].step_itersolve(cmove)
             self.rails[0].step_itersolve(cmove)
+            stepper_bed = self.steppers[0]
+            stepper_bed.step_itersolve(cmove)
+            # Normalize the stepper_bed angle
+            angle = stepper_bed.get_commanded_position()
+            if angle < -math.pi:
+                stepper_bed.set_commanded_position(angle + 2. * math.pi)
+            elif angle > math.pi:
+                stepper_bed.set_commanded_position(angle - 2. * math.pi)
         if axes_d[2]:
             self.rails[1].step_itersolve(cmove)
 
diff --git a/test/klippy/polar.test b/test/klippy/polar.test
index 040531ce..96346db9 100644
--- a/test/klippy/polar.test
+++ b/test/klippy/polar.test
@@ -30,3 +30,45 @@ G1 E0
 
 ; regular extrude move
 G1 X10 Y0 E.01
+
+; Multiple rotations
+g1 X10 Y10
+g1 X-10 Y10
+g1 X-10 Y-10
+g1 X10 Y-10
+
+g1 X10 Y15
+g1 X-10 Y15
+g1 X-10 Y-15
+g1 X10 Y-15
+
+g1 X10 Y20
+g1 X-10 Y20
+g1 X-10 Y-20
+g1 X10 Y-20
+
+g1 X10 Y25
+g1 X-10 Y25
+g1 X-10 Y-25
+g1 X10 Y-25
+
+; Multiple rotations in reverse direction
+g1 X-15 Y-25
+g1 X-15 Y25
+g1 X15 Y25
+g1 X15 Y-25
+
+g1 X-20 Y-25
+g1 X-20 Y25
+g1 X20 Y25
+g1 X20 Y-25
+
+g1 X-25 Y-25
+g1 X-25 Y25
+g1 X25 Y25
+g1 X25 Y-25
+
+g1 X-30 Y-25
+g1 X-30 Y25
+g1 X30 Y25
+g1 X30 Y-25