added homing.
This commit is contained in:
PROFERIS - Mi³osz Stocki
@@ -91,8 +91,9 @@ Name: WijiBoard
|
||||
| `S2-<n>` | Move motor 2 CCW by n steps |
|
||||
| `SPD:<n>` | Set max speed (steps/sec) for both motors |
|
||||
| `ACC:<n>` | 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:
|
||||
|
||||
+91
-6
@@ -12,8 +12,9 @@
|
||||
* S2-<n> → Step motor 2 CCW n steps
|
||||
* SPD:<n> → Set max speed for both motors (steps/sec)
|
||||
* ACC:<n> → 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.");
|
||||
}
|
||||
|
||||
@@ -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 ─────────────────────────────────────────
|
||||
|
||||
@@ -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=>`<button class="btn btn-ghost" data-motor="1" data-steps="${n}">+${n}</button>`).join('')}
|
||||
${[10,50,100,512].map(n=>`<button class="btn btn-ghost" data-motor="1" data-steps="-${n}">-${n}</button>`).join('')}
|
||||
</div>
|
||||
<button class="btn btn-danger btn-sm btn-full" id="s1-zero">Zero M1</button>
|
||||
<button class="btn btn-danger btn-sm btn-full" id="s1-zero">Home M1</button>
|
||||
</div>
|
||||
<!-- Motor 2 -->
|
||||
<div class="jog-motor">
|
||||
@@ -584,9 +590,12 @@ function buildHTML() {
|
||||
${[10,50,100,512].map(n=>`<button class="btn btn-ghost" data-motor="2" data-steps="${n}">+${n}</button>`).join('')}
|
||||
${[10,50,100,512].map(n=>`<button class="btn btn-ghost" data-motor="2" data-steps="-${n}">-${n}</button>`).join('')}
|
||||
</div>
|
||||
<button class="btn btn-danger btn-sm btn-full" id="s2-zero">Zero M2</button>
|
||||
<button class="btn btn-danger btn-sm btn-full" id="s2-zero">Home M2</button>
|
||||
</div>
|
||||
</div>
|
||||
<div style="margin-top: 12px;">
|
||||
<button class="btn btn-danger btn-full" id="sall-zero" style="padding: 10px; font-weight: bold; font-size: 0.9rem;">HOME BOTH MOTORS</button>
|
||||
</div>
|
||||
</div>
|
||||
|
||||
<!-- Event log -->
|
||||
@@ -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 => {
|
||||
|
||||
Reference in New Issue
Block a user