From 242df22ba60743d32c0afedcba1ebc28f7f32aac 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 14:53:47 +0200
Subject: [PATCH] kienamtics

---
 dump_git.js          | 10 ++++++++++
 get_git.js           |  7 +++++++
 src/main.cpp         |  1 +
 test_homing_math.py  | 41 ++++++++++++++++++++++++++++++++++++++
 test_render.js       | 47 ++++++++++++++++++++++++++++++++++++++++++++
 web/js/kinematics.js |  5 +++--
 6 files changed, 109 insertions(+), 2 deletions(-)
 create mode 100644 dump_git.js
 create mode 100644 get_git.js
 create mode 100644 test_homing_math.py
 create mode 100644 test_render.js

diff --git a/dump_git.js b/dump_git.js
new file mode 100644
index 0000000..50cb4d6
--- /dev/null
+++ b/dump_git.js
@@ -0,0 +1,10 @@
+const fs = require('fs');
+const { execSync } = require('child_process');
+
+try {
+  const output = execSync('git log -p -3 src/main.cpp', { cwd: 'm:/Nextcloud/IoT/esp32s3-wiji' });
+  fs.writeFileSync('m:/Nextcloud/IoT/esp32s3-wiji/git_history.txt', output);
+  console.log('Success');
+} catch (e) {
+  console.log('Error', e.message);
+}
diff --git a/get_git.js b/get_git.js
new file mode 100644
index 0000000..2574000
--- /dev/null
+++ b/get_git.js
@@ -0,0 +1,7 @@
+const { execSync } = require('child_process');
+const fs = require('fs');
+
+try {
+  const log = execSync('git log -p -n 10 src/main.cpp', { encoding: 'utf8' });
+  fs.writeFileSync('git_log.txt', log);
+} catch(e) {}
diff --git a/src/main.cpp b/src/main.cpp
index 0bcdf9c..2364aa6 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -289,6 +289,7 @@ void loop() {
       stepper2.setMaxSpeed(currentMaxSpeed);
       stepper1.setAcceleration(currentAccel);
       stepper2.setAcceleration(currentAccel);
+      sendPosition();
       homingState = HOME_IDLE;
       Serial.println("[SYS] Homing all complete.");
     }
diff --git a/test_homing_math.py b/test_homing_math.py
new file mode 100644
index 0000000..7b4bf8b
--- /dev/null
+++ b/test_homing_math.py
@@ -0,0 +1,41 @@
+import math
+
+d2 = 12.9
+l1 = 85.0
+l2 = 110.0
+
+t1_deg = 474 / 2048.0 * 360.0
+t2_deg = 530 / 2048.0 * 360.0
+
+th1 = math.radians(t1_deg)
+th2 = math.radians(t2_deg)
+
+e1x = -d2 + l1 * math.cos(th1)
+e1y = l1 * math.sin(th1)
+
+e2x = d2 + l1 * math.cos(th2)
+e2y = l1 * math.sin(th2)
+
+dx = e2x - e1x
+dy = e2y - e1y
+dist = math.sqrt(dx*dx + dy*dy)
+
+a = dist / 2
+h = math.sqrt(l2*l2 - a*a)
+
+mx = (e1x + e2x) / 2
+my = (e1y + e2y) / 2
+
+px1 = mx + h * (dy / dist)
+py1 = my + h * (-dx / dist)
+
+px2 = mx - h * (dy / dist)
+py2 = my - h * (-dx / dist)
+
+ex = px1 if py1 >= py2 else px2
+ey = py1 if py1 >= py2 else py2
+
+with open("output.txt", "w") as f:
+    f.write(f"End Effector: {ex:.1f}, {ey:.1f}\n")
+    f.write(f"Elbow 1: {e1x:.1f}, {e1y:.1f}\n")
+    f.write(f"Elbow 2: {e2x:.1f}, {e2y:.1f}\n")
diff --git a/test_render.js b/test_render.js
new file mode 100644
index 0000000..e513734
--- /dev/null
+++ b/test_render.js
@@ -0,0 +1,47 @@
+const fs = require('fs');
+
+const d2 = 12.9;
+const l1 = 85;
+const l2 = 110;
+
+function stepsToRad(steps) {
+  return (steps / 2048) * 2 * Math.PI;
+}
+
+function render(steps1, steps2) {
+  const t1 = stepsToRad(steps1);
+  const t2 = stepsToRad(steps2);
+
+  const e1x = -d2 + l1 * Math.cos(t1);
+  const e1y = l1 * Math.sin(t1);
+
+  const e2x = d2 + l1 * Math.cos(t2);
+  const e2y = l1 * Math.sin(t2);
+
+  const dx = e2x - e1x;
+  const dy = e2y - e1y;
+  const dist = Math.sqrt(dx*dx + dy*dy);
+
+  let endX = 0, endY = 0;
+  if (dist <= 2*l2 && dist >= 1e-6) {
+    const a = dist / 2;
+    const h = Math.sqrt(l2*l2 - a*a);
+    const mx = (e1x + e2x) / 2;
+    const my = (e1y + e2y) / 2;
+    const px1 = mx + h * (dy / dist);
+    const py1 = my + h * (-dx / dist);
+    const px2 = mx - h * (dy / dist);
+    const py2 = my - h * (-dx / dist);
+    if (py1 >= py2) { endX = px1; endY = py1; }
+    else { endX = px2; endY = py2; }
+  }
+
+  console.log(`steps1=${steps1}, steps2=${steps2}`);
+  console.log(`t1=${(t1*180/Math.PI).toFixed(1)}°, t2=${(t2*180/Math.PI).toFixed(1)}°`);
+  console.log(`e1=(${e1x.toFixed(1)}, ${e1y.toFixed(1)})`);
+  console.log(`e2=(${e2x.toFixed(1)}, ${e2y.toFixed(1)})`);
+  console.log(`end=(${endX.toFixed(1)}, ${endY.toFixed(1)})`);
+}
+
+render(854, 41);
+render(-41, 967);
diff --git a/web/js/kinematics.js b/web/js/kinematics.js
index 3295f82..8f17b8e 100644
--- a/web/js/kinematics.js
+++ b/web/js/kinematics.js
@@ -294,8 +294,9 @@ function armsCrossed(theta1, theta2) {
     return false;
   }
 
-  const cross1 = lineIntersectsBox(m1x, m1y, e1x, e1y, -BOX_HALF_W, BOX_HALF_W, BOX_Y_MIN, BOX_Y_MAX);
-  const cross2 = lineIntersectsBox(m2x, m2y, e2x, e2y, -BOX_HALF_W, BOX_HALF_W, BOX_Y_MIN, BOX_Y_MAX);
+  // Motors are at Y=0, so they are inside BOX_Y_MIN=-10. Start the collision box above the motors.
+  const cross1 = lineIntersectsBox(m1x, m1y, e1x, e1y, -BOX_HALF_W, BOX_HALF_W, 15, BOX_Y_MAX);
+  const cross2 = lineIntersectsBox(m2x, m2y, e2x, e2y, -BOX_HALF_W, BOX_HALF_W, 15, BOX_Y_MAX);
   
   // Also check if elbows went rogue (e.g. wrapped entirely around the wrong side)
   const e1_rogue = e1x > BOX_HALF_W && e1y < BOX_Y_MAX;