kine

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,
 };