diff --git a/gemini.md b/gemini.md index 0748218..24cb260 100644 --- a/gemini.md +++ b/gemini.md @@ -91,8 +91,9 @@ Name: WijiBoard | `S2-` | Move motor 2 CCW by n steps | | `SPD:` | Set max speed (steps/sec) for both motors | | `ACC:` | Set acceleration (steps/sec²) for both motors | -| `HOME1` | Zero motor 1 step counter | -| `HOME2` | Zero motor 2 step counter | +| `HOMEALL` | Home both motors simultaneously | +| `HOME1` | Home motor 1 | +| `HOME2` | Home motor 2 | | `POS` | Request current positions (triggers NOTIFY) | ### Status notifications (firmware → browser) @@ -424,10 +425,9 @@ 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**: `steps1 = 0, steps2 = 0` is wherever the firmware powers up (no homing - routine implemented yet). The arm must be manually set to a known starting configuration - before the IK will be accurate. Suggested home: arms roughly parallel, EE near centre of - the working area. +- **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. + - 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. - **LIMITS tuning**: All values in `LIMITS` in `kinematics.js` are estimates based on the original project's data. They need physical verification: diff --git a/src/main.cpp b/src/main.cpp index 5a691da..e5fca93 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -12,8 +12,9 @@ * S2- → Step motor 2 CCW n steps * SPD: → Set max speed for both motors (steps/sec) * ACC: → Set acceleration for both motors (steps/sec²) - * HOME1 → Zero motor 1 position - * HOME2 → Zero motor 2 position + * HOME1 → Home motor 1 (Right) + * HOME2 → Home motor 2 (Left) + * HOMEALL → Home both motors simultaneously * POS → Request current positions (triggers NOTIFY) * * BLE Status Characteristic (NOTIFY): @@ -73,6 +74,9 @@ const unsigned long NOTIFY_INTERVAL_MS = 200; // ─── Forward declarations ───────────────────────────────────────── void parseCommand(const String& cmd); void sendPosition(); +void performHomingAll(); +void performHoming1(); +void performHoming2(); // ─── BLE Server Callbacks ───────────────────────────────────────── class ServerCallbacks : public BLEServerCallbacks { @@ -132,15 +136,22 @@ void parseCommand(const String& cmd) { } // HOME1 if (cmd == "HOME1") { - stepper1.setCurrentPosition(0); - Serial.println("[S1] Zeroed"); + Serial.println("[S1] Homing Motor 1..."); + performHoming1(); sendPosition(); return; } // HOME2 if (cmd == "HOME2") { - stepper2.setCurrentPosition(0); - Serial.println("[S2] Zeroed"); + Serial.println("[S2] Homing Motor 2..."); + performHoming2(); + sendPosition(); + return; + } + // HOMEALL + if (cmd == "HOMEALL") { + Serial.println("[SYS] Homing both motors..."); + performHomingAll(); sendPosition(); return; } @@ -233,3 +244,77 @@ void loop() { } } } + +// ─── Homing Routines ────────────────────────────────────────────── + +void performHomingAll() { + // 1. Move against limits (M1 CCW, M2 CCW) + stepper2.moveTo(1024); + stepper1.moveTo(2048); + while (stepper1.distanceToGo() != 0 || stepper2.distanceToGo() != 0) { + stepper1.run(); + stepper2.run(); + } + stepper1.setCurrentPosition(0); + + // 2. Move to negative limits (CW) + stepper2.moveTo(-1050); + stepper1.move(-1300); // equivalent to moveTo(-1300) since pos is 0 + while (stepper1.distanceToGo() != 0 || stepper2.distanceToGo() != 0) { + stepper1.run(); + stepper2.run(); + } + stepper2.setCurrentPosition(0); + + // 3. Move to final home position + stepper2.moveTo(550); + stepper1.moveTo(-530); + while (stepper1.distanceToGo() != 0 || stepper2.distanceToGo() != 0) { + stepper1.run(); + stepper2.run(); + } + + // 4. Set origin relative to this final position + stepper2.setCurrentPosition(-1024); + stepper1.setCurrentPosition(0); + + stepper1.disableOutputs(); + stepper2.disableOutputs(); + Serial.println("[SYS] Homing all complete."); +} + +void performHoming1() { + // M1 (Right) stalls going positive (CCW) + stepper1.move(2048); + while (stepper1.distanceToGo() != 0) { + stepper1.run(); + } + stepper1.setCurrentPosition(0); + + // Move to final home position (CW) + stepper1.moveTo(-530); + while (stepper1.distanceToGo() != 0) { + stepper1.run(); + } + stepper1.setCurrentPosition(0); + stepper1.disableOutputs(); + Serial.println("[S1] Homing complete."); +} + +void performHoming2() { + // M2 (Left) stalls going negative (CW) + stepper2.move(-2048); + while (stepper2.distanceToGo() != 0) { + stepper2.run(); + } + stepper2.setCurrentPosition(0); + + // Move to final home position (CCW) + stepper2.moveTo(550); + while (stepper2.distanceToGo() != 0) { + stepper2.run(); + } + stepper2.setCurrentPosition(-1024); + stepper2.disableOutputs(); + Serial.println("[S2] Homing complete."); +} diff --git a/web/js/kinematics.js b/web/js/kinematics.js index ab5b8c1..5c8d8d0 100644 --- a/web/js/kinematics.js +++ b/web/js/kinematics.js @@ -39,6 +39,7 @@ const ARM = { 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) }; // ── WORKSPACE CONSTRAINTS ───────────────────────────────────────── diff --git a/web/js/sections/stepper-test.js b/web/js/sections/stepper-test.js index 6f6ce96..f7a6315 100644 --- a/web/js/sections/stepper-test.js +++ b/web/js/sections/stepper-test.js @@ -45,8 +45,8 @@ const PX_L2 = IK.ARM.l2 * SV.SCALE; const PX_D2 = IK.ARM.d2 * SV.SCALE; // ── Section state ───────────────────────────────────────────────── -let steps1 = 0; -let steps2 = 0; +let steps1 = IK.ARM.HOME_STEPS.m1; +let steps2 = IK.ARM.HOME_STEPS.m2; let eventCleanup = []; // ── SVG element refs ────────────────────────────────────────────── @@ -215,10 +215,16 @@ async function jogMotor(motor, delta) { } async function zeroMotor(motor) { - if (motor === 1) steps1 = 0; - else steps2 = 0; + if (motor === 'ALL') { + steps1 = IK.ARM.HOME_STEPS.m1; + steps2 = IK.ARM.HOME_STEPS.m2; + } else if (motor === 1) { + steps1 = IK.ARM.HOME_STEPS.m1; + } else { + steps2 = IK.ARM.HOME_STEPS.m2; + } renderArmFromSteps(); - const cmd = `HOME${motor}`; + const cmd = motor === 'ALL' ? 'HOMEALL' : `HOME${motor}`; BLE.isConnected() ? await BLE.write(cmd).catch(e => UI.log(e.message, 'error')) : UI.log(`[sim] ${cmd}`, 'info'); @@ -568,7 +574,7 @@ function buildHTML() { ${[10,50,100,512].map(n=>``).join('')} ${[10,50,100,512].map(n=>``).join('')} - +
@@ -584,9 +590,12 @@ function buildHTML() { ${[10,50,100,512].map(n=>``).join('')} ${[10,50,100,512].map(n=>``).join('')}
- + +
+ +
@@ -698,6 +707,7 @@ const StepperTestSection = { document.getElementById('s2-ccw').addEventListener('click', () => jogMotor(2, -gs(2))); document.getElementById('s1-zero').addEventListener('click', () => zeroMotor(1)); document.getElementById('s2-zero').addEventListener('click', () => zeroMotor(2)); + document.getElementById('sall-zero').addEventListener('click', () => zeroMotor('ALL')); // ── Quick-step buttons ──────────────────────────────────────── container.querySelectorAll('[data-motor][data-steps]').forEach(btn => {