fix kinematics - maybe?
This commit is contained in:
PROFERIS - Mi³osz Stocki
@@ -41,21 +41,19 @@ only BLE between the ESP32 firmware and a local web SPA.
|
||||
ELBOW1 ELBOW2
|
||||
\ /
|
||||
l1 (85 mm) l1 (85 mm)
|
||||
\ /
|
||||
MOTOR1 (+12.9,0) MOTOR2 (-12.9,0)
|
||||
\ /
|
||||
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
|
||||
The original C++ firmware has a variable swap bug where `angle2` is passed to `stepper1` and `angle1` to `stepper2`. Thus, the physical mapping is:
|
||||
- **Motor 1** is the **LEFT** motor, pivoting at **(-d2, 0) = (-12.9, 0)**. It uses `xpd = x + d2`.
|
||||
- **Motor 2** is the **RIGHT** motor, pivoting at **(+d2, 0) = (+12.9, 0)**. It uses `xmd = x - d2`.
|
||||
|
||||
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.**
|
||||
**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 and physically collide.**
|
||||
|
||||
### Arm constants (from `PositionControl.cpp`)
|
||||
|
||||
@@ -184,20 +182,20 @@ hamburger (`☰`) button. It contains `<button data-route="...">` nav items that
|
||||
|
||||
### IK (`solve(x, y)`)
|
||||
|
||||
Exact port of `calculateInverseKinematics()` from `PositionControl.cpp`:
|
||||
Exact port of `calculateInverseKinematics()` from `PositionControl.cpp`, with motor swap applied:
|
||||
|
||||
```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
|
||||
// Motor 1 (Left, pivot at -d2 = -12.9 mm)
|
||||
const xpd = x + d2; // Δx from M1 pivot
|
||||
const t = hypot(xpd, y);
|
||||
const cosW2 = (l2² - t² - l1²) / (-2·l1·t);
|
||||
const theta2 = atan2(y, xpd) + acos(cosW2);
|
||||
const theta1 = atan2(y, xpd) + acos(cosW2);
|
||||
|
||||
// Motor 2 (Right, pivot at +d2 = +12.9 mm)
|
||||
const xmd = x - d2; // Δx from M2 pivot
|
||||
const s = hypot(xmd, y);
|
||||
const cosW1 = (l2² - s² - l1²) / (-2·l1·s);
|
||||
const theta2 = atan2(y, xmd) - acos(cosW1);
|
||||
```
|
||||
|
||||
Returns `{ theta1, theta2, reachable }` — angles in **radians**.
|
||||
@@ -207,9 +205,9 @@ Returns `{ theta1, theta2, reachable }` — angles in **radians**.
|
||||
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) }
|
||||
// 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)
|
||||
```
|
||||
|
||||
@@ -294,9 +292,9 @@ const SV = {
|
||||
### 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);
|
||||
// M1 at -d2 (left side), M2 at +d2 (right side)
|
||||
motor1sx = SV.wx(-IK.ARM.d2);
|
||||
motor2sx = SV.wx(+IK.ARM.d2);
|
||||
```
|
||||
|
||||
### IK click-to-move flow
|
||||
@@ -419,7 +417,7 @@ To add a new section:
|
||||
| **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). |
|
||||
| **M1 at -d2, M2 at +d2** | Corrects a hardware variable swap in C++ firmware where angle2 goes to stepper1. Verified by tracing `xmd = x - d2` to the right motor and `xpd = x + d2` to the left motor. |
|
||||
| **`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`. |
|
||||
| **Linux/BlueZ stability** | `pAdv->setMaxPreferred(0x0C)` is mandatory in firmware. Without it, Linux/ChromeOS will drop the connection immediately after the handshake. |
|
||||
@@ -437,7 +435,7 @@ 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**: 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.
|
||||
- **Home position**: Stall homing is fully implemented matching the original C++ routine but adjusted to align with physical motor mappings. Homing pushes the motors against their physical limits and sets `steps1 = -1024` (-180°, points left) and `steps2 = 0` (0°, points right). 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.
|
||||
|
||||
|
||||
Reference in New Issue
Block a user