3 files changed, 18 insertions, 2 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;
 }