From f898883ff2a520a74d16e0488522739d99f6a634 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 11:55:09 +0200
Subject: [PATCH] fix kinematics - maybe?

---
 gemini.md                       | 50 ++++++++++---------
 web/js/kinematics.js            | 85 +++++++++++++++++----------------
 web/js/sections/stepper-test.js | 36 +++++++-------
 3 files changed, 85 insertions(+), 86 deletions(-)

diff --git a/gemini.md b/gemini.md
index e22d3e8..56d81a4 100644
--- a/gemini.md
+++ b/gemini.md
@@ -41,21 +41,19 @@ only BLE between the ESP32 firmware and a local web SPA.
           ELBOW1              ELBOW2
               \                  /
            l1 (85 mm)      l1 (85 mm)
-                \              /
-         MOTOR1 (+12.9,0)  MOTOR2 (-12.9,0)
+                 \              /
+         MOTOR1 (-12.9,0)  MOTOR2 (+12.9,0)
                   |           |
                  [===BASE (25.8 mm)===]
 ```
 
 **⚠️ CRITICAL MOTOR CONVENTION** (source of many past bugs):
 
-`PositionControl.cpp` from the original project uses:
-- `xmd = x - d2` → Motor 1 pivot is at **(+d2, 0) = (+12.9, 0)** — the RIGHT side
-- `xpd = x + d2` → Motor 2 pivot is at **(-d2, 0) = (-12.9, 0)** — the LEFT side
+The original C++ firmware has a variable swap bug where `angle2` is passed to `stepper1` and `angle1` to `stepper2`. Thus, the physical mapping is:
+- **Motor 1** is the **LEFT** motor, pivoting at **(-d2, 0) = (-12.9, 0)**. It uses `xpd = x + d2`.
+- **Motor 2** is the **RIGHT** motor, pivoting at **(+d2, 0) = (+12.9, 0)**. It uses `xmd = x - d2`.
 
-This is counter-intuitive. Motor 1 is physically on the right. **Every piece of code that
-computes elbow or motor-base positions must use M1 at +12.9 and M2 at -12.9 or the arms
-will appear visually crossed even for valid target positions.**
+**Every piece of code that computes elbow or motor-base positions must use M1 at -12.9 and M2 at +12.9 or the arms will appear visually crossed and physically collide.**
 
 ### Arm constants (from `PositionControl.cpp`)
 
@@ -184,20 +182,20 @@ hamburger (`☰`) button. It contains `<button data-route="...">` nav items that
 
 ### IK (`solve(x, y)`)
 
-Exact port of `calculateInverseKinematics()` from `PositionControl.cpp`:
+Exact port of `calculateInverseKinematics()` from `PositionControl.cpp`, with motor swap applied:
 
 ```javascript
-// Motor 1 (pivot at +d2 = +12.9 mm)
-const xmd = x - d2;                  // Δx from M1 pivot
-const s   = hypot(xmd, y);
-const cosW1 = (l2² - s² - l1²) / (-2·l1·s);
-const theta1 = atan2(y, xmd) - acos(cosW1);
-
-// Motor 2 (pivot at -d2 = -12.9 mm)
-const xpd = x + d2;                  // Δx from M2 pivot
+// Motor 1 (Left, pivot at -d2 = -12.9 mm)
+const xpd = x + d2;                  // Δx from M1 pivot
 const t   = hypot(xpd, y);
 const cosW2 = (l2² - t² - l1²) / (-2·l1·t);
-const theta2 = atan2(y, xpd) + acos(cosW2);
+const theta1 = atan2(y, xpd) + acos(cosW2);
+
+// Motor 2 (Right, pivot at +d2 = +12.9 mm)
+const xmd = x - d2;                  // Δx from M2 pivot
+const s   = hypot(xmd, y);
+const cosW1 = (l2² - s² - l1²) / (-2·l1·s);
+const theta2 = atan2(y, xmd) - acos(cosW1);
 ```
 
 Returns `{ theta1, theta2, reachable }` — angles in **radians**.
@@ -207,9 +205,9 @@ Returns `{ theta1, theta2, reachable }` — angles in **radians**.
 Computes elbow and EE positions for visualisation:
 
 ```javascript
-// M1 at +d2, M2 at -d2 ← critical sign convention
-elbow1 = { x: +d2 + l1·cos(theta1),  y: l1·sin(theta1) }
-elbow2 = { x: -d2 + l1·cos(theta2),  y: l1·sin(theta2) }
+// M1 at -d2, M2 at +d2 ← critical sign convention
+elbow1 = { x: -d2 + l1·cos(theta1),  y: l1·sin(theta1) }
+elbow2 = { x: +d2 + l1·cos(theta2),  y: l1·sin(theta2) }
 // EE = upward circle-circle intersection of circles (elbow1, l2) and (elbow2, l2)
 ```
 
@@ -294,9 +292,9 @@ const SV = {
 ### Motor marker positions in SVG (match FK convention)
 
 ```javascript
-// M1 at +d2 (right side), M2 at -d2 (left side)
-motor1sx = SV.wx(+IK.ARM.d2);
-motor2sx = SV.wx(-IK.ARM.d2);
+// M1 at -d2 (left side), M2 at +d2 (right side)
+motor1sx = SV.wx(-IK.ARM.d2);
+motor2sx = SV.wx(+IK.ARM.d2);
 ```
 
 ### IK click-to-move flow
@@ -419,7 +417,7 @@ To add a new section:
 | **No WiFi / no server** | User requirement — BLE only, no hosting needed |
 | **`?v=Date.now()` cache-buster** | ES module imports are cached by the browser; without this, stale code runs silently after edits |
 | **Steps as primary state** | The firmware tracks absolute step counts. Angles are derived by `stepsToRad()`. This keeps web ↔ firmware in sync. |
-| **M1 at +d2, M2 at -d2** | This is the PositionControl.cpp convention. Reversing it causes visual arm crossing. Verified by tracing `xmd = x - d2` (offset from +d2 pivot) and `xpd = x + d2` (offset from -d2 pivot). |
+| **M1 at -d2, M2 at +d2** | Corrects a hardware variable swap in C++ firmware where angle2 goes to stepper1. Verified by tracing `xmd = x - d2` to the right motor and `xpd = x + d2` to the left motor. |
 | **`forward()` picks upward EE** | Two circle-circle intersections exist; the mechanism always operates in the "above" configuration. The lower solution is physically blocked by the board. |
 | **`armsCrossed()` guard** | Prevents IK moves that would drive an elbow into the mechanism housing from the wrong side. Tunable via `LIMITS.ELBOW_BOX_X_INNER` and `ELBOW_BOX_Y_MAX`. |
 | **Linux/BlueZ stability** | `pAdv->setMaxPreferred(0x0C)` is mandatory in firmware. Without it, Linux/ChromeOS will drop the connection immediately after the handshake. |
@@ -437,7 +435,7 @@ To add a new section:
   assume a specific physical motor orientation. If the real arm moves in the wrong direction
   when clicking a target, negate the step delta for that motor in `moveToXY()`.
 
-- **Home position**: Stall homing is fully implemented matching the original C++ routine but adjusted to align with `PositionControl.cpp` motor mappings. Homing pushes the motors against their physical limits and sets `steps1 = 0` (0°, points right) and `steps2 = -1024` (-180°, points left). This corresponds to the arms folded OUTWARDS.
+- **Home position**: Stall homing is fully implemented matching the original C++ routine but adjusted to align with physical motor mappings. Homing pushes the motors against their physical limits and sets `steps1 = -1024` (-180°, points left) and `steps2 = 0` (0°, points right). This corresponds to the arms folded OUTWARDS.
   - The initial web simulation starts with these coordinates, meaning the UI assumes the arm has already been homed before the browser connects.
   - Homing can be triggered individually per-motor (`HOME1`, `HOME2`) or combined (`HOMEALL`) from the web UI.
 
diff --git a/web/js/kinematics.js b/web/js/kinematics.js
index 5c8d8d0..cb28915 100644
--- a/web/js/kinematics.js
+++ b/web/js/kinematics.js
@@ -18,15 +18,16 @@
  *                   d2=12.9 mm
  *
  * ── IMPORTANT: PositionControl.cpp motor convention ─────────────
- *   Motor 1 pivot is at (+d2, 0) = (+12.9, 0)  [right side!]
- *   Motor 2 pivot is at (-d2, 0) = (-12.9, 0)  [left  side!]
+ *   Motor 1 pivot is at (-d2, 0) = (-12.9, 0)  [left  side!]
+ *   Motor 2 pivot is at (+d2, 0) = (+12.9, 0)  [right side!]
  *
- *   This is determined by how the C++ IK formulas use the offsets:
- *     Motor 1:  xmd = x - d2  → target is measured from x = +d2
- *     Motor 2:  xpd = x + d2  → target is measured from x = -d2
+ *   This is determined by a variable swap in the C++ firmware where
+ *   angle2 is passed to stepper1 and angle1 to stepper2. Thus:
+ *     Motor 1 (Left):   xpd = x + d2  → target is measured from x = -d2
+ *     Motor 2 (Right):  xmd = x - d2  → target is measured from x = +d2
  *
- *   All FK / visualisation code MUST use this same convention or
- *   the arms will appear visually crossed even for valid positions.
+ *   All FK / visualisation code MUST use this exact convention or
+ *   the arms will appear visually crossed and physically collide.
  *
  * Source: lib/Position/PositionControl.cpp  (nerd-sniped/WijiBoard)
  */
@@ -34,12 +35,12 @@
 // ── Exact constants from PositionControl.cpp ─────────────────────
 const ARM = {
   d: 25.8,   // full motor separation (mm)
-  d2: 12.9,   // half separation; M1 at (+d2, 0), M2 at (-d2, 0)
+  d2: 12.9,   // half separation; M1 at (-d2, 0), M2 at (+d2, 0)
   l1: 85.0,  // proximal link length (mm)
   l2: 110.0,  // distal  link length (mm)
   STEPS_PER_REV: 2048,
   STEP_ANGLE_DEG: 360 / 2048,  // ≈ 0.17578125 °/step
-  HOME_STEPS: { m1: 0, m2: -1024 }, // M1 at 0°, M2 at -180° (arms folded outward)
+  HOME_STEPS: { m1: -1024, m2: 0 }, // M1 (Left) at -180°, M2 (Right) at 0° (arms folded outward)
 };
 
 // ── WORKSPACE CONSTRAINTS ─────────────────────────────────────────
@@ -62,10 +63,10 @@ const LIMITS = {
 
   // ── Elbow exclusion zone (prevents arm crossing near the box) ─
   // Each elbow has a separate rectangular exclusion box.
-  // Left-side elbow (from M1 at +d2): must NOT enter this region.
-  // Right-side elbow (from M2 at -d2): uses mirrored X limits.
-  //   If ELBOW_BOX_X_INNER is 5, the left elbow's X must be > +5 mm
-  //   (can never cross to the other side of the box mid-point).
+  // Left-side elbow (from M1 at -d2): must NOT enter this region.
+  // Right-side elbow (from M2 at +d2): uses mirrored X limits.
+  //   If ELBOW_BOX_X_INNER is 5, the left elbow's X must be < -5 mm
+  //   (can never cross to the right side of the box mid-point).
   ELBOW_BOX_X_INNER: 5,   // ← TUNE: inner X margin from centre for each elbow
   ELBOW_BOX_Y_MAX: 50,  // ← TUNE: Y below which elbow crossing is forbidden
 };
@@ -130,19 +131,8 @@ const LOOKUP_TABLE = {
 function solve(x, y) {
   const { d2, l1, l2 } = ARM;
 
-  // ── Motor 1 (pivot at +d2, 0 = +12.9 mm) ─────────────────────
-  // xmd = x - d2  is the X component of (target – M1_pivot).
-  const xmd = x - d2;
-  const s = Math.sqrt(xmd * xmd + y * y);
-  if (s < 1e-6) return { theta1: 0, theta2: 0, reachable: false };
-  const cosW1 = (l2 * l2 - s * s - l1 * l1) / (-2 * l1 * s);
-  if (cosW1 < -1 || cosW1 > 1) return { theta1: 0, theta2: 0, reachable: false };
-  const q = Math.atan2(y, xmd);
-  const w1 = Math.acos(cosW1);
-  const theta1 = q - w1;
-
-  // ── Motor 2 (pivot at -d2, 0 = -12.9 mm) ─────────────────────
-  // xpd = x + d2  is the X component of (target – M2_pivot).
+  // ── Motor 1 (Left, pivot at -d2, 0 = -12.9 mm) ───────────────
+  // xpd = x + d2 is the X component of (target – M1_pivot).
   const xpd = x + d2;
   const t = Math.sqrt(xpd * xpd + y * y);
   if (t < 1e-6) return { theta1: 0, theta2: 0, reachable: false };
@@ -150,7 +140,18 @@ function solve(x, y) {
   if (cosW2 < -1 || cosW2 > 1) return { theta1: 0, theta2: 0, reachable: false };
   const r = Math.atan2(y, xpd);
   const w2 = Math.acos(cosW2);
-  const theta2 = r + w2;
+  const theta1 = r + w2;
+
+  // ── Motor 2 (Right, pivot at +d2, 0 = +12.9 mm) ──────────────
+  // xmd = x - d2 is the X component of (target – M2_pivot).
+  const xmd = x - d2;
+  const s = Math.sqrt(xmd * xmd + y * y);
+  if (s < 1e-6) return { theta1: 0, theta2: 0, reachable: false };
+  const cosW1 = (l2 * l2 - s * s - l1 * l1) / (-2 * l1 * s);
+  if (cosW1 < -1 || cosW1 > 1) return { theta1: 0, theta2: 0, reachable: false };
+  const q = Math.atan2(y, xmd);
+  const w1 = Math.acos(cosW1);
+  const theta2 = q - w1;
 
   return { theta1, theta2, reachable: true };
 }
@@ -174,17 +175,17 @@ function solve(x, y) {
 function forward(theta1, theta2) {
   const { d2, l1, l2 } = ARM;
 
-  // ── IMPORTANT: match PositionControl.cpp motor convention ──────
-  // Motor 1 pivot at (+d2, 0), Motor 2 pivot at (-d2, 0).
+  // ── IMPORTANT: match physical hardware convention ──────────────
+  // Motor 1 pivot at (-d2, 0), Motor 2 pivot at (+d2, 0).
   // Using the opposite sign here is the single most common source
   // of visually-crossed arms in the SVG visualiser.
 
-  // Elbow 1 — tip of Motor 1 proximal link (motor at +d2)
-  const e1x = +d2 + l1 * Math.cos(theta1);
+  // Elbow 1 — tip of Motor 1 proximal link (motor at -d2, Left)
+  const e1x = -d2 + l1 * Math.cos(theta1);
   const e1y = l1 * Math.sin(theta1);
 
-  // Elbow 2 — tip of Motor 2 proximal link (motor at -d2)
-  const e2x = -d2 + l1 * Math.cos(theta2);
+  // Elbow 2 — tip of Motor 2 proximal link (motor at +d2, Right)
+  const e2x = +d2 + l1 * Math.cos(theta2);
   const e2y = l1 * Math.sin(theta2);
 
   // End-effector: intersection of the two distal-link circles
@@ -234,9 +235,9 @@ function forward(theta1, theta2) {
  * cross each other near the centre mechanism box.
  *
  * Physical rule: each elbow must stay on the OUTER side of the
- * mechanism housing. If elbow1 (from M1 at +d2) has a small
- * positive X at low Y, or elbow2 (from M2 at -d2) has a small
- * negative X at low Y, the arm would collide with the housing.
+ * mechanism housing. If elbow1 (from M1 at -d2) has a small
+ * negative X at low Y, or elbow2 (from M2 at +d2) has a small
+ * positive X at low Y, the arm would collide with the housing.
  *
  * @param {number} theta1  Motor 1 angle (rad)
  * @param {number} theta2  Motor 2 angle (rad)
@@ -247,16 +248,16 @@ function armsCrossed(theta1, theta2) {
   const { ELBOW_BOX_X_INNER: XI, ELBOW_BOX_Y_MAX: YM } = LIMITS;
 
   // Elbow positions (same formula as forward())
-  const e1x = +d2 + l1 * Math.cos(theta1);
+  const e1x = -d2 + l1 * Math.cos(theta1);
   const e1y = l1 * Math.sin(theta1);
-  const e2x = -d2 + l1 * Math.cos(theta2);
+  const e2x = +d2 + l1 * Math.cos(theta2);
   const e2y = l1 * Math.sin(theta2);
 
-  // Elbow1 (from M1 on the RIGHT) must not appear far to the LEFT at low height
-  // Elbow2 (from M2 on the LEFT)  must not appear far to the RIGHT at low height
+  // Elbow1 (from M1 on the LEFT) must not appear far to the RIGHT at low height
+  // Elbow2 (from M2 on the RIGHT) must not appear far to the LEFT at low height
   // Both conditions together catch the "arms have swapped sides" scenario.
-  const e1_crossed = e1x < -XI && e1y < YM;  // M1's elbow went too far left
-  const e2_crossed = e2x > XI && e2y < YM;  // M2's elbow went too far right
+  const e1_crossed = e1x > XI && e1y < YM;  // M1's elbow went too far right
+  const e2_crossed = e2x < -XI && e2y < YM;  // M2's elbow went too far left
 
   return e1_crossed || e2_crossed;
 }
diff --git a/web/js/sections/stepper-test.js b/web/js/sections/stepper-test.js
index 55288ed..bbdf3c2 100644
--- a/web/js/sections/stepper-test.js
+++ b/web/js/sections/stepper-test.js
@@ -4,8 +4,8 @@
  *
  * 5-bar parallel linkage visualiser + IK-based click-to-move.
  *
- * Mechanism geometry (from PositionControl.cpp):
- *   Motor 1 at (+12.9, 0) mm   Motor 2 at (-12.9, 0) mm   ← NOTE: M1 is on the RIGHT
+ * Mechanism geometry (corrected from PositionControl.cpp bug):
+ *   Motor 1 at (-12.9, 0) mm   Motor 2 at (+12.9, 0) mm   ← NOTE: M1 is on the LEFT
  *   Proximal links: l1 = 85 mm   Distal links: l2 = 110 mm
  *
  * IK flow:  click XY → IK.solve(x,y) → {θ1, θ2} → steps → BLE
@@ -93,9 +93,9 @@ function setCircle(el, cx, cy) {
 function renderArm(theta1, theta2, isGhost = false) {
   const { elbow1: e1, elbow2: e2, endX, endY } = IK.forward(theta1, theta2);
 
-  // Motor base SVG positions — M1 at +d2, M2 at -d2 (PositionControl.cpp convention)
-  const motor1sx = SV.wx(+IK.ARM.d2); const motor1sy = SV.wy(0);
-  const motor2sx = SV.wx(-IK.ARM.d2); const motor2sy = SV.wy(0);
+  // Motor base SVG positions — M1 at -d2, M2 at +d2 (hardware convention)
+  const motor1sx = SV.wx(-IK.ARM.d2); const motor1sy = SV.wy(0);
+  const motor2sx = SV.wx(+IK.ARM.d2); const motor2sy = SV.wy(0);
   const elbow1sx = SV.wx(e1.x); const elbow1sy = SV.wy(e1.y);
   const elbow2sx = SV.wx(e2.x); const elbow2sy = SV.wy(e2.y);
   const endsx = SV.wx(endX); const endsy = SV.wy(endY);
@@ -275,9 +275,9 @@ function buildZones() {
   const ew = L.BOX_HALF_W * 2 * SV.SCALE;
   const eh = (L.BOX_Y_MAX - L.BOX_Y_MIN) * SV.SCALE;
 
-  // Motor positions — M1 at +d2, M2 at -d2
-  const m1x = SV.wx(+IK.ARM.d2);
-  const m2x = SV.wx(-IK.ARM.d2);
+  // Motor positions — M1 at -d2, M2 at +d2
+  const m1x = SV.wx(-IK.ARM.d2);
+  const m2x = SV.wx(+IK.ARM.d2);
   const my = SV.wy(0);
 
   // Scale ruler
@@ -432,31 +432,31 @@ function buildHTML() {
                 font-family="monospace" style="display:none"/>
 
           <!-- ── Real arm ──────────────────────────────────── -->
-          <!-- Arm 1: proximal + distal (M1 at +d2) -->
+          <!-- Arm 1: proximal + distal (M1 at -d2) -->
           <line id="arm1-prox"
-                x1="${SV.wx(+IK.ARM.d2)}" y1="${SV.wy(0)}"
-                x2="${SV.wx(+IK.ARM.d2)}" y2="${SV.wy(IK.ARM.l1)}"
+                x1="${SV.wx(-IK.ARM.d2)}" y1="${SV.wy(0)}"
+                x2="${SV.wx(-IK.ARM.d2)}" y2="${SV.wy(IK.ARM.l1)}"
                 stroke="#448aff" stroke-width="5" stroke-linecap="round"/>
           <line id="arm1-dist"
-                x1="${SV.wx(+IK.ARM.d2)}" y1="${SV.wy(IK.ARM.l1)}"
+                x1="${SV.wx(-IK.ARM.d2)}" y1="${SV.wy(IK.ARM.l1)}"
                 x2="${SV.wx(0)}"           y2="${SV.wy(IK.ARM.l1 + IK.ARM.l2)}"
                 stroke="#7eb8f7" stroke-width="3.5" stroke-linecap="round" stroke-dasharray="6 3"/>
 
-          <!-- Arm 2: proximal + distal (M2 at -d2) -->
+          <!-- Arm 2: proximal + distal (M2 at +d2) -->
           <line id="arm2-prox"
-                x1="${SV.wx(-IK.ARM.d2)}" y1="${SV.wy(0)}"
-                x2="${SV.wx(-IK.ARM.d2)}" y2="${SV.wy(IK.ARM.l1)}"
+                x1="${SV.wx(+IK.ARM.d2)}" y1="${SV.wy(0)}"
+                x2="${SV.wx(+IK.ARM.d2)}" y2="${SV.wy(IK.ARM.l1)}"
                 stroke="#f7a03c" stroke-width="5" stroke-linecap="round"/>
           <line id="arm2-dist"
-                x1="${SV.wx(-IK.ARM.d2)}" y1="${SV.wy(IK.ARM.l1)}"
+                x1="${SV.wx(+IK.ARM.d2)}" y1="${SV.wy(IK.ARM.l1)}"
                 x2="${SV.wx(0)}"           y2="${SV.wy(IK.ARM.l1 + IK.ARM.l2)}"
                 stroke="#ffd08a" stroke-width="3.5" stroke-linecap="round" stroke-dasharray="6 3"/>
 
           <!-- Elbow joints -->
           <circle id="elbow1" r="5" fill="#a0c4ff" stroke="#0a0e18" stroke-width="1.5"
-                  cx="${SV.wx(+IK.ARM.d2)}" cy="${SV.wy(IK.ARM.l1)}"/>
-          <circle id="elbow2" r="5" fill="#ffd08a" stroke="#0a0e18" stroke-width="1.5"
                   cx="${SV.wx(-IK.ARM.d2)}" cy="${SV.wy(IK.ARM.l1)}"/>
+          <circle id="elbow2" r="5" fill="#ffd08a" stroke="#0a0e18" stroke-width="1.5"
+                  cx="${SV.wx(+IK.ARM.d2)}" cy="${SV.wy(IK.ARM.l1)}"/>
 
           <!-- End effector -->
           <circle id="end-eff" r="7"