added proper kinematics
This commit is contained in:
PROFERIS - Mi³osz Stocki
+111
-19
@@ -5,7 +5,7 @@
|
||||
* 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
|
||||
* Motor 1 at (+12.9, 0) mm Motor 2 at (-12.9, 0) mm ← NOTE: M1 is on the RIGHT
|
||||
* Proximal links: l1 = 85 mm Distal links: l2 = 110 mm
|
||||
*
|
||||
* IK flow: click XY → IK.solve(x,y) → {θ1, θ2} → steps → BLE
|
||||
@@ -47,6 +47,7 @@ const PX_D2 = IK.ARM.d2 * SV.SCALE;
|
||||
// ── Section state ─────────────────────────────────────────────────
|
||||
let steps1 = 0;
|
||||
let steps2 = 0;
|
||||
let isHoming = false; // blocks all controls during homing sequence
|
||||
let eventCleanup = [];
|
||||
|
||||
// ── SVG element refs ──────────────────────────────────────────────
|
||||
@@ -80,8 +81,9 @@ function setCircle(el, cx, cy) {
|
||||
function renderArm(theta1, theta2, isGhost = false) {
|
||||
const { elbow1: e1, elbow2: e2, endX, endY } = IK.forward(theta1, theta2);
|
||||
|
||||
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 (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);
|
||||
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);
|
||||
@@ -223,6 +225,59 @@ async function zeroMotor(motor) {
|
||||
: UI.log(`[sim] ${cmd}`, 'info');
|
||||
}
|
||||
|
||||
// ─────────────────────────────────────────────────────────────────
|
||||
// Homing
|
||||
// ─────────────────────────────────────────────────────────────────
|
||||
|
||||
/** Block / unblock all interactive controls while homing is running. */
|
||||
function setHomingActive(active) {
|
||||
isHoming = active;
|
||||
const root = document.querySelector('.section-page');
|
||||
if (!root) return;
|
||||
if (active) {
|
||||
root.classList.add('homing-active');
|
||||
} else {
|
||||
root.classList.remove('homing-active');
|
||||
}
|
||||
// Update the button label
|
||||
const btn = document.getElementById('btn-find-home');
|
||||
const spin = document.getElementById('home-spinner');
|
||||
const lbl = document.getElementById('home-label');
|
||||
if (!btn) return;
|
||||
btn.disabled = active;
|
||||
if (spin) spin.style.display = active ? '' : 'none';
|
||||
if (lbl) lbl.textContent = active ? 'Homing…' : 'Find Home';
|
||||
}
|
||||
|
||||
/** Called when the firmware sends HOMED or when simulating. */
|
||||
function onHomingComplete() {
|
||||
steps1 = 0;
|
||||
steps2 = 0;
|
||||
setHomingActive(false);
|
||||
renderArmFromSteps();
|
||||
UI.log('✓ Homing complete — step counters reset to zero.', 'success');
|
||||
}
|
||||
|
||||
async function startHoming() {
|
||||
if (isHoming) return;
|
||||
setHomingActive(true);
|
||||
UI.log('⧐ Homing started — arm moving to mechanical stops…', 'warn');
|
||||
|
||||
if (BLE.isConnected()) {
|
||||
try {
|
||||
await BLE.write('HOME');
|
||||
// onHomingComplete() will be called when HOMED notify arrives
|
||||
} catch (e) {
|
||||
UI.log(`BLE error during home: ${e.message}`, 'error');
|
||||
setHomingActive(false);
|
||||
}
|
||||
} else {
|
||||
// Simulation: instant reset
|
||||
UI.log('[sim] HOME command sent — simulating instant home', 'info');
|
||||
setTimeout(onHomingComplete, 600);
|
||||
}
|
||||
}
|
||||
|
||||
// ─────────────────────────────────────────────────────────────────
|
||||
// SVG workspace zones (pre-computed)
|
||||
// ─────────────────────────────────────────────────────────────────
|
||||
@@ -235,15 +290,15 @@ function buildZones() {
|
||||
const bw = (L.X_MAX - L.X_MIN) * SV.SCALE;
|
||||
const bh = (L.Y_MAX - L.Y_MIN) * SV.SCALE;
|
||||
|
||||
// Mechanism exclusion box
|
||||
// Mechanism exclusion box (uses updated LIMITS fields)
|
||||
const ex = SV.wx(-L.BOX_HALF_W);
|
||||
const ey = SV.wy(L.BOX_Y_MAX);
|
||||
const ew = L.BOX_HALF_W * 2 * SV.SCALE;
|
||||
const eh = (L.BOX_Y_MAX - L.BOX_Y_MIN) * SV.SCALE;
|
||||
|
||||
// Motor positions
|
||||
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
|
||||
@@ -345,6 +400,20 @@ function buildHTML() {
|
||||
.quick-steps { display:flex; gap:4px; flex-wrap:wrap; }
|
||||
.quick-steps button { flex:1; min-width:30px; padding:4px 2px; font-size:0.68rem;
|
||||
border-radius:var(--radius-xs); }
|
||||
|
||||
/* Disable all interactive elements during homing */
|
||||
.homing-active button:not(#btn-find-home),
|
||||
.homing-active input,
|
||||
.homing-active #scara-svg {
|
||||
pointer-events: none;
|
||||
opacity: 0.4;
|
||||
}
|
||||
/* Spinner animation for homing button */
|
||||
@keyframes spin { to { transform: rotate(360deg); } }
|
||||
.home-spin { display:inline-block; width:13px; height:13px;
|
||||
border:2px solid currentColor; border-top-color:transparent;
|
||||
border-radius:50%; animation:spin 0.7s linear infinite;
|
||||
vertical-align:middle; margin-right:5px; }
|
||||
</style>
|
||||
|
||||
<div class="section-page fade-in">
|
||||
@@ -394,31 +463,31 @@ function buildHTML() {
|
||||
font-family="monospace" style="display:none"/>
|
||||
|
||||
<!-- ── Real arm ──────────────────────────────────── -->
|
||||
<!-- Arm 1: proximal + distal -->
|
||||
<!-- 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 -->
|
||||
<!-- 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)}"
|
||||
x2="${SV.wx(0)}" y2="${SV.wy(IK.ARM.l1 + IK.ARM.l2)}"
|
||||
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)}"/>
|
||||
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)}"/>
|
||||
cx="${SV.wx(-IK.ARM.d2)}" cy="${SV.wy(IK.ARM.l1)}"/>
|
||||
|
||||
<!-- End effector -->
|
||||
<circle id="end-eff" r="7"
|
||||
@@ -544,6 +613,21 @@ function buildHTML() {
|
||||
<span class="card-title">Manual Jog</span>
|
||||
<span style="font-size:0.72rem;color:var(--text-muted)">Incremental step control — bypasses IK</span>
|
||||
</div>
|
||||
|
||||
<!-- Find Home -->
|
||||
<div style="padding:4px 0 14px;border-bottom:1px solid var(--border);margin-bottom:12px">
|
||||
<div style="font-size:0.70rem;color:var(--text-muted);margin-bottom:8px;line-height:1.5">
|
||||
⚠️ Drives each arm slowly into its mechanical stop then backs off to
|
||||
operating home. Takes ~15–20 s. All controls are locked during homing.
|
||||
</div>
|
||||
<button class="btn btn-full" id="btn-find-home"
|
||||
style="background:rgba(255,152,0,0.12);border:1px solid rgba(255,152,0,0.35);
|
||||
color:var(--accent-amber);font-weight:700;font-size:0.85rem;padding:10px">
|
||||
<span class="home-spin" id="home-spinner" style="display:none"></span>
|
||||
<span id="home-label">⌂ Find Home</span>
|
||||
</button>
|
||||
</div>
|
||||
|
||||
<div class="jog-compact">
|
||||
<!-- Motor 1 -->
|
||||
<div class="jog-motor">
|
||||
@@ -619,7 +703,7 @@ const StepperTestSection = {
|
||||
ghost2Dist = document.getElementById('ghost-2d');
|
||||
ghostElbow1 = document.getElementById('ghost-e1');
|
||||
ghostElbow2 = document.getElementById('ghost-e2');
|
||||
ghostEnd = document.getElementById('ghost-end');
|
||||
ghostEnd = document.getElementById('ghost-e1');
|
||||
ghostLabel = document.getElementById('ghost-label');
|
||||
targetMarker= document.getElementById('target-marker');
|
||||
|
||||
@@ -734,6 +818,11 @@ const StepperTestSection = {
|
||||
// ── BLE NOTIFY position feedback ──────────────────────────────
|
||||
function onBLEStatus(e) {
|
||||
const msg = e.detail;
|
||||
if (msg === 'HOMED') {
|
||||
// Firmware completed homing — sync browser counters
|
||||
onHomingComplete();
|
||||
return;
|
||||
}
|
||||
if (msg.startsWith('P:')) {
|
||||
const [s1, s2] = msg.slice(2).split(',').map(Number);
|
||||
steps1 = s1; steps2 = s2;
|
||||
@@ -742,6 +831,9 @@ const StepperTestSection = {
|
||||
}
|
||||
document.addEventListener('ble:status', onBLEStatus);
|
||||
|
||||
// ── Find Home button ──────────────────────────────────────
|
||||
document.getElementById('btn-find-home').addEventListener('click', startHoming);
|
||||
|
||||
eventCleanup = [
|
||||
() => BLE.off('connected', updateBadge),
|
||||
() => BLE.off('disconnected', updateBadge),
|
||||
|
||||
Reference in New Issue
Block a user