added proper kinematics
This commit is contained in:
PROFERIS - Mi³osz Stocki
@@ -0,0 +1 @@
|
||||
You didn't finish writing home for web. Analyse it properly as it was quota reached. I can see that there is definitely wron position on map at the start. I do not know what else but you should properly finish homing sequence it was done for mcu already
|
||||
@@ -0,0 +1,457 @@
|
||||
# WijiBoard – Project Context for AI Assistants
|
||||
|
||||
> **Purpose**: This file is the authoritative project memory. Read it in full before making
|
||||
> any changes. It captures hardware, architecture, current state, open work, and every
|
||||
> important design decision made so far.
|
||||
|
||||
---
|
||||
|
||||
## 1. Project Overview
|
||||
|
||||
WijiBoard is a **5-bar parallel linkage SCARA robot** (spirit-board / planchette mover) controlled
|
||||
wirelessly from a browser via **Web Bluetooth**. There is **no mobile app, no WiFi, no server** —
|
||||
only BLE between the ESP32 firmware and a local web SPA.
|
||||
|
||||
- **Inspiration / upstream**: `nerd-sniped/WijiBoard` on GitHub. We took *only* the kinematics
|
||||
and lookup tables from that project. Everything else (frontend, BLE protocol, SPA shell,
|
||||
section architecture) was written from scratch.
|
||||
|
||||
---
|
||||
|
||||
## 2. Hardware
|
||||
|
||||
| Component | Detail |
|
||||
|---|---|
|
||||
| **MCU** | Waveshare ESP32-S3-Zero |
|
||||
| **Framework** | PlatformIO + Arduino core |
|
||||
| **Motors** | 2× 28BYJ-48 (unipolar stepper via ULN2003 driver) |
|
||||
| **Motor mode** | `AccelStepper::FULL4WIRE` — 2048 steps/rev |
|
||||
| **Motor 1 pins** | IN1=4, IN2=5, IN3=6, IN4=7 |
|
||||
| **Motor 2 pins** | IN1=8, IN2=9, IN3=10, IN4=11 |
|
||||
| **USB Serial** | Native USB CDC (`-D ARDUINO_USB_MODE=1 -D ARDUINO_USB_CDC_ON_BOOT=1`) |
|
||||
| **Default speed** | 600 steps/s, accel 100 steps/s² |
|
||||
|
||||
### Mechanism geometry (critical — do NOT change without hardware verification)
|
||||
|
||||
```
|
||||
END EFFECTOR
|
||||
/ \
|
||||
l2 (110 mm) l2 (110 mm)
|
||||
/ \
|
||||
ELBOW1 ELBOW2
|
||||
\ /
|
||||
l1 (85 mm) l1 (85 mm)
|
||||
\ /
|
||||
MOTOR1 (+12.9,0) MOTOR2 (-12.9,0)
|
||||
| |
|
||||
[===BASE (25.8 mm)===]
|
||||
```
|
||||
|
||||
**⚠️ CRITICAL MOTOR CONVENTION** (source of many past bugs):
|
||||
|
||||
`PositionControl.cpp` from the original project uses:
|
||||
- `xmd = x - d2` → Motor 1 pivot is at **(+d2, 0) = (+12.9, 0)** — the RIGHT side
|
||||
- `xpd = x + d2` → Motor 2 pivot is at **(-d2, 0) = (-12.9, 0)** — the LEFT side
|
||||
|
||||
This is counter-intuitive. Motor 1 is physically on the right. **Every piece of code that
|
||||
computes elbow or motor-base positions must use M1 at +12.9 and M2 at -12.9 or the arms
|
||||
will appear visually crossed even for valid target positions.**
|
||||
|
||||
### Arm constants (from `PositionControl.cpp`)
|
||||
|
||||
```
|
||||
d = 25.8 mm (full motor separation)
|
||||
d2 = 12.9 mm (half separation)
|
||||
l1 = 85.0 mm (proximal link)
|
||||
l2 = 110.0 mm (distal link)
|
||||
STEPS_PER_REV = 2048
|
||||
STEP_ANGLE = 360/2048 ≈ 0.17578125 °/step
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## 3. BLE Protocol
|
||||
|
||||
### UUIDs (must be identical in firmware AND `web/js/ble.js`)
|
||||
|
||||
```
|
||||
Service: a0b1c2d3-e4f5-6789-abcd-ef0123456700
|
||||
Command: a0b1c2d3-e4f5-6789-abcd-ef0123456701 (WRITE | WRITE_NR)
|
||||
Status: a0b1c2d3-e4f5-6789-abcd-ef0123456702 (NOTIFY)
|
||||
Name: WijiBoard
|
||||
```
|
||||
|
||||
### Command format (ASCII strings, write-without-response)
|
||||
|
||||
| Command | Meaning |
|
||||
|---|---|
|
||||
| `S1+<n>` | Move motor 1 CW by n steps |
|
||||
| `S1-<n>` | Move motor 1 CCW by n steps |
|
||||
| `S2+<n>` | Move motor 2 CW by n steps |
|
||||
| `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 |
|
||||
| `POS` | Request current positions (triggers NOTIFY) |
|
||||
|
||||
### Status notifications (firmware → browser)
|
||||
|
||||
`P:<s1>,<s2>` — current step positions for motor 1 and 2, sent every 200 ms while moving.
|
||||
|
||||
---
|
||||
|
||||
## 4. Project Structure
|
||||
|
||||
```
|
||||
esp32s3-wiji/
|
||||
├── index.html # SPA shell — ES module bootstrap, sidebar, routing
|
||||
├── platformio.ini # PlatformIO config (env: esp32-s3-devkitc-1)
|
||||
├── fivebarIKGame.js # REFERENCE ONLY — original site's kinematics (do not import)
|
||||
├── src/
|
||||
│ └── main.cpp # ESP32 firmware (BLE + AccelStepper)
|
||||
└── web/
|
||||
├── styles/
|
||||
│ ├── base.css # CSS variables, reset, typography, layout primitives
|
||||
│ └── components.css # Cards, buttons, inputs, sidebar, nav items
|
||||
└── js/
|
||||
├── ble.js # Web Bluetooth singleton
|
||||
├── kinematics.js # 5-bar IK/FK (PositionControl.cpp port)
|
||||
├── router.js # Hash-based SPA router
|
||||
├── ui.js # UI helpers (log panel, toast, BLE status badge)
|
||||
└── sections/
|
||||
├── home.js # Home/landing section (placeholder)
|
||||
└── stepper-test.js # Stepper test section — SVG arm visualiser + jog + IK
|
||||
```
|
||||
|
||||
### Module loading (index.html)
|
||||
|
||||
All ES modules are loaded with a **cache-buster** to prevent stale code on refresh:
|
||||
|
||||
```javascript
|
||||
const V = Date.now();
|
||||
const { default: Router } = await import(`./web/js/router.js?v=${V}`);
|
||||
const { default: BLE } = await import(`./web/js/ble.js?v=${V}`);
|
||||
const { default: UI } = await import(`./web/js/ui.js?v=${V}`);
|
||||
const { default: HomeSection } = await import(`./web/js/sections/home.js?v=${V}`);
|
||||
const { default: StepperSection } = await import(`./web/js/sections/stepper-test.js?v=${V}`);
|
||||
```
|
||||
|
||||
> The `?v=${V}` suffix on ES module imports is mandatory. Without it, browsers aggressively
|
||||
> cache modules and you will see stale code running despite file changes.
|
||||
|
||||
---
|
||||
|
||||
## 5. SPA Architecture
|
||||
|
||||
### Router (`web/js/router.js`)
|
||||
|
||||
Hash-based SPA router. Sections are mounted/unmounted as the user navigates.
|
||||
|
||||
```javascript
|
||||
Router.register('home', HomeSection);
|
||||
Router.register('stepper-test', StepperSection);
|
||||
Router.init('#view', '#home');
|
||||
```
|
||||
|
||||
Each section module must export `{ mount(containerEl), unmount() }`.
|
||||
|
||||
### Section lifecycle
|
||||
|
||||
- `mount(el)` — called when navigating TO this section; injects HTML, sets up event listeners
|
||||
- `unmount()` — called when navigating AWAY; removes event listeners from `eventCleanup[]`
|
||||
|
||||
Pattern used in every section:
|
||||
|
||||
```javascript
|
||||
let eventCleanup = [];
|
||||
|
||||
export default {
|
||||
async mount(container) {
|
||||
container.innerHTML = buildHTML();
|
||||
// grab refs...
|
||||
const handler = (e) => { ... };
|
||||
el.addEventListener('click', handler);
|
||||
eventCleanup.push(() => el.removeEventListener('click', handler));
|
||||
renderArmFromSteps();
|
||||
},
|
||||
unmount() {
|
||||
eventCleanup.forEach(fn => fn());
|
||||
eventCleanup = [];
|
||||
},
|
||||
};
|
||||
```
|
||||
|
||||
### Sidebar
|
||||
|
||||
The sidebar is in `index.html` (not in any section). It is a collapsible panel opened by a
|
||||
hamburger (`☰`) button. It contains `<button data-route="...">` nav items that call
|
||||
`Router.navigate(route)`. The active item gets `class="active"` via the router.
|
||||
|
||||
---
|
||||
|
||||
## 6. Kinematics (`web/js/kinematics.js`)
|
||||
|
||||
### IK (`solve(x, y)`)
|
||||
|
||||
Exact port of `calculateInverseKinematics()` from `PositionControl.cpp`:
|
||||
|
||||
```javascript
|
||||
// Motor 1 (pivot at +d2 = +12.9 mm)
|
||||
const xmd = x - d2; // Δx from M1 pivot
|
||||
const s = hypot(xmd, y);
|
||||
const cosW1 = (l2² - s² - l1²) / (-2·l1·s);
|
||||
const theta1 = atan2(y, xmd) - acos(cosW1);
|
||||
|
||||
// Motor 2 (pivot at -d2 = -12.9 mm)
|
||||
const xpd = x + d2; // Δx from M2 pivot
|
||||
const t = hypot(xpd, y);
|
||||
const cosW2 = (l2² - t² - l1²) / (-2·l1·t);
|
||||
const theta2 = atan2(y, xpd) + acos(cosW2);
|
||||
```
|
||||
|
||||
Returns `{ theta1, theta2, reachable }` — angles in **radians**.
|
||||
|
||||
### FK (`forward(theta1, theta2)`)
|
||||
|
||||
Computes elbow and EE positions for visualisation:
|
||||
|
||||
```javascript
|
||||
// M1 at +d2, M2 at -d2 ← critical sign convention
|
||||
elbow1 = { x: +d2 + l1·cos(theta1), y: l1·sin(theta1) }
|
||||
elbow2 = { x: -d2 + l1·cos(theta2), y: l1·sin(theta2) }
|
||||
// EE = upward circle-circle intersection of circles (elbow1, l2) and (elbow2, l2)
|
||||
```
|
||||
|
||||
Returns `{ elbow1, elbow2, endX, endY, valid }`.
|
||||
|
||||
### Arm crossing check (`armsCrossed(theta1, theta2)`)
|
||||
|
||||
Prevents moves that would push an elbow into the mechanism housing from the wrong side.
|
||||
Based on elbow position relative to tunable thresholds (see LIMITS below).
|
||||
|
||||
### Workspace check (`checkWorkspace(x, y)`)
|
||||
|
||||
Four ordered checks:
|
||||
1. Board outer boundary (rectangular)
|
||||
2. Mechanism housing exclusion box (rectangular, centred on origin)
|
||||
3. IK geometric reachability (cosW1/cosW2 range)
|
||||
4. Elbow crossing guard (`armsCrossed`)
|
||||
|
||||
### LIMITS object (all values in mm — **tune to hardware**)
|
||||
|
||||
```javascript
|
||||
const LIMITS = {
|
||||
// Outer board boundary
|
||||
X_MIN: -150, // ← TUNE
|
||||
X_MAX: 150, // ← TUNE
|
||||
Y_MIN: -30, // ← TUNE (numbers start at ~44 mm, signs below 0)
|
||||
Y_MAX: 145, // ← TUNE (highest letter ~128 mm)
|
||||
|
||||
// Centre mechanism housing exclusion box
|
||||
BOX_HALF_W: 30, // ← TUNE: half-width (motors at ±12.9, housing wider)
|
||||
BOX_Y_MIN: -10, // ← TUNE: box bottom
|
||||
BOX_Y_MAX: 40, // ← TUNE: box top
|
||||
|
||||
// Elbow crossing thresholds
|
||||
ELBOW_BOX_X_INNER: 5, // ← TUNE: inner X margin each elbow must stay outside
|
||||
ELBOW_BOX_Y_MAX: 50, // ← TUNE: Y below which elbow crossing is forbidden
|
||||
};
|
||||
```
|
||||
|
||||
### Unit converters
|
||||
|
||||
```javascript
|
||||
IK.stepsToRad(steps) // steps → radians (for FK from jog state)
|
||||
IK.radToSteps(rad) // radians → steps (for IK result → BLE command)
|
||||
IK.stepsToDeg(steps) // steps → degrees (for readout display)
|
||||
IK.radToDeg(rad) // radians → degrees
|
||||
```
|
||||
|
||||
### Lookup table
|
||||
|
||||
`IK.LOOKUP_TABLE` — 36 character positions (A–Z, 0–9, +, -, *, ,) in mm.
|
||||
`IK.lookup('T')` → `{ x: 1.3, y: 97.8 }`.
|
||||
|
||||
---
|
||||
|
||||
## 7. Stepper Test Section (`web/js/sections/stepper-test.js`)
|
||||
|
||||
### State
|
||||
|
||||
```javascript
|
||||
let steps1 = 0; // accumulated step count from home for motor 1
|
||||
let steps2 = 0; // accumulated step count from home for motor 2
|
||||
```
|
||||
|
||||
Step counts are the authoritative position. All FK calls convert steps → radians first.
|
||||
|
||||
### SVG viewport
|
||||
|
||||
```javascript
|
||||
const SV = {
|
||||
W: 480, H: 380,
|
||||
SCALE: 1.35, // px/mm
|
||||
OX: 240, // SVG x of world origin
|
||||
OY: 295, // SVG y of world origin (base line, motors here)
|
||||
wx(worldX) { return this.OX + worldX * this.SCALE; },
|
||||
wy(worldY) { return this.OY - worldY * this.SCALE; }, // Y inverted
|
||||
svgToWorldX(sx) { return (sx - this.OX) / this.SCALE; },
|
||||
svgToWorldY(sy) { return -(sy - this.OY) / this.SCALE; },
|
||||
};
|
||||
```
|
||||
|
||||
### Motor marker positions in SVG (match FK convention)
|
||||
|
||||
```javascript
|
||||
// M1 at +d2 (right side), M2 at -d2 (left side)
|
||||
motor1sx = SV.wx(+IK.ARM.d2);
|
||||
motor2sx = SV.wx(-IK.ARM.d2);
|
||||
```
|
||||
|
||||
### IK click-to-move flow
|
||||
|
||||
1. SVG click → `SV.svgToWorldX/Y(event coords)` → world `(x, y)`
|
||||
2. `IK.checkWorkspace(x, y)` — reject with log message if invalid
|
||||
3. `IK.solve(x, y)` → `{ theta1, theta2 }`
|
||||
4. `IK.radToSteps(theta1/2)` → new step counts
|
||||
5. Compute deltas: `delta1 = newSteps1 - steps1`
|
||||
6. Send BLE: `S1+<delta1>`, `S2+<delta2>`
|
||||
7. Update `steps1/2`, re-render arm
|
||||
|
||||
### Ghost arm (hover preview)
|
||||
|
||||
On `mousemove` over SVG: calls `checkWorkspace` → if valid, calls `showGhost(theta1, theta2)`
|
||||
which renders a translucent preview arm. If invalid, hides ghost.
|
||||
|
||||
### Jog controls
|
||||
|
||||
Manual per-motor step controls. Delta applied directly to `steps1/2`, then
|
||||
`renderArmFromSteps()` redraws. BLE command sent as `S<motor><±delta>`.
|
||||
|
||||
---
|
||||
|
||||
## 8. Development Workflow
|
||||
|
||||
### Running locally
|
||||
|
||||
```bash
|
||||
# From project root:
|
||||
python -m http.server 8080
|
||||
# Then open http://localhost:8080 in Chrome/Edge
|
||||
```
|
||||
|
||||
> Web Bluetooth requires Chrome or Edge. It does NOT work in Firefox.
|
||||
> It works over `localhost` without HTTPS.
|
||||
|
||||
### PlatformIO build
|
||||
|
||||
```bash
|
||||
pio run # compile
|
||||
pio run --target upload # flash
|
||||
pio device monitor # serial monitor (115200 baud)
|
||||
```
|
||||
|
||||
### Cache-buster reminder
|
||||
|
||||
If you make JS changes and the browser still shows old behaviour, hard-refresh
|
||||
(`Ctrl+Shift+R`) or open DevTools → Application → Clear Storage.
|
||||
The `?v=Date.now()` on imports handles this automatically on page load.
|
||||
|
||||
---
|
||||
|
||||
## 9. What Is Done
|
||||
|
||||
- [x] SPA shell (`index.html`) with collapsible sidebar, hamburger button, CSS dark theme
|
||||
- [x] Hash-based router (`router.js`) with mount/unmount lifecycle
|
||||
- [x] BLE singleton (`ble.js`) — connect, write-without-response, NOTIFY subscription
|
||||
- [x] Kinematics module (`kinematics.js`) — IK, FK, workspace check, lookup table
|
||||
- [x] Stepper Test section (`stepper-test.js`):
|
||||
- SVG SCARA arm visualiser with correct motor convention
|
||||
- Click-to-move via IK with workspace validation
|
||||
- Ghost arm hover preview
|
||||
- Per-motor jog controls (±10, ±50, ±100, ±500 steps)
|
||||
- Motor zero (HOME) buttons
|
||||
- Current position readout panel (X/Y, θ1/θ2, steps)
|
||||
- Move-to-XY numeric input with Go button
|
||||
- Board boundary + mechanism exclusion zone visualisation
|
||||
- Simulation badge (yellow) when BLE not connected
|
||||
- [x] Home section (`home.js`) — placeholder landing page, same visual style
|
||||
- [x] Firmware (`src/main.cpp`) — AccelStepper + BLE command parser + position NOTIFY
|
||||
|
||||
---
|
||||
|
||||
## 10. What Remains (Planned Sections)
|
||||
|
||||
The router currently only has two routes. These sections need to be created:
|
||||
|
||||
### `board-control` section (not started)
|
||||
- Full keyboard layout overlaid on a top-view board image
|
||||
- Click a letter/number → IK move to `LOOKUP_TABLE[char]`
|
||||
- Perhaps a "type a word" sequential move feature
|
||||
|
||||
### `sequence-editor` section (not started)
|
||||
- Record and replay a sequence of moves
|
||||
- Save/load sequences to localStorage
|
||||
- Simple list UI: add step, delete step, reorder, run
|
||||
|
||||
To add a new section:
|
||||
1. Create `web/js/sections/<name>.js` exporting `{ mount(el), unmount() }`
|
||||
2. In `index.html` add the import: `const { default: XSection } = await import('./web/js/sections/<name>.js?v=${V}');`
|
||||
3. Register: `Router.register('<name>', XSection);`
|
||||
4. Add a nav button in the sidebar HTML inside `index.html`
|
||||
|
||||
---
|
||||
|
||||
## 11. Key Design Decisions (Why Things Are The Way They Are)
|
||||
|
||||
| Decision | Reason |
|
||||
|---|---|
|
||||
| **No WiFi / no server** | User requirement — BLE only, no hosting needed |
|
||||
| **`?v=Date.now()` cache-buster** | ES module imports are cached by the browser; without this, stale code runs silently after edits |
|
||||
| **Steps as primary state** | The firmware tracks absolute step counts. Angles are derived by `stepsToRad()`. This keeps web ↔ firmware in sync. |
|
||||
| **M1 at +d2, M2 at -d2** | This is the PositionControl.cpp convention. Reversing it causes visual arm crossing. Verified by tracing `xmd = x - d2` (offset from +d2 pivot) and `xpd = x + d2` (offset from -d2 pivot). |
|
||||
| **`forward()` picks upward EE** | Two circle-circle intersections exist; the mechanism always operates in the "above" configuration. The lower solution is physically blocked by the board. |
|
||||
| **`armsCrossed()` guard** | Prevents IK moves that would drive an elbow into the mechanism housing from the wrong side. Tunable via `LIMITS.ELBOW_BOX_X_INNER` and `ELBOW_BOX_Y_MAX`. |
|
||||
| **`fivebarIKGame.js` is reference only** | The original site's game uses a different angle convention (absolute degrees, different home, with `ikElbowSigns` tracking). We use the C++ IK formula instead because it directly produces step deltas from home. `fivebarIKGame.js` is kept in the repo for reference and understanding, not imported. |
|
||||
| **Hash-based routing** | Keeps the SPA working from `file://` and simple static servers without needing a history API setup |
|
||||
|
||||
---
|
||||
|
||||
## 12. Known Issues / Things to Verify on Hardware
|
||||
|
||||
- **Motor direction / sign**: The signs of the IK formulas (`theta1 = q - w1`, `theta2 = r + w2`)
|
||||
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.
|
||||
|
||||
- **LIMITS tuning**: All values in `LIMITS` in `kinematics.js` are estimates based on the
|
||||
original project's data. They need physical verification:
|
||||
- `BOX_HALF_W`, `BOX_Y_MAX`: size of the actual motor housing
|
||||
- `ELBOW_BOX_X_INNER`, `ELBOW_BOX_Y_MAX`: elbow clearance above the housing
|
||||
|
||||
- **Step speed / acceleration**: `DEFAULT_SPEED = 600`, `DEFAULT_ACCEL = 100` in firmware.
|
||||
28BYJ-48 motors are slow; these may need adjustment. Adjustable at runtime via `SPD:<n>`
|
||||
and `ACC:<n>` BLE commands.
|
||||
|
||||
---
|
||||
|
||||
## 13. Reference: `fivebarIKGame.js`
|
||||
|
||||
This file (in the project root) is the original website's game kinematics. **Do not import it.**
|
||||
Use it as a reference for understanding the mechanism, especially:
|
||||
|
||||
- `S.solveFK(a1_deg, a2_deg)` — forward kinematics in absolute degrees, with stateful branch
|
||||
tracking (`fkBranchIndex`, `eeHistory`) and `isValidElbow` cross-product check.
|
||||
- `S.solveIK(target, currentAngles)` — IK with `ikElbowSigns` continuity tracking and motor
|
||||
angle limits: M1 = [-330°, -90°], M2 = [-90°, +150°] (in absolute angle convention).
|
||||
- `intersectCircles(p1, p2, r)` — correct circle-circle intersection.
|
||||
- `noGoRadius: 50` — the game's centre exclusion radius in mm.
|
||||
- Home state in game: `a1 = -180°`, `a2 = 0°` → EE at approximately (0, 50.3 mm).
|
||||
|
||||
The motor angle limits and no-go radius from the game are good starting points for tuning
|
||||
the LIMITS object in `kinematics.js`.
|
||||
+96
-1
@@ -50,6 +50,20 @@
|
||||
#define DEFAULT_SPEED 600.0f // steps/sec
|
||||
#define DEFAULT_ACCEL 100.0f // steps/sec²
|
||||
|
||||
// ─── Homing configuration ─────────────────────────────────────────
|
||||
// TUNE these after first hardware test.
|
||||
// SEEK_DIR: +1 = CW (positive steps), -1 = CCW toward stop
|
||||
// SEEK_STEPS: must exceed maximum arm travel (~240° = 1365 steps)
|
||||
// OP_OFFSET: steps from the mechanical stop to the operating home
|
||||
// (derived from original PositionControl.cpp homing offsets)
|
||||
#define M1_HOME_DIR (-1) // ← TUNE: flip to +1 if M1 moves wrong way
|
||||
#define M2_HOME_DIR (+1) // ← TUNE: flip to -1 if M2 moves wrong way
|
||||
#define HOME_SEEK_STEPS 1600 // ← TUNE: > max travel; 1600 ≈ 281° of a rev
|
||||
#define HOME_SPEED 180.0f // ← TUNE: slow seek to avoid missing steps
|
||||
#define HOME_ACCEL 50.0f // ← TUNE: gentle accel during seek
|
||||
#define M1_OP_OFFSET 550 // ← TUNE: steps from M1 stop → operating home
|
||||
#define M2_OP_OFFSET -530 // ← TUNE: steps from M2 stop → operating home
|
||||
|
||||
// ─── BLE UUIDs ────────────────────────────────────────────────────
|
||||
#define SERVICE_UUID "a0b1c2d3-e4f5-6789-abcd-ef0123456700"
|
||||
#define CMD_UUID "a0b1c2d3-e4f5-6789-abcd-ef0123456701"
|
||||
@@ -73,6 +87,7 @@ const unsigned long NOTIFY_INTERVAL_MS = 200;
|
||||
// ─── Forward declarations ─────────────────────────────────────────
|
||||
void parseCommand(const String& cmd);
|
||||
void sendPosition();
|
||||
void runHoming();
|
||||
|
||||
// ─── BLE Server Callbacks ─────────────────────────────────────────
|
||||
class ServerCallbacks : public BLEServerCallbacks {
|
||||
@@ -130,7 +145,13 @@ void parseCommand(const String& cmd) {
|
||||
Serial.printf("[CFG] Acceleration → %.0f steps/sec²\n", acc);
|
||||
return;
|
||||
}
|
||||
// HOME1
|
||||
// HOME – full stall-seek homing sequence
|
||||
if (cmd == "HOME") {
|
||||
Serial.println("[HOME] Received HOME command");
|
||||
runHoming();
|
||||
return;
|
||||
}
|
||||
// HOME1 / HOME2 – manual zero (soft zero, no motion)
|
||||
if (cmd == "HOME1") {
|
||||
stepper1.setCurrentPosition(0);
|
||||
Serial.println("[S1] Zeroed");
|
||||
@@ -152,6 +173,80 @@ void parseCommand(const String& cmd) {
|
||||
Serial.printf("[CMD] Unknown command: '%s'\n", cmd.c_str());
|
||||
}
|
||||
|
||||
// ─── Stall-based homing sequence ─────────────────────────────────
|
||||
/**
|
||||
* Drives each motor slowly into its mechanical hard stop (the arm
|
||||
* folds against the motor housing). The 28BYJ-48 stalls harmlessly
|
||||
* for a fraction of a second at the stop. After both stops are
|
||||
* found, the arm backs off to the operating home position and both
|
||||
* step counters are zeroed.
|
||||
*
|
||||
* This is a BLOCKING call — BLE is not serviced during homing
|
||||
* (~15-20 s total). The browser shows "Homing…" until the
|
||||
* HOMED notify arrives.
|
||||
*
|
||||
* Tune M1_HOME_DIR, M2_HOME_DIR, HOME_SEEK_STEPS, and OP_OFFSETs
|
||||
* after the first hardware test.
|
||||
*/
|
||||
void runHoming() {
|
||||
Serial.println("[HOME] Starting homing sequence…");
|
||||
|
||||
// Reduce speed for the seek phase
|
||||
stepper1.setMaxSpeed(HOME_SPEED);
|
||||
stepper1.setAcceleration(HOME_ACCEL);
|
||||
stepper2.setMaxSpeed(HOME_SPEED);
|
||||
stepper2.setAcceleration(HOME_ACCEL);
|
||||
|
||||
// ── Seek M1 to its mechanical stop ───────────────────────────
|
||||
Serial.println("[HOME] Seeking M1 stop…");
|
||||
stepper1.move(M1_HOME_DIR * HOME_SEEK_STEPS);
|
||||
while (stepper1.isRunning()) {
|
||||
stepper1.run();
|
||||
stepper2.run(); // keep M2 coils energised during M1 seek
|
||||
}
|
||||
stepper1.setCurrentPosition(0);
|
||||
Serial.println("[HOME] M1 stop found → zeroed");
|
||||
delay(200); // brief pause before M2 seek
|
||||
|
||||
// ── Seek M2 to its mechanical stop ───────────────────────────
|
||||
Serial.println("[HOME] Seeking M2 stop…");
|
||||
stepper2.move(M2_HOME_DIR * HOME_SEEK_STEPS);
|
||||
while (stepper2.isRunning()) {
|
||||
stepper1.run();
|
||||
stepper2.run();
|
||||
}
|
||||
stepper2.setCurrentPosition(0);
|
||||
Serial.println("[HOME] M2 stop found → zeroed");
|
||||
delay(200);
|
||||
|
||||
// ── Move to operating home (restore normal speed) ─────────────
|
||||
stepper1.setMaxSpeed(DEFAULT_SPEED);
|
||||
stepper1.setAcceleration(DEFAULT_ACCEL);
|
||||
stepper2.setMaxSpeed(DEFAULT_SPEED);
|
||||
stepper2.setAcceleration(DEFAULT_ACCEL);
|
||||
|
||||
Serial.println("[HOME] Moving to operating home…");
|
||||
stepper1.moveTo(M1_OP_OFFSET);
|
||||
stepper2.moveTo(M2_OP_OFFSET);
|
||||
while (stepper1.isRunning() || stepper2.isRunning()) {
|
||||
stepper1.run();
|
||||
stepper2.run();
|
||||
}
|
||||
|
||||
// Declare operating home as the new step zero
|
||||
stepper1.setCurrentPosition(0);
|
||||
stepper2.setCurrentPosition(0);
|
||||
Serial.println("[HOME] Homing complete — at operating home (0, 0)");
|
||||
|
||||
// Notify the browser so it can sync its step counters
|
||||
if (bleConnected && pStatusChar) {
|
||||
pStatusChar->setValue("HOMED");
|
||||
pStatusChar->notify();
|
||||
}
|
||||
delay(50);
|
||||
sendPosition(); // immediately follow with P:0,0
|
||||
}
|
||||
|
||||
// ─── Send current positions via BLE NOTIFY ────────────────────────
|
||||
void sendPosition() {
|
||||
if (!bleConnected || !pStatusChar) return;
|
||||
|
||||
+145
-80
@@ -8,52 +8,65 @@
|
||||
* / \
|
||||
* L2 (110) L2 (110)
|
||||
* / \
|
||||
* ELBOW1 ELBOW2
|
||||
* ELBOW1 ELBOW2
|
||||
* \ /
|
||||
* L1 (85) L1 (85)
|
||||
* \ /
|
||||
* MOTOR1 (-d2,0) MOTOR2 (+d2,0)
|
||||
* MOTOR1 (+d2,0) MOTOR2 (-d2,0)
|
||||
* | |
|
||||
* [===BASE===] (d=25.8 mm wide)
|
||||
* d2=12.9 mm
|
||||
*
|
||||
* Both motors are mounted in the centre mechanism box.
|
||||
* Motor 1 is at (-d2, 0), Motor 2 is at (+d2, 0).
|
||||
* Each motor drives a proximal arm (l1). The distal arms (l2)
|
||||
* connect the elbows to the shared end-effector.
|
||||
* ── IMPORTANT: PositionControl.cpp motor convention ─────────────
|
||||
* Motor 1 pivot is at (+d2, 0) = (+12.9, 0) [right side!]
|
||||
* Motor 2 pivot is at (-d2, 0) = (-12.9, 0) [left side!]
|
||||
*
|
||||
* IK: given target (x, y), solve θ1 and θ2 independently:
|
||||
* Motor 1 sees the target at (x - d2, y) from its pivot.
|
||||
* Motor 2 sees the target at (x + d2, y) from its pivot.
|
||||
* Each uses the standard 2-link IK (law of cosines).
|
||||
* This is determined by how the C++ IK formulas use the offsets:
|
||||
* Motor 1: xmd = x - d2 → target is measured from x = +d2
|
||||
* Motor 2: xpd = x + d2 → target is measured from x = -d2
|
||||
*
|
||||
* All FK / visualisation code MUST use this same convention or
|
||||
* the arms will appear visually crossed even for valid positions.
|
||||
*
|
||||
* Source: lib/Position/PositionControl.cpp (nerd-sniped/WijiBoard)
|
||||
*/
|
||||
|
||||
// ── Exact constants from PositionControl.cpp ─────────────────────
|
||||
const ARM = {
|
||||
d: 25.8, // full motor separation (mm)
|
||||
d2: 12.9, // half motor separation — motor 1 at (-d2,0), motor 2 at (+d2,0)
|
||||
l1: 85.0, // proximal link length (mm)
|
||||
l2: 110.0, // distal link length (mm)
|
||||
STEPS_PER_REV: 2048,
|
||||
STEP_ANGLE_DEG: 360 / 2048, // ≈ 0.17578125°
|
||||
d: 25.8, // full motor separation (mm)
|
||||
d2: 12.9, // half separation; M1 at (+d2, 0), M2 at (-d2, 0)
|
||||
l1: 85.0, // proximal link length (mm)
|
||||
l2: 110.0, // distal link length (mm)
|
||||
STEPS_PER_REV: 2048,
|
||||
STEP_ANGLE_DEG: 360 / 2048, // ≈ 0.17578125 °/step
|
||||
};
|
||||
|
||||
// ── Workspace limits ──────────────────────────────────────────────
|
||||
// ── WORKSPACE CONSTRAINTS ─────────────────────────────────────────
|
||||
// All values are in mm. Tune these to match real hardware.
|
||||
// They are exported so the visualiser can draw the zones.
|
||||
const LIMITS = {
|
||||
// Outer bounding box (the board surface)
|
||||
X_MIN: -150,
|
||||
X_MAX: 150,
|
||||
Y_MIN: -30, // numbers sit slightly below Y=0
|
||||
Y_MAX: 145,
|
||||
// ── Outer board boundary ───────────────────────────────────────
|
||||
// The EE cannot be requested outside this rectangle.
|
||||
X_MIN: -150, // ← TUNE: left edge of board
|
||||
X_MAX: 150, // ← TUNE: right edge of board
|
||||
Y_MIN: -30, // ← TUNE: bottom (numbers reach ~44 mm; sign coords go lower)
|
||||
Y_MAX: 145, // ← TUNE: top (highest letter is ~128 mm)
|
||||
|
||||
// Centre exclusion zone (the motor/mechanism box)
|
||||
// Motors are at ±12.9 mm; box is a bit larger to account for the housing
|
||||
BOX_HALF_W: 22, // ±22 mm in X (tune to real hardware)
|
||||
BOX_HALF_H: 22, // 0..22 mm in Y (box sits above the base line)
|
||||
BOX_Y_MIN: -5,
|
||||
BOX_Y_MAX: 22,
|
||||
// ── Centre mechanism exclusion box ────────────────────────────
|
||||
// Rectangular zone centred on (0, 0) where the motor housing sits.
|
||||
// The EE and both elbows must stay outside this area.
|
||||
BOX_HALF_W: 30, // ← TUNE: half-width in X (motors at ±12.9, housing wider)
|
||||
BOX_Y_MIN: -10, // ← TUNE: bottom of housing
|
||||
BOX_Y_MAX: 40, // ← TUNE: top of housing
|
||||
|
||||
// ── Elbow exclusion zone (prevents arm crossing near the box) ─
|
||||
// Each elbow has a separate rectangular exclusion box.
|
||||
// Left-side elbow (from M1 at +d2): must NOT enter this region.
|
||||
// Right-side elbow (from M2 at -d2): uses mirrored X limits.
|
||||
// If ELBOW_BOX_X_INNER is 5, the left elbow's X must be > +5 mm
|
||||
// (can never cross to the other side of the box mid-point).
|
||||
ELBOW_BOX_X_INNER: 5, // ← TUNE: inner X margin from centre for each elbow
|
||||
ELBOW_BOX_Y_MAX: 50, // ← TUNE: Y below which elbow crossing is forbidden
|
||||
};
|
||||
|
||||
// ── Letter / number position lookup table ────────────────────────
|
||||
@@ -116,22 +129,26 @@ const LOOKUP_TABLE = {
|
||||
function solve(x, y) {
|
||||
const { d2, l1, l2 } = ARM;
|
||||
|
||||
// ── Motor 1 (left pivot at -d2, 0) ───────────────────────────
|
||||
const xmd = x - d2; // target X relative to left motor
|
||||
const s = Math.sqrt(xmd * xmd + y * y); // distance: left motor → target
|
||||
// ── Motor 1 (pivot at +d2, 0 = +12.9 mm) ─────────────────────
|
||||
// xmd = x - d2 is the X component of (target – M1_pivot).
|
||||
const xmd = x - d2;
|
||||
const s = Math.sqrt(xmd * xmd + y * y);
|
||||
if (s < 1e-6) return { theta1: 0, theta2: 0, reachable: false };
|
||||
const cosW1 = (l2 * l2 - s * s - l1 * l1) / (-2 * l1 * s);
|
||||
if (cosW1 < -1 || cosW1 > 1) return { theta1: 0, theta2: 0, reachable: false };
|
||||
const q = Math.atan2(y, xmd);
|
||||
const w1 = Math.acos(cosW1);
|
||||
const q = Math.atan2(y, xmd);
|
||||
const w1 = Math.acos(cosW1);
|
||||
const theta1 = q - w1;
|
||||
|
||||
// ── Motor 2 (right pivot at +d2, 0) ──────────────────────────
|
||||
const xpd = x + d2; // target X relative to right motor
|
||||
const t = Math.sqrt(xpd * xpd + y * y); // distance: right motor → target
|
||||
// ── Motor 2 (pivot at -d2, 0 = -12.9 mm) ─────────────────────
|
||||
// xpd = x + d2 is the X component of (target – M2_pivot).
|
||||
const xpd = x + d2;
|
||||
const t = Math.sqrt(xpd * xpd + y * y);
|
||||
if (t < 1e-6) return { theta1: 0, theta2: 0, reachable: false };
|
||||
const cosW2 = (l2 * l2 - t * t - l1 * l1) / (-2 * l1 * t);
|
||||
if (cosW2 < -1 || cosW2 > 1) return { theta1: 0, theta2: 0, reachable: false };
|
||||
const r = Math.atan2(y, xpd);
|
||||
const w2 = Math.acos(cosW2);
|
||||
const r = Math.atan2(y, xpd);
|
||||
const w2 = Math.acos(cosW2);
|
||||
const theta2 = r + w2;
|
||||
|
||||
return { theta1, theta2, reachable: true };
|
||||
@@ -156,81 +173,129 @@ function solve(x, y) {
|
||||
function forward(theta1, theta2) {
|
||||
const { d2, l1, l2 } = ARM;
|
||||
|
||||
// Elbow 1 (tip of motor 1's proximal link)
|
||||
const e1x = -d2 + l1 * Math.cos(theta1);
|
||||
const e1y = l1 * Math.sin(theta1);
|
||||
// ── IMPORTANT: match PositionControl.cpp motor convention ──────
|
||||
// Motor 1 pivot at (+d2, 0), Motor 2 pivot at (-d2, 0).
|
||||
// Using the opposite sign here is the single most common source
|
||||
// of visually-crossed arms in the SVG visualiser.
|
||||
|
||||
// Elbow 2 (tip of motor 2's proximal link)
|
||||
const e2x = d2 + l1 * Math.cos(theta2);
|
||||
const e2y = l1 * Math.sin(theta2);
|
||||
// Elbow 1 — tip of Motor 1 proximal link (motor at +d2)
|
||||
const e1x = +d2 + l1 * Math.cos(theta1);
|
||||
const e1y = l1 * Math.sin(theta1);
|
||||
|
||||
// End-effector: intersection of circle(elbow1, l2) and circle(elbow2, l2)
|
||||
// Use the same approach as the IK: each distal link points from its elbow to the EE.
|
||||
// For visualisation accuracy, reconstruct EE by reversing the IK:
|
||||
// From IK: theta1 = q - w1 → q = atan2(y, x - d2)
|
||||
// We know theta1 and the elbow position, so EE is at l2 along some direction.
|
||||
// Simplest: use circle-circle intersection of the two elbow-radius-l2 circles.
|
||||
const dx = e2x - e1x;
|
||||
const dy = e2y - e1y;
|
||||
// Elbow 2 — tip of Motor 2 proximal link (motor at -d2)
|
||||
const e2x = -d2 + l1 * Math.cos(theta2);
|
||||
const e2y = l1 * Math.sin(theta2);
|
||||
|
||||
// End-effector: intersection of the two distal-link circles
|
||||
// (radius l2, centred on each elbow). Pick the "upward" solution.
|
||||
const dx = e2x - e1x;
|
||||
const dy = e2y - e1y;
|
||||
const dist = Math.sqrt(dx * dx + dy * dy);
|
||||
|
||||
if (dist < 1e-6 || dist > 2 * l2) {
|
||||
// Degenerate / unreachable — just average the two elbows
|
||||
// Degenerate / unreachable — fall back to midpoint
|
||||
return {
|
||||
elbow1: { x: e1x, y: e1y },
|
||||
elbow2: { x: e2x, y: e2y },
|
||||
endX: (e1x + e2x) / 2,
|
||||
endY: (e1y + e2y) / 2,
|
||||
valid: false,
|
||||
};
|
||||
}
|
||||
|
||||
const a = dist / 2;
|
||||
const h = Math.sqrt(l2 * l2 - a * a);
|
||||
const a = dist / 2;
|
||||
const h = Math.sqrt(Math.max(0, l2 * l2 - a * a));
|
||||
const mx = (e1x + e2x) / 2;
|
||||
const my = (e1y + e2y) / 2;
|
||||
|
||||
// Two intersection candidates — pick the one with higher Y (the "up" configuration)
|
||||
const px1 = mx + h * (dy / dist);
|
||||
const py1 = my - h * (dx / dist);
|
||||
const px2 = mx - h * (dy / dist);
|
||||
const py2 = my + h * (dx / dist);
|
||||
// Two intersection candidates
|
||||
const px1 = mx + h * ( dy / dist);
|
||||
const py1 = my + h * (-dx / dist);
|
||||
const px2 = mx - h * ( dy / dist);
|
||||
const py2 = my - h * (-dx / dist);
|
||||
|
||||
const { endX, endY } = py1 > py2
|
||||
? { endX: px1, endY: py1 }
|
||||
: { endX: px2, endY: py2 };
|
||||
// Always pick the candidate with higher Y (EE above the elbow line)
|
||||
const useFirst = py1 >= py2;
|
||||
const endX = useFirst ? px1 : px2;
|
||||
const endY = useFirst ? py1 : py2;
|
||||
|
||||
return {
|
||||
elbow1: { x: e1x, y: e1y },
|
||||
elbow2: { x: e2x, y: e2y },
|
||||
endX, endY,
|
||||
valid: true,
|
||||
};
|
||||
}
|
||||
|
||||
// ── Arm-crossing check ────────────────────────────────────────────
|
||||
/**
|
||||
* Returns true if the two proximal arm segments geometrically
|
||||
* cross each other near the centre mechanism box.
|
||||
*
|
||||
* Physical rule: each elbow must stay on the OUTER side of the
|
||||
* mechanism housing. If elbow1 (from M1 at +d2) has a small
|
||||
* positive X at low Y, or elbow2 (from M2 at -d2) has a small
|
||||
* negative X at low Y, the arm would collide with the housing.
|
||||
*
|
||||
* @param {number} theta1 Motor 1 angle (rad)
|
||||
* @param {number} theta2 Motor 2 angle (rad)
|
||||
* @returns {boolean} true = arms will cross / collide
|
||||
*/
|
||||
function armsCrossed(theta1, theta2) {
|
||||
const { d2, l1 } = ARM;
|
||||
const { ELBOW_BOX_X_INNER: XI, ELBOW_BOX_Y_MAX: YM } = LIMITS;
|
||||
|
||||
// Elbow positions (same formula as forward())
|
||||
const e1x = +d2 + l1 * Math.cos(theta1);
|
||||
const e1y = l1 * Math.sin(theta1);
|
||||
const e2x = -d2 + l1 * Math.cos(theta2);
|
||||
const e2y = l1 * Math.sin(theta2);
|
||||
|
||||
// Elbow1 (from M1 on the RIGHT) must not appear far to the LEFT at low height
|
||||
// Elbow2 (from M2 on the LEFT) must not appear far to the RIGHT at low height
|
||||
// Both conditions together catch the "arms have swapped sides" scenario.
|
||||
const e1_crossed = e1x < -XI && e1y < YM; // M1's elbow went too far left
|
||||
const e2_crossed = e2x > XI && e2y < YM; // M2's elbow went too far right
|
||||
|
||||
return e1_crossed || e2_crossed;
|
||||
}
|
||||
|
||||
// ── Workspace check ───────────────────────────────────────────────
|
||||
/**
|
||||
* Check whether a point is inside the valid workspace.
|
||||
* Check whether a target point is safe to move to.
|
||||
*
|
||||
* Order of checks (fail-fast):
|
||||
* 1. Board outer boundary
|
||||
* 2. Mechanism housing exclusion box
|
||||
* 3. IK geometric reachability
|
||||
* 4. Elbow-crossing guard (prevents physically impossible arm configs)
|
||||
*
|
||||
* @param {number} x
|
||||
* @param {number} y
|
||||
* @returns {{ ok: boolean, reason: string }}
|
||||
*/
|
||||
function checkWorkspace(x, y) {
|
||||
// Outer bounding box
|
||||
if (x < LIMITS.X_MIN || x > LIMITS.X_MAX)
|
||||
return { ok: false, reason: `X=${x.toFixed(1)} outside board limits [${LIMITS.X_MIN}, ${LIMITS.X_MAX}]` };
|
||||
if (y < LIMITS.Y_MIN || y > LIMITS.Y_MAX)
|
||||
return { ok: false, reason: `Y=${y.toFixed(1)} outside board limits [${LIMITS.Y_MIN}, ${LIMITS.Y_MAX}]` };
|
||||
const L = LIMITS;
|
||||
|
||||
// Centre exclusion zone (mechanism box)
|
||||
if (
|
||||
x > -LIMITS.BOX_HALF_W && x < LIMITS.BOX_HALF_W &&
|
||||
y > LIMITS.BOX_Y_MIN && y < LIMITS.BOX_Y_MAX
|
||||
) {
|
||||
return { ok: false, reason: `(${x.toFixed(1)}, ${y.toFixed(1)}) is inside the mechanism exclusion zone` };
|
||||
}
|
||||
// 1. Board boundary
|
||||
if (x < L.X_MIN || x > L.X_MAX)
|
||||
return { ok: false, reason: `X=${x.toFixed(1)} mm outside board (${L.X_MIN}…${L.X_MAX})` };
|
||||
if (y < L.Y_MIN || y > L.Y_MAX)
|
||||
return { ok: false, reason: `Y=${y.toFixed(1)} mm outside board (${L.Y_MIN}…${L.Y_MAX})` };
|
||||
|
||||
// IK reachability
|
||||
const { reachable } = solve(x, y);
|
||||
if (!reachable) return { ok: false, reason: `(${x.toFixed(1)}, ${y.toFixed(1)}) is outside arm reach` };
|
||||
// 2. Centre mechanism exclusion box
|
||||
if (x > -L.BOX_HALF_W && x < L.BOX_HALF_W &&
|
||||
y > L.BOX_Y_MIN && y < L.BOX_Y_MAX)
|
||||
return { ok: false, reason: `(${x.toFixed(1)}, ${y.toFixed(1)}) is inside the mechanism housing` };
|
||||
|
||||
// 3. IK geometric reachability
|
||||
const { theta1, theta2, reachable } = solve(x, y);
|
||||
if (!reachable)
|
||||
return { ok: false, reason: `(${x.toFixed(1)}, ${y.toFixed(1)}) is geometrically unreachable` };
|
||||
|
||||
// 4. Elbow-crossing guard
|
||||
if (armsCrossed(theta1, theta2))
|
||||
return { ok: false, reason: `(${x.toFixed(1)}, ${y.toFixed(1)}) would cross elbows near the mechanism box` };
|
||||
|
||||
return { ok: true, reason: '' };
|
||||
}
|
||||
@@ -256,6 +321,6 @@ function radToDeg(rad) {
|
||||
|
||||
export default {
|
||||
ARM, LIMITS, LOOKUP_TABLE,
|
||||
solve, forward, checkWorkspace, lookup,
|
||||
solve, forward, armsCrossed, checkWorkspace, lookup,
|
||||
stepsToRad, radToSteps, stepsToDeg, radToDeg,
|
||||
};
|
||||
|
||||
+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