kinematic motion controller
This commit is contained in:
osiu97
@@ -176,6 +176,14 @@ The sidebar is in `index.html` (not in any section). It is a collapsible panel o
|
||||
hamburger (`☰`) button. It contains `<button data-route="...">` nav items that call
|
||||
`Router.navigate(route)`. The active item gets `class="active"` via the router.
|
||||
|
||||
### Kinematic Motion Controller (`web/js/motion.js`)
|
||||
|
||||
Centralised trajectory generation and streaming module.
|
||||
- **Why**: Moving side-to-side linearly in joint space causes the arms to arc dangerously out-of-bounds at the top.
|
||||
- **How it works**: Intercepts `Motion.goto(x, y)`, slices the Cartesian straight line into small segments (max `25 mm` by default, configurable in `Motion.CONFIG.SEGMENT_SIZE_MM`), and streams them to the ESP32.
|
||||
- **Dynamic Blending**: It monitors `P:` positional updates from the ESP32 and fires the next waypoint *before* `AccelStepper` decelerates (when within `BLEND_THRESHOLD_STEPS`). This produces a seamless Cartesian trajectory with no MCU code changes and allows for future "visual flavor" movement effects (e.g. snaky, erratic).
|
||||
- **Simulation Sync**: In simulation mode, `Motion.goto()` instantly updates its internal steps and manually dispatches a fake `ble:status` event containing `P:s1,s2`. This allows all UI components to effortlessly sync their visual state using their existing BLE listeners.
|
||||
|
||||
---
|
||||
|
||||
## 6. Kinematics (`web/js/kinematics.js`)
|
||||
|
||||
@@ -0,0 +1,186 @@
|
||||
import BLE from './ble.js';
|
||||
import IK from './kinematics.js';
|
||||
import UI from './ui.js';
|
||||
|
||||
const CONFIG = {
|
||||
SEGMENT_SIZE_MM: 25, // Maximum distance between waypoints to prevent out-of-bounds joint-space arcs
|
||||
BLEND_THRESHOLD_STEPS: 300 // Send next waypoint when within this many steps (prevents deceleration)
|
||||
};
|
||||
|
||||
// Global state tracking
|
||||
let steps1 = 0;
|
||||
let steps2 = 0;
|
||||
|
||||
// Listen to BLE position updates
|
||||
document.addEventListener('ble:status', (e) => {
|
||||
const msg = e.detail;
|
||||
if (msg.startsWith('P:')) {
|
||||
const [s1, s2] = msg.slice(2).split(',').map(Number);
|
||||
steps1 = s1;
|
||||
steps2 = s2;
|
||||
}
|
||||
});
|
||||
|
||||
// To allow UI sections to override/sync steps initially
|
||||
function setSteps(s1, s2) {
|
||||
steps1 = s1;
|
||||
steps2 = s2;
|
||||
}
|
||||
|
||||
function getSteps() {
|
||||
return { steps1, steps2 };
|
||||
}
|
||||
|
||||
let motionQueue = [];
|
||||
let isMoving = false;
|
||||
let currentResolve = null;
|
||||
let queueCheckInterval = null;
|
||||
|
||||
async function executeQueue() {
|
||||
if (motionQueue.length === 0) {
|
||||
isMoving = false;
|
||||
if (queueCheckInterval) clearInterval(queueCheckInterval);
|
||||
if (currentResolve) currentResolve(true);
|
||||
currentResolve = null;
|
||||
return;
|
||||
}
|
||||
|
||||
isMoving = true;
|
||||
let target = motionQueue.shift();
|
||||
|
||||
const cmd = `X:${target.s1},${target.s2}`;
|
||||
if (BLE.isConnected()) {
|
||||
try {
|
||||
await BLE.write(cmd);
|
||||
} catch (e) {
|
||||
UI.log(`BLE error: ${e.message}`, 'error');
|
||||
motionQueue = []; // clear queue on error
|
||||
isMoving = false;
|
||||
if (queueCheckInterval) clearInterval(queueCheckInterval);
|
||||
if (currentResolve) currentResolve(false);
|
||||
currentResolve = null;
|
||||
return;
|
||||
}
|
||||
|
||||
// Setup interval to monitor progress
|
||||
if (queueCheckInterval) clearInterval(queueCheckInterval);
|
||||
queueCheckInterval = setInterval(async () => {
|
||||
// Calculate distance to target in steps
|
||||
const dist1 = Math.abs(target.s1 - steps1);
|
||||
const dist2 = Math.abs(target.s2 - steps2);
|
||||
const maxDist = Math.max(dist1, dist2);
|
||||
|
||||
if (motionQueue.length > 0) {
|
||||
// We have more waypoints. Blend!
|
||||
if (target.mode === 'direct') {
|
||||
if (maxDist <= CONFIG.BLEND_THRESHOLD_STEPS) {
|
||||
clearInterval(queueCheckInterval);
|
||||
executeQueue(); // start next waypoint early
|
||||
}
|
||||
} else if (target.mode === 'erratic') {
|
||||
if (maxDist <= 5) { // wait for full stop
|
||||
clearInterval(queueCheckInterval);
|
||||
setTimeout(executeQueue, 150); // slight pause for erratic effect
|
||||
}
|
||||
} else {
|
||||
// Default to direct blending
|
||||
if (maxDist <= CONFIG.BLEND_THRESHOLD_STEPS) {
|
||||
clearInterval(queueCheckInterval);
|
||||
executeQueue();
|
||||
}
|
||||
}
|
||||
} else {
|
||||
// Last waypoint. Wait until closely reached.
|
||||
if (maxDist <= 10) {
|
||||
clearInterval(queueCheckInterval);
|
||||
motionQueue = [];
|
||||
executeQueue(); // Will trigger resolve
|
||||
}
|
||||
}
|
||||
}, 40); // check ~25 times a second
|
||||
|
||||
} else {
|
||||
// Simulation: instantly jump to final target
|
||||
const finalTarget = motionQueue.length > 0 ? motionQueue[motionQueue.length - 1] : target;
|
||||
steps1 = finalTarget.s1;
|
||||
steps2 = finalTarget.s2;
|
||||
console.log(`[sim] Jumped to X:${steps1},${steps2} via trajectory`);
|
||||
|
||||
// Trigger pseudo-status event for the UI to update markers
|
||||
document.dispatchEvent(new CustomEvent('ble:status', { detail: `P:${steps1},${steps2}` }));
|
||||
|
||||
motionQueue = [];
|
||||
isMoving = false;
|
||||
if (currentResolve) {
|
||||
currentResolve(true);
|
||||
currentResolve = null;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Moves the arm to Cartesian targetX, targetY.
|
||||
* Dynamically slices the path into segments to prevent out-of-bounds arcs.
|
||||
* @returns {Promise<boolean>} Resolves to true if successful, false if aborted/error.
|
||||
*/
|
||||
async function goto(targetX, targetY, mode = 'direct') {
|
||||
if (isMoving) {
|
||||
UI.log('Arm is currently moving! Wait for it to finish.', 'warn');
|
||||
return false;
|
||||
}
|
||||
|
||||
// Calculate current Cartesian position
|
||||
const t1 = IK.stepsToRad(steps1);
|
||||
const t2 = IK.stepsToRad(steps2);
|
||||
const currentPos = IK.forward(t1, t2);
|
||||
|
||||
if (!currentPos.valid || !isFinite(currentPos.endX) || !isFinite(currentPos.endY)) {
|
||||
UI.log('Cannot calculate current IK position (arms disconnected?). Force Home needed.', 'error');
|
||||
return false;
|
||||
}
|
||||
|
||||
const dx = targetX - currentPos.endX;
|
||||
const dy = targetY - currentPos.endY;
|
||||
const distance = Math.sqrt(dx * dx + dy * dy);
|
||||
|
||||
let numSegments = Math.ceil(distance / CONFIG.SEGMENT_SIZE_MM);
|
||||
if (numSegments === 0) numSegments = 1;
|
||||
|
||||
motionQueue = [];
|
||||
|
||||
for (let i = 1; i <= numSegments; i++) {
|
||||
const fraction = i / numSegments;
|
||||
const px = currentPos.endX + dx * fraction;
|
||||
const py = currentPos.endY + dy * fraction;
|
||||
|
||||
const wsCheck = IK.checkWorkspace(px, py);
|
||||
if (!wsCheck.ok) {
|
||||
UI.log(`⛔ Waypoint blocked: ${wsCheck.reason}`, 'error');
|
||||
return false; // abort full move
|
||||
}
|
||||
|
||||
const res = IK.solve(px, py);
|
||||
if (!res.reachable || IK.armsCrossed(res.theta1, res.theta2)) {
|
||||
UI.log(`Waypoint geometrically unreachable.`, 'error');
|
||||
return false;
|
||||
}
|
||||
|
||||
const s1 = IK.radToStepsAbsolute(res.theta1, 1);
|
||||
const s2 = IK.radToStepsAbsolute(res.theta2, 2);
|
||||
|
||||
motionQueue.push({ s1, s2, mode });
|
||||
}
|
||||
|
||||
return new Promise((resolve) => {
|
||||
currentResolve = resolve;
|
||||
executeQueue();
|
||||
});
|
||||
}
|
||||
|
||||
export default {
|
||||
CONFIG,
|
||||
goto,
|
||||
setSteps,
|
||||
getSteps,
|
||||
get isMoving() { return isMoving; }
|
||||
};
|
||||
@@ -2,6 +2,7 @@ import BLE from '../ble.js';
|
||||
import UI from '../ui.js';
|
||||
import IK from '../kinematics.js';
|
||||
import Settings from '../settings.js';
|
||||
import Motion from '../motion.js';
|
||||
import { buildQuickSequencesHTML, initQuickSequences } from '../quick-sequences.js';
|
||||
|
||||
let eventCleanup = [];
|
||||
@@ -16,6 +17,7 @@ function syncStateFromStorage() {
|
||||
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;
|
||||
Motion.setSteps(steps1, steps2);
|
||||
}
|
||||
|
||||
function saveSteps() {
|
||||
@@ -255,54 +257,8 @@ async function executeMove(spot) {
|
||||
}
|
||||
|
||||
const mode = document.getElementById('mode-select').value;
|
||||
if (mode !== 'direct') {
|
||||
UI.log(`Mode '${mode}' not fully implemented yet! Using direct move.`, 'warn');
|
||||
}
|
||||
|
||||
// Calculate IK
|
||||
const wsCheck = IK.checkWorkspace(spot.x, spot.y);
|
||||
if (!wsCheck.ok) {
|
||||
UI.log(`⛔ ${wsCheck.reason}`, 'error');
|
||||
return;
|
||||
}
|
||||
|
||||
const res = IK.solve(spot.x, spot.y);
|
||||
if (!res.reachable) {
|
||||
UI.log(`Target ${spot.label} is unreachable geometrically.`, 'error');
|
||||
return;
|
||||
}
|
||||
|
||||
if (IK.armsCrossed(res.theta1, res.theta2)) {
|
||||
UI.log(`Target ${spot.label} rejected: elbows crossed!`, 'error');
|
||||
return;
|
||||
}
|
||||
|
||||
const newSteps1 = IK.radToStepsAbsolute(res.theta1, 1);
|
||||
const newSteps2 = IK.radToStepsAbsolute(res.theta2, 2);
|
||||
const delta1 = newSteps1 - steps1;
|
||||
const delta2 = newSteps2 - steps2;
|
||||
|
||||
const cmd1 = `S1${delta1 >= 0 ? '+' : ''}${delta1}`;
|
||||
const cmd2 = `S2${delta2 >= 0 ? '+' : ''}${delta2}`;
|
||||
|
||||
UI.log(`Moving to ${spot.label} (${spot.x}, ${spot.y})`, 'success');
|
||||
|
||||
if (BLE.isConnected()) {
|
||||
try {
|
||||
await BLE.write(cmd1);
|
||||
await BLE.write(cmd2);
|
||||
} catch (e) {
|
||||
UI.log(`BLE error: ${e.message}`, 'error');
|
||||
}
|
||||
} else {
|
||||
steps1 = newSteps1;
|
||||
steps2 = newSteps2;
|
||||
saveSteps();
|
||||
updatePositionReadout();
|
||||
const simMsg = `[sim] ${cmd1} ${cmd2}`;
|
||||
console.log(simMsg);
|
||||
UI.log(simMsg, 'info');
|
||||
}
|
||||
UI.log(`Moving to ${spot.label} (${spot.x}, ${spot.y}) [${mode}]`, 'success');
|
||||
await Motion.goto(spot.x, spot.y, mode);
|
||||
}
|
||||
|
||||
export default {
|
||||
@@ -369,6 +325,7 @@ export default {
|
||||
} else {
|
||||
steps1 = IK.ARM.HOME_STEPS.m1;
|
||||
steps2 = IK.ARM.HOME_STEPS.m2;
|
||||
Motion.setSteps(steps1, steps2);
|
||||
saveSteps();
|
||||
updatePositionReadout();
|
||||
UI.log('[sim] HOMEALL', 'info');
|
||||
|
||||
@@ -2,6 +2,7 @@ import BLE from '../ble.js';
|
||||
import UI from '../ui.js';
|
||||
import IK from '../kinematics.js';
|
||||
import Settings from '../settings.js';
|
||||
import Motion from '../motion.js';
|
||||
import { buildQuickSequencesHTML, initQuickSequences } from '../quick-sequences.js';
|
||||
|
||||
let eventCleanup = [];
|
||||
@@ -17,6 +18,7 @@ function syncStateFromStorage() {
|
||||
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;
|
||||
Motion.setSteps(steps1, steps2);
|
||||
}
|
||||
|
||||
function saveSteps() {
|
||||
@@ -263,44 +265,8 @@ function delay(ms) {
|
||||
}
|
||||
|
||||
async function executeMove(spot) {
|
||||
const wsCheck = IK.checkWorkspace(spot.x, spot.y);
|
||||
if (!wsCheck.ok) {
|
||||
UI.log(`⛔ ${wsCheck.reason}`, 'error');
|
||||
return false;
|
||||
}
|
||||
|
||||
const res = IK.solve(spot.x, spot.y);
|
||||
if (!res.reachable || IK.armsCrossed(res.theta1, res.theta2)) {
|
||||
UI.log(`Target ${spot.label} is unreachable or crosses arms.`, 'error');
|
||||
return false;
|
||||
}
|
||||
|
||||
const newSteps1 = IK.radToStepsAbsolute(res.theta1, 1);
|
||||
const newSteps2 = IK.radToStepsAbsolute(res.theta2, 2);
|
||||
const delta1 = newSteps1 - steps1;
|
||||
const delta2 = newSteps2 - steps2;
|
||||
|
||||
const cmd1 = `S1${delta1 >= 0 ? '+' : ''}${delta1}`;
|
||||
const cmd2 = `S2${delta2 >= 0 ? '+' : ''}${delta2}`;
|
||||
|
||||
UI.log(`Spelling: ${spot.label}`, 'info');
|
||||
|
||||
if (BLE.isConnected()) {
|
||||
try {
|
||||
await BLE.write(cmd1);
|
||||
await BLE.write(cmd2);
|
||||
} catch (e) {
|
||||
UI.log(`BLE error: ${e.message}`, 'error');
|
||||
return false;
|
||||
}
|
||||
} else {
|
||||
steps1 = newSteps1;
|
||||
steps2 = newSteps2;
|
||||
saveSteps();
|
||||
updatePosMarker();
|
||||
console.log(`[sim] ${cmd1} ${cmd2}`);
|
||||
}
|
||||
return true;
|
||||
return await Motion.goto(spot.x, spot.y);
|
||||
}
|
||||
|
||||
async function spellText(text) {
|
||||
@@ -414,6 +380,7 @@ export default {
|
||||
} else {
|
||||
steps1 = IK.ARM.HOME_STEPS.m1;
|
||||
steps2 = IK.ARM.HOME_STEPS.m2;
|
||||
Motion.setSteps(steps1, steps2);
|
||||
saveSteps();
|
||||
updatePosMarker();
|
||||
UI.log('[sim] HOMEALL', 'info');
|
||||
|
||||
@@ -15,6 +15,7 @@
|
||||
import BLE from '../ble.js';
|
||||
import IK from '../kinematics.js';
|
||||
import UI from '../ui.js';
|
||||
import Motion from '../motion.js';
|
||||
|
||||
// ── SVG viewport ──────────────────────────────────────────────────
|
||||
// We map the real workspace onto the SVG canvas.
|
||||
@@ -59,6 +60,7 @@ function syncStateFromStorage() {
|
||||
steps1 = 1024;
|
||||
localStorage.setItem('wiji_steps1', 1024);
|
||||
}
|
||||
Motion.setSteps(steps1, steps2);
|
||||
}
|
||||
|
||||
function saveSteps() {
|
||||
@@ -186,36 +188,14 @@ function showTargetMarker(sx, sy) {
|
||||
// ─────────────────────────────────────────────────────────────────
|
||||
|
||||
async function moveToXY(targetX, targetY) {
|
||||
// Check workspace limits first
|
||||
// Check workspace limits first (quick check before queue)
|
||||
const wsCheck = IK.checkWorkspace(targetX, targetY);
|
||||
if (!wsCheck.ok) {
|
||||
UI.log(`⛔ ${wsCheck.reason}`, 'warn');
|
||||
return false;
|
||||
}
|
||||
|
||||
const { theta1, theta2 } = IK.solve(targetX, targetY);
|
||||
|
||||
const newSteps1 = IK.radToStepsAbsolute(theta1, 1);
|
||||
const newSteps2 = IK.radToStepsAbsolute(theta2, 2);
|
||||
|
||||
const cmd = `X:${newSteps1},${newSteps2}`;
|
||||
|
||||
if (BLE.isConnected()) {
|
||||
try {
|
||||
await BLE.write(cmd);
|
||||
// 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)}° | ${cmd}`;
|
||||
console.log(simMsg);
|
||||
UI.log(simMsg, 'info');
|
||||
}
|
||||
return true;
|
||||
return await Motion.goto(targetX, targetY);
|
||||
}
|
||||
|
||||
// ─────────────────────────────────────────────────────────────────
|
||||
@@ -229,6 +209,7 @@ async function jogMotor(motor, delta) {
|
||||
} else {
|
||||
if (motor === 1) steps1 += delta;
|
||||
else steps2 += delta;
|
||||
Motion.setSteps(steps1, steps2);
|
||||
renderArmFromSteps();
|
||||
console.log(`[sim] ${cmd}`);
|
||||
UI.log(`[sim] ${cmd}`, 'info');
|
||||
@@ -249,6 +230,7 @@ async function zeroMotor(motor) {
|
||||
} else {
|
||||
steps2 = IK.ARM.HOME_STEPS.m2;
|
||||
}
|
||||
Motion.setSteps(steps1, steps2);
|
||||
renderArmFromSteps();
|
||||
console.log(`[sim] ${cmd}`);
|
||||
UI.log(`[sim] ${cmd}`, 'info');
|
||||
@@ -752,6 +734,7 @@ const StepperTestSection = {
|
||||
} else {
|
||||
steps1 = IK.ARM.HOME_STEPS.m1;
|
||||
steps2 = IK.ARM.HOME_STEPS.m2;
|
||||
Motion.setSteps(steps1, steps2);
|
||||
renderArmFromSteps();
|
||||
console.log(`[sim] ZEROALL`);
|
||||
UI.log(`[sim] ZEROALL (Reset Position)`, 'info');
|
||||
|
||||
+10
-29
@@ -2,6 +2,7 @@ import BLE from './ble.js';
|
||||
import UI from './ui.js';
|
||||
import IK from './kinematics.js';
|
||||
import Settings from './settings.js';
|
||||
import Motion from './motion.js';
|
||||
|
||||
let isRunning = false;
|
||||
|
||||
@@ -52,41 +53,21 @@ export default {
|
||||
continue;
|
||||
}
|
||||
|
||||
const wsCheck = IK.checkWorkspace(targetX, targetY);
|
||||
if (!wsCheck.ok) {
|
||||
UI.log(`⛔ ${wsCheck.reason} [${label}]`, 'error');
|
||||
continue; // or break? let's continue
|
||||
}
|
||||
|
||||
const res = IK.solve(targetX, targetY);
|
||||
if (!res.reachable || IK.armsCrossed(res.theta1, res.theta2)) {
|
||||
UI.log(`Target ${label} is unreachable or crosses arms.`, 'error');
|
||||
continue;
|
||||
}
|
||||
|
||||
const newSteps1 = IK.radToStepsAbsolute(res.theta1, 1);
|
||||
const newSteps2 = IK.radToStepsAbsolute(res.theta2, 2);
|
||||
|
||||
const cmd = `X:${newSteps1},${newSteps2}`;
|
||||
|
||||
UI.log(`Seq step: ${label}`, 'info');
|
||||
|
||||
if (BLE.isConnected()) {
|
||||
try {
|
||||
await BLE.write(cmd);
|
||||
} catch (e) {
|
||||
UI.log(`BLE error: ${e.message}`, 'error');
|
||||
isRunning = false;
|
||||
return false;
|
||||
}
|
||||
} else {
|
||||
steps1 = newSteps1;
|
||||
steps2 = newSteps2;
|
||||
const success = await Motion.goto(targetX, targetY, step.mode || 'direct');
|
||||
if (!success) {
|
||||
isRunning = false;
|
||||
return false;
|
||||
}
|
||||
|
||||
if (!BLE.isConnected()) {
|
||||
const st = Motion.getSteps();
|
||||
steps1 = st.steps1;
|
||||
steps2 = st.steps2;
|
||||
if (onStepComplete) {
|
||||
onStepComplete(steps1, steps2);
|
||||
}
|
||||
console.log(`[sim] ${cmd}`);
|
||||
}
|
||||
|
||||
// Wait for delay
|
||||
|
||||
Reference in New Issue
Block a user