From 374eb9876565272c041e1d6950e7547d0604dffa Mon Sep 17 00:00:00 2001
From: Kevin O'Connor <kevin@koconnor.net>
Date: Sun, 27 Oct 2019 11:14:40 -0400
Subject: [PATCH] polar: Implement bed angle wrapping adjustments in chelper
 code

Signed-off-by: Kevin O'Connor <kevin@koconnor.net>
---
 klippy/chelper/itersolve.c |  2 ++
 klippy/chelper/itersolve.h |  2 ++
 klippy/chelper/kin_polar.c | 16 ++++++++++++++--
 klippy/kinematics/polar.py |  9 +--------
 4 files changed, 19 insertions(+), 10 deletions(-)

diff --git a/klippy/chelper/itersolve.c b/klippy/chelper/itersolve.c
index 5cb3fffc..2949b9c2 100644
--- a/klippy/chelper/itersolve.c
+++ b/klippy/chelper/itersolve.c
@@ -206,6 +206,8 @@ itersolve_gen_steps(struct stepper_kinematics *sk, struct move *m)
     }
     queue_append_finish(qa);
     sk->commanded_pos = last.position;
+    if (sk->post_cb)
+        sk->post_cb(sk);
     return 0;
 }
 
diff --git a/klippy/chelper/itersolve.h b/klippy/chelper/itersolve.h
index 08447fa6..23743498 100644
--- a/klippy/chelper/itersolve.h
+++ b/klippy/chelper/itersolve.h
@@ -32,10 +32,12 @@ struct coord move_get_coord(struct move *m, double move_time);
 struct stepper_kinematics;
 typedef double (*sk_calc_callback)(struct stepper_kinematics *sk, struct move *m
                                    , double move_time);
+typedef void (*sk_post_callback)(struct stepper_kinematics *sk);
 struct stepper_kinematics {
     double step_dist, commanded_pos;
     struct stepcompress *sc;
     sk_calc_callback calc_position_cb;
+    sk_post_callback post_cb;
 };
 
 int32_t itersolve_gen_steps(struct stepper_kinematics *sk, struct move *m);
diff --git a/klippy/chelper/kin_polar.c b/klippy/chelper/kin_polar.c
index bba5546a..77755d70 100644
--- a/klippy/chelper/kin_polar.c
+++ b/klippy/chelper/kin_polar.c
@@ -32,14 +32,26 @@ polar_stepper_angle_calc_position(struct stepper_kinematics *sk, struct move *m
     return angle;
 }
 
+static void
+polar_stepper_angle_post_fixup(struct stepper_kinematics *sk)
+{
+    // Normalize the stepper_bed angle
+    if (sk->commanded_pos < -M_PI)
+        sk->commanded_pos += 2 * M_PI;
+    else if (sk->commanded_pos > M_PI)
+        sk->commanded_pos -= 2 * M_PI;
+}
+
 struct stepper_kinematics * __visible
 polar_stepper_alloc(char type)
 {
     struct stepper_kinematics *sk = malloc(sizeof(*sk));
     memset(sk, 0, sizeof(*sk));
-    if (type == 'r')
+    if (type == 'r') {
         sk->calc_position_cb = polar_stepper_radius_calc_position;
-    else if (type == 'a')
+    } else if (type == 'a') {
         sk->calc_position_cb = polar_stepper_angle_calc_position;
+        sk->post_cb = polar_stepper_angle_post_fixup;
+    }
     return sk;
 }
diff --git a/klippy/kinematics/polar.py b/klippy/kinematics/polar.py
index 230e67f6..97de0a9d 100644
--- a/klippy/kinematics/polar.py
+++ b/klippy/kinematics/polar.py
@@ -120,14 +120,7 @@ class PolarKinematics:
         cmove = move.cmove
         if axes_d[0] or axes_d[1]:
             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)
+            self.steppers[0].step_itersolve(cmove)
         if axes_d[2]:
             self.rails[1].step_itersolve(cmove)
     def get_status(self):