From e1416f697963cb6c81235c2ec6e198c83dfbac12 Mon Sep 17 00:00:00 2001 From: osiu97 Date: Wed, 8 Jul 2026 17:52:38 +0200 Subject: [PATCH] Upgrade pathing --- src/main.cpp | 8 ++++++++ web/js/kinematics.js | 18 +++++++++++------- web/js/sections/board-control.js | 4 ++-- web/js/sections/input-text.js | 4 ++-- web/js/sections/stepper-test.js | 30 +++++++++++++++++++++++++++--- web/js/sequence-runner.js | 4 ++-- 6 files changed, 52 insertions(+), 16 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index 08fa69d..34fc7cb 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -185,6 +185,14 @@ void parseCommand(const String &cmd) { sendPosition(); return; } + // ZEROALL + if (cmd == "ZEROALL") { + stepper1.setCurrentPosition(1024); + stepper2.setCurrentPosition(0); + Serial.println("[SYS] Position blindly reset to home (1024, 0)."); + sendPosition(); + return; + } // POS – explicit position request if (cmd == "POS") { sendPosition(); diff --git a/web/js/kinematics.js b/web/js/kinematics.js index 2ea6d62..7124cd5 100644 --- a/web/js/kinematics.js +++ b/web/js/kinematics.js @@ -327,12 +327,16 @@ function radToSteps(rad) { 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 radToStepsAbsolute(rad, motor) { + let steps = radToSteps(rad); + // Motor 1 (Left) is constrained near 1536 steps (90 deg, UP) + // Motor 2 (Right) is constrained near -512 steps (90 deg, UP) + const center = motor === 1 ? 1536 : -512; + // Snap the step position to the closest equivalent angle around the center. + // This guarantees we always take the "top" physical path and never wrap through the bottom. + while (steps - center > 1024) steps -= 2048; + while (steps - center < -1024) steps += 2048; + return steps; } function stepsToDeg(steps) { return steps * ARM.STEP_ANGLE_DEG; @@ -344,5 +348,5 @@ function radToDeg(rad) { export default { ARM, LIMITS, LOOKUP_TABLE, solve, forward, armsCrossed, checkWorkspace, lookup, - stepsToRad, radToSteps, radToStepsClosest, stepsToDeg, radToDeg, + stepsToRad, radToSteps, radToStepsAbsolute, stepsToDeg, radToDeg, }; diff --git a/web/js/sections/board-control.js b/web/js/sections/board-control.js index 01a4664..a6ca918 100644 --- a/web/js/sections/board-control.js +++ b/web/js/sections/board-control.js @@ -277,8 +277,8 @@ async function executeMove(spot) { return; } - const newSteps1 = IK.radToStepsClosest(res.theta1, steps1); - const newSteps2 = IK.radToStepsClosest(res.theta2, steps2); + const newSteps1 = IK.radToStepsAbsolute(res.theta1, 1); + const newSteps2 = IK.radToStepsAbsolute(res.theta2, 2); const delta1 = newSteps1 - steps1; const delta2 = newSteps2 - steps2; diff --git a/web/js/sections/input-text.js b/web/js/sections/input-text.js index 257bc60..be9ce54 100644 --- a/web/js/sections/input-text.js +++ b/web/js/sections/input-text.js @@ -275,8 +275,8 @@ async function executeMove(spot) { return false; } - const newSteps1 = IK.radToStepsClosest(res.theta1, steps1); - const newSteps2 = IK.radToStepsClosest(res.theta2, steps2); + const newSteps1 = IK.radToStepsAbsolute(res.theta1, 1); + const newSteps2 = IK.radToStepsAbsolute(res.theta2, 2); const delta1 = newSteps1 - steps1; const delta2 = newSteps2 - steps2; diff --git a/web/js/sections/stepper-test.js b/web/js/sections/stepper-test.js index e820974..2d52c6b 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.radToStepsClosest(theta1, steps1); - const newSteps2 = IK.radToStepsClosest(theta2, steps2); + const newSteps1 = IK.radToStepsAbsolute(theta1, 1); + const newSteps2 = IK.radToStepsAbsolute(theta2, 2); const cmd = `X:${newSteps1},${newSteps2}`; @@ -252,6 +252,9 @@ async function zeroMotor(motor) { renderArmFromSteps(); console.log(`[sim] ${cmd}`); UI.log(`[sim] ${cmd}`, 'info'); + + const warningEl = document.getElementById('home-warning'); + if (warningEl) warningEl.style.display = localStorage.getItem('wiji_homed') === 'true' ? 'none' : 'block'; } } @@ -498,7 +501,10 @@ function buildHTML() {
-
Current Position
+
+ Current Position + +
0.0
@@ -622,6 +628,7 @@ function buildHTML() {
+
@@ -736,6 +743,20 @@ const StepperTestSection = { document.getElementById('s1-zero').addEventListener('click', () => zeroMotor(1)); document.getElementById('s2-zero').addEventListener('click', () => zeroMotor(2)); document.getElementById('sall-zero').addEventListener('click', () => zeroMotor('ALL')); + document.getElementById('fake-home').addEventListener('click', async () => { + localStorage.setItem('wiji_homed', 'false'); + const warningEl = document.getElementById('home-warning'); + if (warningEl) warningEl.style.display = 'block'; + if (BLE.isConnected()) { + await BLE.write('ZEROALL').catch(e => UI.log(e.message, 'error')); + } else { + steps1 = IK.ARM.HOME_STEPS.m1; + steps2 = IK.ARM.HOME_STEPS.m2; + renderArmFromSteps(); + console.log(`[sim] ZEROALL`); + UI.log(`[sim] ZEROALL (Reset Position)`, 'info'); + } + }); // ── Quick-step buttons ──────────────────────────────────────── container.querySelectorAll('[data-motor][data-steps]').forEach(btn => { @@ -796,6 +817,9 @@ const StepperTestSection = { ]; // ── Initial render ───────────────────────────────────────────── + const warningEl = document.getElementById('home-warning'); + if (warningEl) warningEl.style.display = localStorage.getItem('wiji_homed') === 'true' ? 'none' : 'block'; + renderArmFromSteps(); UI.log('Stepper Test ready — 5-bar parallel IK active.', 'success'); UI.log(`Arm: d=${IK.ARM.d}mm l1=${IK.ARM.l1}mm l2=${IK.ARM.l2}mm`, 'info'); diff --git a/web/js/sequence-runner.js b/web/js/sequence-runner.js index 3b5e14a..400a8e8 100644 --- a/web/js/sequence-runner.js +++ b/web/js/sequence-runner.js @@ -64,8 +64,8 @@ export default { continue; } - const newSteps1 = IK.radToStepsClosest(res.theta1, steps1); - const newSteps2 = IK.radToStepsClosest(res.theta2, steps2); + const newSteps1 = IK.radToStepsAbsolute(res.theta1, 1); + const newSteps2 = IK.radToStepsAbsolute(res.theta2, 2); const cmd = `X:${newSteps1},${newSteps2}`;