From ae0341dc879af36200475940a58b5c363614734b Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?PROFERIS=20-=20Mi=C2=B3osz=20Stocki?=
 <milosz.stocki@proferis.pl>
Date: Wed, 8 Jul 2026 16:38:38 +0200
Subject: [PATCH] kine

---
 web/js/kinematics.js            | 16 +++++++++++++---
 web/js/sections/stepper-test.js |  4 ++--
 2 files changed, 15 insertions(+), 5 deletions(-)

diff --git a/web/js/kinematics.js b/web/js/kinematics.js
index daa1944..2ea6d62 100644
--- a/web/js/kinematics.js
+++ b/web/js/kinematics.js
@@ -319,10 +319,20 @@ function lookup(char) {
 
 // ── Unit converters ───────────────────────────────────────────────
 function stepsToRad(steps) {
-  return (steps / ARM.STEPS_PER_REV) * 2 * Math.PI;
+  // Physical motors are wired such that + steps is CW. 
+  // We negate here so UI math maps correctly to hardware.
+  return -(steps / ARM.STEPS_PER_REV) * 2 * Math.PI;
 }
 function radToSteps(rad) {
-  return Math.round((rad / (2 * Math.PI)) * ARM.STEPS_PER_REV);
+  return Math.round(-(rad / (2 * Math.PI)) * ARM.STEPS_PER_REV);
+}
+
+function radToStepsClosest(rad, currentSteps) {
+  let target = radToSteps(rad);
+  // Ensure we take the shortest path instead of winding up the motors
+  while (target - currentSteps > 1024) target -= 2048;
+  while (target - currentSteps < -1024) target += 2048;
+  return target;
 }
 function stepsToDeg(steps) {
   return steps * ARM.STEP_ANGLE_DEG;
@@ -334,5 +344,5 @@ function radToDeg(rad) {
 export default {
   ARM, LIMITS, LOOKUP_TABLE,
   solve, forward, armsCrossed, checkWorkspace, lookup,
-  stepsToRad, radToSteps, stepsToDeg, radToDeg,
+  stepsToRad, radToSteps, radToStepsClosest, stepsToDeg, radToDeg,
 };
diff --git a/web/js/sections/stepper-test.js b/web/js/sections/stepper-test.js
index 6eea721..e820974 100644
--- a/web/js/sections/stepper-test.js
+++ b/web/js/sections/stepper-test.js
@@ -195,8 +195,8 @@ async function moveToXY(targetX, targetY) {
 
   const { theta1, theta2 } = IK.solve(targetX, targetY);
 
-  const newSteps1 = IK.radToSteps(theta1);
-  const newSteps2 = IK.radToSteps(theta2);
+  const newSteps1 = IK.radToStepsClosest(theta1, steps1);
+  const newSteps2 = IK.radToStepsClosest(theta2, steps2);
 
   const cmd = `X:${newSteps1},${newSteps2}`;