more kiniematics

test_math.js

@@ -0,0 +1,39 @@
+const ARM = { d2: 12.9, l1: 85.0, l2: 110.0, STEPS_PER_REV: 2048 };
+const HOME_STEPS = { m1: 1024, m2: 0 };
+
+function solve(x, y) {
+  const { d2, l1, l2 } = ARM;
+  const xpd = x + d2;
+  const t = Math.sqrt(xpd * xpd + y * y);
+  const cosW2 = (l2 * l2 - t * t - l1 * l1) / (-2 * l1 * t);
+  const r = Math.atan2(y, xpd);
+  const w2 = Math.acos(cosW2);
+  const theta1 = r + w2;
+
+  const xmd = x - d2;
+  const s = Math.sqrt(xmd * xmd + y * y);
+  const cosW1 = (l2 * l2 - s * s - l1 * l1) / (-2 * l1 * s);
+  const q = Math.atan2(y, xmd);
+  const w1 = Math.acos(cosW1);
+  const theta2 = q - w1;
+  return { theta1, theta2 };
+}
+
+function radToSteps(rad) { return Math.round((rad / (2 * Math.PI)) * ARM.STEPS_PER_REV); }
+
+const targetX = 115, targetY = 113;
+const { theta1, theta2 } = solve(targetX, targetY);
+console.log(`Target: (${targetX}, ${targetY})`);
+console.log(`theta1: ${theta1 * 180 / Math.PI} deg`);
+console.log(`theta2: ${theta2 * 180 / Math.PI} deg`);
+
+const newSteps1 = radToSteps(theta1);
+const newSteps2 = radToSteps(theta2);
+console.log(`newSteps1: ${newSteps1}`);
+console.log(`newSteps2: ${newSteps2}`);
+
+console.log(`delta1 from HOME_STEPS.m1 (1024): ${newSteps1 - HOME_STEPS.m1}`);
+console.log(`delta2 from HOME_STEPS.m2 (0): ${newSteps2 - HOME_STEPS.m2}`);
+
+const delta1_old = newSteps1 - (-1024);
+console.log(`delta1 from OLD_HOME_STEPS.m1 (-1024): ${delta1_old}`);

web/js/sections/stepper-test.js

@@ -53,6 +53,12 @@ function syncStateFromStorage() {
   const isHomed = localStorage.getItem('wiji_homed') === 'true';
   steps1 = isHomed ? parseInt(localStorage.getItem('wiji_steps1') || IK.ARM.HOME_STEPS.m1) : IK.ARM.HOME_STEPS.m1;
   steps2 = isHomed ? parseInt(localStorage.getItem('wiji_steps2') || IK.ARM.HOME_STEPS.m2) : IK.ARM.HOME_STEPS.m2;
+  
+  // Auto-migrate the old buggy negative home position to the new positive one
+  if (steps1 === -1024) {
+    steps1 = 1024;
+    localStorage.setItem('wiji_steps1', 1024);
+  }
 }
 
 function saveSteps() {
@@ -194,10 +200,6 @@ async function moveToXY(targetX, targetY) {
   const delta1 = newSteps1 - steps1;
   const delta2 = newSteps2 - steps2;
 
-  steps1 = newSteps1;
-  steps2 = newSteps2;
-  renderArm(theta1, theta2);
-
   const cmd1 = `S1${delta1 >= 0 ? '+' : ''}${delta1}`;
   const cmd2 = `S2${delta2 >= 0 ? '+' : ''}${delta2}`;
 
@@ -205,10 +207,14 @@ async function moveToXY(targetX, targetY) {
     try {
       await BLE.write(cmd1);
       await BLE.write(cmd2);
+      // Do not update steps1/steps2 here; let the P: notify handler update the UI smoothly
     } catch (e) {
       UI.log(`BLE error: ${e.message}`, 'error');
     }
   } else {
+    steps1 = newSteps1;
+    steps2 = newSteps2;
+    renderArm(theta1, theta2);
     const simMsg = `[sim] IK→ θ1=${IK.radToDeg(theta1).toFixed(1)}° θ2=${IK.radToDeg(theta2).toFixed(1)}° | ${cmd1}  ${cmd2}`;
     console.log(simMsg);
     UI.log(simMsg, 'info');
@@ -221,33 +227,33 @@ async function moveToXY(targetX, targetY) {
 // ─────────────────────────────────────────────────────────────────
 
 async function jogMotor(motor, delta) {
-  if (motor === 1) steps1 += delta;
-  else steps2 += delta;
-  renderArmFromSteps();
   const cmd = `S${motor}${delta >= 0 ? '+' : ''}${delta}`;
   if (BLE.isConnected()) {
     await BLE.write(cmd).catch(e => UI.log(e.message, 'error'));
   } else {
+    if (motor === 1) steps1 += delta;
+    else steps2 += delta;
+    renderArmFromSteps();
     console.log(`[sim] ${cmd}`);
     UI.log(`[sim] ${cmd}`, 'info');
   }
 }
 
 async function zeroMotor(motor) {
-  if (motor === 'ALL') {
-    steps1 = IK.ARM.HOME_STEPS.m1;
-    steps2 = IK.ARM.HOME_STEPS.m2;
-    localStorage.setItem('wiji_homed', 'true');
-  } else if (motor === 1) {
-    steps1 = IK.ARM.HOME_STEPS.m1;
-  } else {
-    steps2 = IK.ARM.HOME_STEPS.m2;
-  }
-  renderArmFromSteps();
   const cmd = motor === 'ALL' ? 'HOMEALL' : `HOME${motor}`;
   if (BLE.isConnected()) {
     await BLE.write(cmd).catch(e => UI.log(e.message, 'error'));
   } else {
+    if (motor === 'ALL') {
+      steps1 = IK.ARM.HOME_STEPS.m1;
+      steps2 = IK.ARM.HOME_STEPS.m2;
+      localStorage.setItem('wiji_homed', 'true');
+    } else if (motor === 1) {
+      steps1 = IK.ARM.HOME_STEPS.m1;
+    } else {
+      steps2 = IK.ARM.HOME_STEPS.m2;
+    }
+    renderArmFromSteps();
     console.log(`[sim] ${cmd}`);
     UI.log(`[sim] ${cmd}`, 'info');
   }