Compare commits
3 Commits
main
...
sl-mechani
| Author | SHA1 | Date | |
|---|---|---|---|
| 3e4764b3eb | |||
| 165f5c174c | |||
| 4fed80f36d |
228
chassis/rover_BOM.md
Normal file
228
chassis/rover_BOM.md
Normal file
@ -0,0 +1,228 @@
|
||||
# SaltyRover Chassis — BOM + Assembly Notes
|
||||
**Rev A — 2026-03-01 — sl-mechanical**
|
||||
**Issue: #73**
|
||||
|
||||
---
|
||||
|
||||
## Overview
|
||||
|
||||
SaltyRover is the stable 4-wheel variant of SaltyLab.
|
||||
Low CG, wide track — designed not to tip over.
|
||||
Reuses the 25 mm vertical stem, sensor head, RPLIDAR, RealSense,
|
||||
IMX219 camera mounts, and roll cage from SaltyLab without modification.
|
||||
|
||||
```
|
||||
Top view (schematic):
|
||||
|
||||
Forward (+Y)
|
||||
│
|
||||
◉─────────────────────◉ ← front axle (AXLE_BASE/2 = 170 mm forward)
|
||||
│ [Orin NX] │
|
||||
│ [battery] │ ← flat packs below deck (low CG)
|
||||
│ [battery] │
|
||||
│ [FC] │
|
||||
◉─────────────────────◉ ← rear axle
|
||||
│
|
||||
stem (centre)
|
||||
|
||||
Dimensions (approx):
|
||||
Robot width (tyre-to-tyre): ~810 mm (TRACK_W=540 + 2×TIRE_W/2)
|
||||
Robot length (axle-to-axle): ~340 mm (AXLE_BASE)
|
||||
Deck width: 480 mm
|
||||
Deck length: 500 mm
|
||||
Ground clearance: ~50 mm (deck bottom to ground)
|
||||
Overall height (with 550 mm stem + sensor head): ~800 mm
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Height Stack
|
||||
|
||||
```
|
||||
Z from ground:
|
||||
Z = 0 mm — ground
|
||||
Z = 50 mm — chassis bottom (battery tray floor)
|
||||
Z = 109 mm — deck bottom face
|
||||
Z = 117 mm — deck top face (Z=0 in SCAD coords)
|
||||
Z = 127 mm — motor axle CL (+10 mm above deck top)
|
||||
Z = 139 mm — deck stem collar top (117+22)
|
||||
Z = 145 mm — stem adapter flange top (139+6)
|
||||
Z = 173 mm — stem clamp top (145+28)
|
||||
Z = 723 mm — stem top with 550 mm rover stem
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## File Index
|
||||
|
||||
| File | Description | RENDER |
|
||||
|------|-------------|--------|
|
||||
| `saltyrover_chassis.scad` | Main deck plate, stem collar, standoffs | `assembly` / `deck` / `deck_2d` |
|
||||
| `rover_motor_mount.scad` | L-bracket + axle clamp plate, 4× | `assembly` / `bracket` / `clamp_plate` / `bracket_2d` |
|
||||
| `rover_battery_tray.scad` | Slide-out battery tray, 2–4 packs | `assembly` / `tray` / `rail` / `latch` / `tray_2d` |
|
||||
| `rover_stem_adapter.scad` | Flange + split clamp, locks stem to deck | `assembly` / `base_flange` / `clamp_front` / `clamp_rear` |
|
||||
|
||||
---
|
||||
|
||||
## Part A — Deck Plate (`saltyrover_chassis.scad`)
|
||||
|
||||
### Structural parts
|
||||
|
||||
| # | RENDER | Qty | Material | Process | Notes |
|
||||
|---|--------|-----|----------|---------|-------|
|
||||
| 1 | `deck_2d` | 1 | 8 mm 5052-H32 Al | Waterjet or CNC router | 480×500 mm blank |
|
||||
| — | — | — | **or** 8 mm PETG FDM | Print in sections, bolted lap joints | Prototype only |
|
||||
|
||||
### Fasteners (deck to motor brackets)
|
||||
|
||||
| # | Spec | Qty | Use |
|
||||
|---|------|-----|-----|
|
||||
| 2 | M5×20 SHCS | 16 | Motor bracket flange to deck (4 per corner × 4) |
|
||||
| 3 | M5 hex nut | 16 | Captured on deck underside |
|
||||
| 4 | M4×16 SHCS | 4 | Stem adapter flange to deck collar |
|
||||
| 5 | M4 flat washer | 4 | Under bolt head |
|
||||
|
||||
---
|
||||
|
||||
## Part B — Motor Brackets (`rover_motor_mount.scad`)
|
||||
|
||||
Print or CNC 4×. Each bracket handles one drive motor.
|
||||
|
||||
### Printed parts
|
||||
|
||||
| # | RENDER | Qty | Material | Settings | Notes |
|
||||
|---|--------|-----|----------|----------|-------|
|
||||
| 6 | `bracket` | 4 | PETG or PC | 5 perims, 60% infill | Print flange face down; add M5 inserts or use nuts |
|
||||
| 7 | `clamp_plate` | 4 | PETG | 4 perims, 40% infill | Axle retention; print flat |
|
||||
|
||||
**CNC alternative:** `bracket_2d` → DXF, CNC route from 10 mm 6061-T6 Al plate.
|
||||
|
||||
### Fasteners
|
||||
|
||||
| # | Spec | Qty | Use |
|
||||
|---|------|-----|-----|
|
||||
| 8 | M4×20 SHCS | 8 | Axle clamp plate to bracket (2 per motor × 4) |
|
||||
| 9 | M4 hex nut | 8 | Captured in bracket |
|
||||
| 10 | Axle lock nut | 4 | Axle tip retention — **verify axle thread before ordering** (BOM.md: Ø≈10 mm tip) |
|
||||
| 11 | M5×20 SHCS | 16 | Bracket flange to deck (from item 2 above) |
|
||||
|
||||
> ⚠ **Axle note:** BOM.md caliper values: base OD 16.11 mm, D-cut OD 15.95 mm, flat chord 13.00 mm.
|
||||
> Verify AXLE_D / AXLE_FLAT / BEARING_OD in `rover_motor_mount.scad` before printing.
|
||||
|
||||
---
|
||||
|
||||
## Part C — Battery Tray (`rover_battery_tray.scad`)
|
||||
|
||||
### Printed parts
|
||||
|
||||
| # | RENDER | Qty | Material | Settings | Notes |
|
||||
|---|--------|-----|----------|----------|-------|
|
||||
| 12 | `tray` | 1 | PETG | 4 perims, 30% infill | Print open-top face up; no supports |
|
||||
| 13 | `rail` | 2 | PETG | 5 perims, 40% infill | T-slot slide rail; print top-face down |
|
||||
| 14 | `latch` | 1 | PETG or TPU 95A | 4 perims, 30% infill | Spring retention clip; TPU gives better snap feel |
|
||||
|
||||
### Fasteners
|
||||
|
||||
| # | Spec | Qty | Use |
|
||||
|---|------|-----|-----|
|
||||
| 15 | M4×12 FHCS | 6 | Slide rail to deck underside (3 per rail, countersunk) |
|
||||
| 16 | M4 T-nut or nyloc | 6 | Captured under deck for rail bolts |
|
||||
| 17 | M3×10 SHCS | 2 | Spring latch to tray end |
|
||||
| 18 | Velcro strap 25 mm × 500 mm | 4–8 | Battery pack retention (1–2 per pack through tray floor slots) |
|
||||
|
||||
### Batteries
|
||||
|
||||
| # | Part | Qty | Spec |
|
||||
|---|------|-----|------|
|
||||
| 19 | Battery pack | 2–4 | 24 V, **420×88×56 mm** (BOM.md caliper-verified) |
|
||||
| 20 | BMS board | 1 | Matched to cell chemistry; mount to deck underside near Y+ wall |
|
||||
|
||||
---
|
||||
|
||||
## Part D — Stem Adapter (`rover_stem_adapter.scad`)
|
||||
|
||||
| # | RENDER | Qty | Material | Settings | Notes |
|
||||
|---|--------|-----|----------|----------|-------|
|
||||
| 21 | `base_flange` | 1 | PETG | 5 perims, 40% infill | Print flat; add M4 brass inserts or use through-bolts |
|
||||
| 22 | `clamp_front` | 1 | PETG | 5 perims, 40% infill | Flat-face down |
|
||||
| 23 | `clamp_rear` | 1 | PETG | 5 perims, 40% infill | Mirror; flat-face down |
|
||||
|
||||
### Fasteners
|
||||
|
||||
| # | Spec | Qty | Use |
|
||||
|---|------|-----|-----|
|
||||
| 24 | M4×20 SHCS | 2 | Stem clamp clamping bolts |
|
||||
| 25 | M4 hex nut | 2 | Captured in clamp rear half |
|
||||
| 26 | M4×8 set screw | 1 | Rotation lock (front half) |
|
||||
| 27 | M4×16 FHCS | 4 | Flange to deck collar (countersunk) |
|
||||
|
||||
### Stem tube
|
||||
|
||||
| # | Part | Qty | Spec | Notes |
|
||||
|---|------|-----|------|-------|
|
||||
| 28 | Vertical stem | 1 | 25 mm OD × 1.5 mm wall 6061-T6 Al, **550 mm length** | Cut from 1 m stock; SaltyLab uses 1000 mm |
|
||||
|
||||
> Sensor head, RPLIDAR, cameras, and roll cage from SaltyLab mount
|
||||
> directly onto this 25 mm stem — no modifications required.
|
||||
|
||||
---
|
||||
|
||||
## Motors
|
||||
|
||||
Same hoverboard hub motors as SaltyLab. **4× required for rover.**
|
||||
|
||||
| # | Part | Qty | Spec |
|
||||
|---|------|-----|------|
|
||||
| 29 | Hub motor | 4 | 10×2.125" tire, 36V nominal, ~350W; axle OD 16.11mm (caliper) |
|
||||
| 30 | Motor cable extension | 4 | 6-pin JST-PH 300 mm hall sensor + 3-phase power |
|
||||
|
||||
---
|
||||
|
||||
## Mass Estimate
|
||||
|
||||
| Assembly | Est. mass |
|
||||
|----------|-----------|
|
||||
| Al deck plate | ~1.4 kg |
|
||||
| Motor brackets × 4 (PETG) | ~0.5 kg |
|
||||
| Hub motors × 4 | ~7.2 kg |
|
||||
| Battery packs × 2 | ~1.4 kg |
|
||||
| Battery tray (PETG) | ~0.3 kg |
|
||||
| Stem + sensor head (from SaltyLab) | ~1.5 kg |
|
||||
| Electronics (FC + Orin + wiring) | ~0.6 kg |
|
||||
| Stem adapter + hardware | ~0.2 kg |
|
||||
| Fasteners | ~0.3 kg |
|
||||
| **Total estimate** | **~13.4 kg** |
|
||||
|
||||
> Wide 540 mm track + low 117 mm deck height → tip-over angle > 45°.
|
||||
> CG is at approximately Z = 200 mm with packs flat and full payload.
|
||||
|
||||
---
|
||||
|
||||
## Assembly Sequence
|
||||
|
||||
1. **Deck plate:** Cut or print `deck_2d`. Waterjet preferred for Al.
|
||||
2. **Motor brackets:** Print (or CNC) 4× `bracket` + 4× `clamp_plate`.
|
||||
- Test-fit axle D-cut bore before assembling to deck.
|
||||
3. **Slide rails:** Print 2× `rail`. Bolt to deck underside.
|
||||
4. **Stem adapter:** Print `base_flange` + `clamp_front` + `clamp_rear`.
|
||||
- Insert stem through deck collar bore; seat flange on collar top.
|
||||
- Bolt flange with 4× M4×16 FHCS.
|
||||
- Slide clamp halves on stem above flange; tighten M4 clamping bolts (1.5 N·m).
|
||||
- Tighten M4 set screw.
|
||||
5. **Motor brackets:** Slide flange into deck edge slot; 4× M5×20 bolts.
|
||||
- Do not torque until axle alignment is confirmed.
|
||||
6. **Motors:** Drop axle into bracket dropout slot. Install clamp plate.
|
||||
- Tighten clamp M4 bolts. Axle lock nut finger-tight + ¼ turn.
|
||||
7. **Battery tray:** Slide tray in from +X with packs loaded.
|
||||
- Latch snaps at full insertion.
|
||||
8. **Sensor head + stem accessories:** Install from SaltyLab as-is.
|
||||
- Recommend 550 mm stem for rover (sensor head at ~720 mm, stable).
|
||||
|
||||
---
|
||||
|
||||
## Tools Required
|
||||
|
||||
- M3/M4/M5 hex drivers
|
||||
- Torque wrench: M5 structural bolts 4 N·m, M4 clamp bolts 1.5 N·m
|
||||
- Thread locker (Loctite 243 blue) on axle lock nuts and stem clamp set screw
|
||||
- Dial caliper — verify axle dimensions before bracket print/CNC
|
||||
255
chassis/rover_battery_tray.scad
Normal file
255
chassis/rover_battery_tray.scad
Normal file
@ -0,0 +1,255 @@
|
||||
// ============================================================
|
||||
// rover_battery_tray.scad — SaltyRover Flat Battery Tray
|
||||
// Rev A 2026-03-01 sl-mechanical
|
||||
// ============================================================
|
||||
// Slide-out tray for 2–4 × 420×88×56 mm battery packs
|
||||
// laid FLAT below the rover deck plate.
|
||||
//
|
||||
// Packs orient with 420 mm left-right (X), 88 mm fore-aft
|
||||
// per pack (Y), 56 mm tall (Z). BATT_N packs stack fore-aft.
|
||||
//
|
||||
// Tray slides out to the RIGHT (+X) for battery swap.
|
||||
// Two slide rails (T-slot profile) run fore-aft inside
|
||||
// the deck opening. Tray body rides on these rails.
|
||||
// A spring-clip latch on the +X end retains the tray.
|
||||
//
|
||||
// Base plate attachment:
|
||||
// Rail mounts bolt to deck underside (4× M4 per rail,
|
||||
// counter-sunk flat-head).
|
||||
//
|
||||
// ⚠ VERIFY:
|
||||
// BATT_PACK_L / W / H from caliper.
|
||||
// RAIL_SLOT_W / H from your chosen extrusion or print.
|
||||
//
|
||||
// RENDER options:
|
||||
// "assembly" tray + phantom packs (default)
|
||||
// "tray" tray body for printing
|
||||
// "rail" one T-slot slide rail (print 2×)
|
||||
// "latch" spring latch for tray end (print 1×)
|
||||
// "tray_2d" floor projection → DXF
|
||||
// ============================================================
|
||||
|
||||
RENDER = "assembly";
|
||||
|
||||
// ── Battery packs (BOM.md caliper-verified) ───────────────
|
||||
BATT_PACK_L = 420.0; // left-right (X)
|
||||
BATT_PACK_W = 88.0; // fore-aft per pack (Y)
|
||||
BATT_PACK_H = 56.0; // height when flat (Z)
|
||||
BATT_N = 2; // packs arranged fore-aft (2 or 4)
|
||||
|
||||
// ── Tray geometry ─────────────────────────────────────────
|
||||
TRAY_WALL = 2.5; // wall thickness
|
||||
TRAY_FLOOR = 3.0; // floor thickness
|
||||
TRAY_PAD = 2.0; // per-side clearance inside tray (pack-to-wall)
|
||||
|
||||
// Inner cavity
|
||||
TRAY_INN_W = BATT_PACK_L + 2*TRAY_PAD;
|
||||
TRAY_INN_D = BATT_PACK_W * BATT_N + 2*TRAY_PAD;
|
||||
TRAY_INN_H = BATT_PACK_H + TRAY_PAD; // open top
|
||||
|
||||
// Outer tray body
|
||||
TRAY_OUT_W = TRAY_INN_W + 2*TRAY_WALL;
|
||||
TRAY_OUT_D = TRAY_INN_D + 2*TRAY_WALL;
|
||||
TRAY_OUT_H = TRAY_INN_H + TRAY_FLOOR;
|
||||
|
||||
// ── Slide rail ────────────────────────────────────────────
|
||||
// T-profile rail runs fore-aft (Y), mounts to deck underside.
|
||||
// Tray has matching T-slots on its side walls.
|
||||
RAIL_L = TRAY_OUT_D + 40.0; // rail longer than tray for stop
|
||||
RAIL_W = 14.0; // rail body width
|
||||
RAIL_H = 8.0; // rail height below deck
|
||||
RAIL_T_W = 10.0; // T-slot head width on tray side
|
||||
RAIL_T_H = 4.0; // T-slot head height (captured in tray slot)
|
||||
RAIL_CL = 0.3; // running clearance
|
||||
RAIL_M4_SPC = 80.0; // M4 deck attachment bolt spacing along rail
|
||||
|
||||
// Tray T-slot channel (cut into tray outer wall)
|
||||
SLOT_W = RAIL_T_W + 2*RAIL_CL;
|
||||
SLOT_H = RAIL_T_H + RAIL_CL;
|
||||
|
||||
// Rail Y-spacing: rails at ±RAIL_Y from tray centre (fore-aft)
|
||||
RAIL_Y_SPC = TRAY_INN_W - 20.0; // rails near X edges of tray
|
||||
// Note: rails run in Y, so RAIL_Y_SPC is the X spread between them
|
||||
|
||||
// ── Strap slots ───────────────────────────────────────────
|
||||
STRAP_W = 20.0; // Velcro strap width
|
||||
STRAP_T = 3.0; // slot depth
|
||||
|
||||
// ── Latch ─────────────────────────────────────────────────
|
||||
LATCH_L = 40.0; // latch body length
|
||||
LATCH_W = 20.0;
|
||||
LATCH_T = 4.0; // plate thickness
|
||||
LATCH_FLEX = 25.0; // spring arm length
|
||||
LATCH_TAB_H = 5.0; // retention tab height (hooks over deck edge)
|
||||
LATCH_BOLT_D= 3.3; // M3 attachment bolt
|
||||
|
||||
// ── Fasteners ─────────────────────────────────────────────
|
||||
M3_D = 3.2;
|
||||
M4_D = 4.3;
|
||||
M4_CS_D = 7.0; // flat-head countersink diam
|
||||
|
||||
$fn = 48;
|
||||
e = 0.01;
|
||||
|
||||
// ─────────────────────────────────────────────────────────
|
||||
// battery_tray()
|
||||
// Z=0 at tray bottom face. Slides in +X direction.
|
||||
// Inner cavity origin at (TRAY_WALL, TRAY_WALL, TRAY_FLOOR).
|
||||
// ─────────────────────────────────────────────────────────
|
||||
module battery_tray() {
|
||||
difference() {
|
||||
union() {
|
||||
// ── Outer tray body ────────────────────────
|
||||
cube([TRAY_OUT_W, TRAY_OUT_D, TRAY_OUT_H]);
|
||||
|
||||
// ── Pull tab on +X face ────────────────────
|
||||
translate([TRAY_OUT_W, TRAY_OUT_D/2 - 15, TRAY_FLOOR])
|
||||
cube([18, 30, TRAY_OUT_H - TRAY_FLOOR]);
|
||||
}
|
||||
|
||||
// ── Inner cavity (open top) ────────────────────
|
||||
translate([TRAY_WALL, TRAY_WALL, TRAY_FLOOR])
|
||||
cube([TRAY_INN_W, TRAY_INN_D, TRAY_INN_H + e]);
|
||||
|
||||
// ── Pack divider slot (for multi-pack separation) ──
|
||||
if (BATT_N > 1)
|
||||
for (pn = [1 : BATT_N - 1])
|
||||
translate([TRAY_WALL - e,
|
||||
TRAY_WALL + BATT_PACK_W * pn - 1,
|
||||
TRAY_FLOOR])
|
||||
cube([TRAY_INN_W + 2*e, 2, TRAY_INN_H + e]);
|
||||
|
||||
// ── Strap slots through floor (×2 per pack) ───
|
||||
for (pn = [0 : BATT_N - 1])
|
||||
for (sx = [TRAY_WALL + TRAY_INN_W*0.25,
|
||||
TRAY_WALL + TRAY_INN_W*0.75])
|
||||
translate([sx - STRAP_W/2,
|
||||
TRAY_WALL + BATT_PACK_W * pn + 10,
|
||||
-e])
|
||||
cube([STRAP_W, BATT_PACK_W - 20, TRAY_FLOOR + 2*e]);
|
||||
|
||||
// ── T-slot channels on left (+Y) and right (-Y) faces ──
|
||||
// Tray left wall: Y+ face — rail rides in from -X end
|
||||
for (face_y=[TRAY_OUT_D - TRAY_WALL - e, -e]) {
|
||||
translate([TRAY_OUT_W*0.2 - SLOT_W/2, face_y,
|
||||
TRAY_FLOOR + TRAY_INN_H/2 - SLOT_H/2])
|
||||
cube([TRAY_OUT_W*0.6, TRAY_WALL + 2*e, SLOT_H]);
|
||||
// T-head channel
|
||||
translate([TRAY_OUT_W*0.2 - RAIL_T_W/2 - RAIL_CL,
|
||||
face_y,
|
||||
TRAY_FLOOR + TRAY_INN_H/2 - SLOT_H/2 - RAIL_CL])
|
||||
cube([RAIL_T_W + 2*RAIL_CL,
|
||||
TRAY_WALL + 2*e,
|
||||
SLOT_H + 2*RAIL_CL]);
|
||||
}
|
||||
|
||||
// ── Ventilation / weight-save holes in floor (×4) ──
|
||||
for (vx=[TRAY_OUT_W*0.25, TRAY_OUT_W*0.75])
|
||||
for (vy=[TRAY_WALL + TRAY_INN_D*0.25,
|
||||
TRAY_WALL + TRAY_INN_D*0.75])
|
||||
translate([vx, vy, -e])
|
||||
cylinder(d=25, h=TRAY_FLOOR + 2*e);
|
||||
}
|
||||
}
|
||||
|
||||
// ─────────────────────────────────────────────────────────
|
||||
// slide_rail()
|
||||
// Mounts to deck underside. Runs fore-aft (Y).
|
||||
// T-head protrudes inward (X direction) into tray slot.
|
||||
// ─────────────────────────────────────────────────────────
|
||||
module slide_rail() {
|
||||
difference() {
|
||||
union() {
|
||||
// Rail body
|
||||
translate([-RAIL_W/2, 0, 0])
|
||||
cube([RAIL_W, RAIL_L, RAIL_H]);
|
||||
// T-head flange (protrudes toward tray)
|
||||
translate([-RAIL_T_W/2, 0, RAIL_H - RAIL_T_H])
|
||||
cube([RAIL_T_W, RAIL_L, RAIL_T_H]);
|
||||
}
|
||||
|
||||
// M4 countersunk attachment holes to deck (×3 along rail)
|
||||
for (ry = [15, RAIL_L/2, RAIL_L - 15])
|
||||
translate([0, ry, RAIL_H + e])
|
||||
rotate([180, 0, 0]) {
|
||||
cylinder(d=M4_D, h=RAIL_H + 2*e);
|
||||
cylinder(d1=M4_CS_D, d2=M4_D, h=3.5 + e);
|
||||
}
|
||||
|
||||
// Rail end stop slot (tray cannot slide past rail end)
|
||||
translate([-RAIL_W/2 - e, RAIL_L - 8, -e])
|
||||
cube([RAIL_W + 2*e, 8 + e, RAIL_H - RAIL_T_H + e]);
|
||||
}
|
||||
}
|
||||
|
||||
// ─────────────────────────────────────────────────────────
|
||||
// spring_latch()
|
||||
// Clips to tray +X end face. Spring arm hooks over
|
||||
// deck opening edge to retain tray. Squeeze tab to release.
|
||||
// ─────────────────────────────────────────────────────────
|
||||
module spring_latch() {
|
||||
difference() {
|
||||
union() {
|
||||
// Base plate (bolts to tray end face)
|
||||
cube([LATCH_W, LATCH_T, LATCH_L]);
|
||||
// Spring arm
|
||||
translate([LATCH_W/2 - 4, LATCH_T, LATCH_L - LATCH_FLEX])
|
||||
cube([8, LATCH_T + LATCH_TAB_H, LATCH_FLEX + LATCH_TAB_H]);
|
||||
// Retention tab
|
||||
translate([LATCH_W/2 - 6, LATCH_T, LATCH_L - 2])
|
||||
cube([12, LATCH_T + LATCH_TAB_H + 4, LATCH_TAB_H + 2]);
|
||||
}
|
||||
// M3 attachment holes (×2)
|
||||
for (lz=[10, LATCH_L - 20])
|
||||
translate([LATCH_W/2, -e, lz])
|
||||
rotate([-90,0,0])
|
||||
cylinder(d=LATCH_BOLT_D, h=LATCH_T + 2*e);
|
||||
}
|
||||
}
|
||||
|
||||
// ─────────────────────────────────────────────────────────
|
||||
// Render selector
|
||||
// ─────────────────────────────────────────────────────────
|
||||
if (RENDER == "assembly") {
|
||||
color("SteelBlue", 0.88) battery_tray();
|
||||
|
||||
// Phantom battery packs inside tray
|
||||
for (pn = [0 : BATT_N - 1])
|
||||
color("Gold", 0.35)
|
||||
translate([TRAY_WALL + TRAY_PAD,
|
||||
TRAY_WALL + TRAY_PAD + pn * BATT_PACK_W,
|
||||
TRAY_FLOOR])
|
||||
cube([BATT_PACK_L, BATT_PACK_W, BATT_PACK_H]);
|
||||
|
||||
// Slide rails
|
||||
color("Silver", 0.80) {
|
||||
translate([0, -20, TRAY_OUT_H])
|
||||
rotate([0, 90, 0])
|
||||
slide_rail();
|
||||
translate([0, TRAY_OUT_D + 20 - RAIL_L, TRAY_OUT_H])
|
||||
rotate([0, 90, 0])
|
||||
slide_rail();
|
||||
}
|
||||
|
||||
// Latch at +X end
|
||||
color("OrangeRed", 0.85)
|
||||
translate([TRAY_OUT_W + 18,
|
||||
TRAY_OUT_D/2 - LATCH_W/2,
|
||||
TRAY_OUT_H/2 - LATCH_L/2])
|
||||
rotate([0, -90, 0])
|
||||
spring_latch();
|
||||
|
||||
} else if (RENDER == "tray") {
|
||||
battery_tray();
|
||||
|
||||
} else if (RENDER == "rail") {
|
||||
slide_rail();
|
||||
|
||||
} else if (RENDER == "latch") {
|
||||
spring_latch();
|
||||
|
||||
} else if (RENDER == "tray_2d") {
|
||||
projection(cut=true)
|
||||
translate([0, 0, -TRAY_FLOOR/2])
|
||||
battery_tray();
|
||||
}
|
||||
240
chassis/rover_motor_mount.scad
Normal file
240
chassis/rover_motor_mount.scad
Normal file
@ -0,0 +1,240 @@
|
||||
// ============================================================
|
||||
// rover_motor_mount.scad — SaltyRover Drive Motor Brackets
|
||||
// Rev A 2026-03-01 sl-mechanical
|
||||
// ============================================================
|
||||
// L-bracket that bolts to the deck edge and positions the drive
|
||||
// motor axle at the correct height. Print or CNC 4×.
|
||||
//
|
||||
// Default values from BOM.md (caliper-verified hoverboard motor):
|
||||
// Axle base OD: 16.11 mm
|
||||
// D-cut OD: 15.95 mm (flat chord 13.00 mm)
|
||||
// Bearing seat OD: 37.80 mm
|
||||
// Axle CL above gnd: 127 mm
|
||||
// Tire OD: 254 mm (10×2.125")
|
||||
//
|
||||
// Bracket geometry (all dims in mm):
|
||||
// Vertical flange — bolts to deck edge (2× M5 SHCS)
|
||||
// Horizontal arm — extends BRKT_REACH beyond deck edge
|
||||
// to position axle CL at TRACK_W/2
|
||||
// Axle channel — open-bottom dropout slot for easy
|
||||
// motor install/removal; retained by
|
||||
// a separate clamping plate (×1 per bracket)
|
||||
// Bearing recess — 37.8 mm recess on inboard face
|
||||
//
|
||||
// ⚠ VERIFY before printing / CNC:
|
||||
// AXLE_D, AXLE_FLAT, BEARING_OD from caliper measurement.
|
||||
// DECK_BOT_H = GND_CLR + BATT_FLOOR_T + BATT_PACK_H.
|
||||
// Adjust BRKT_REACH so TRACK_W matches chassis layout.
|
||||
//
|
||||
// Print orientation: flange face flat on bed.
|
||||
// Material: PETG or PC, 5 perims, 60% infill.
|
||||
// Alternative: CNC-routed 10mm 6061-T6 aluminium plate.
|
||||
//
|
||||
// RENDER options:
|
||||
// "assembly" bracket + clamp plate + phantom motor axle
|
||||
// "bracket" main L-bracket for printing/CNC
|
||||
// "clamp_plate" axle retention clamp plate (print 4×)
|
||||
// "bracket_2d" bracket floor projection → DXF
|
||||
// ============================================================
|
||||
|
||||
RENDER = "assembly";
|
||||
|
||||
// ── Axle / motor (BOM.md caliper-verified) ────────────────
|
||||
AXLE_D = 16.11; // axle base section OD (round)
|
||||
AXLE_FLAT = 13.00; // D-cut flat chord width
|
||||
AXLE_D_DCUT = 15.95; // D-cut section OD
|
||||
BEARING_OD = 37.80; // bearing seat collar OD
|
||||
BEARING_D = BEARING_OD + 1.0; // bore clearance for bearing seat
|
||||
BEARING_H = 8.0; // bearing recess depth on inboard face
|
||||
|
||||
AXLE_H = 127.0; // axle CL above ground
|
||||
TIRE_OD = 254.0; // 10×2.125" tire outer diameter
|
||||
|
||||
// Dropout slot dims
|
||||
DROP_W = AXLE_D + 2.0; // slot width (axle + 1 mm each side)
|
||||
DROP_D = AXLE_H; // slot depth from bracket bottom (full height)
|
||||
|
||||
// ── Bracket geometry ─────────────────────────────────────
|
||||
// Deck geometry (must match saltyrover_chassis.scad)
|
||||
GND_CLR = 50.0;
|
||||
BATT_FLOOR_T = 3.0;
|
||||
BATT_PACK_H = 56.0;
|
||||
DECK_T = 8.0;
|
||||
DECK_BOT_H = GND_CLR + BATT_FLOOR_T + BATT_PACK_H; // 109 mm
|
||||
DECK_TOP_H = DECK_BOT_H + DECK_T; // 117 mm
|
||||
|
||||
// Axle height relative to deck top:
|
||||
AXLE_ABOVE_DECK = AXLE_H - DECK_TOP_H; // ~10 mm
|
||||
|
||||
// Bracket reach beyond deck edge to motor axle CL
|
||||
BRKT_REACH = 40.0; // motor CL is BRKT_REACH mm outside deck edge
|
||||
// (TRACK_W/2 - ROVER_W/2 = 270 - 240 = 30 mm +
|
||||
// bearing housing clearance margin = 40 mm)
|
||||
|
||||
// Bracket plate dimensions
|
||||
BRKT_T = 10.0; // bracket plate thickness
|
||||
BRKT_H_ABOVE = AXLE_ABOVE_DECK + BEARING_OD/2 + 8.0; // above deck top
|
||||
BRKT_H_BELOW = DECK_T + 12.0; // below deck bottom
|
||||
BRKT_TOTAL_H = BRKT_H_ABOVE + BRKT_H_BELOW; // full height
|
||||
|
||||
// Width of bracket arm (fore-aft direction)
|
||||
BRKT_W = 60.0; // covers motor fore-aft attachment bolts
|
||||
|
||||
// Flange for deck attachment
|
||||
FLANGE_T = 8.0; // flange plate thickness
|
||||
FLANGE_DEPTH = 20.0; // how deep flange sits on deck face (Y direction)
|
||||
|
||||
// M5 bolt holes: 2× through flange (deck attachment)
|
||||
M5_D = 5.3;
|
||||
MOT_BOLT_SPC = 30.0; // matches saltyrover_chassis.scad MOT_BOLT_SPC
|
||||
|
||||
// Clamping plate (retains axle in dropout slot)
|
||||
CLAMP_T = 6.0; // clamp plate thickness
|
||||
CLAMP_W = DROP_W + 12.0;
|
||||
CLAMP_H = BEARING_D + 12.0;
|
||||
CLAMP_BOLT_D= 4.3; // M4 clearance
|
||||
CLAMP_BOLT_SPC = CLAMP_W - 10.0;
|
||||
|
||||
// Gusset (triangular fillet between flange and arm)
|
||||
GUSSET_T = 8.0;
|
||||
|
||||
$fn = 64;
|
||||
e = 0.01;
|
||||
|
||||
// ─────────────────────────────────────────────────────────
|
||||
// motor_bracket()
|
||||
// Main L-bracket. Coordinate: Z=0 at deck top.
|
||||
// Bracket outboard face at X=0 (deck edge).
|
||||
// Bracket arm extends +X toward motor.
|
||||
// ─────────────────────────────────────────────────────────
|
||||
module motor_bracket() {
|
||||
axle_z = AXLE_ABOVE_DECK; // axle CL in bracket coords
|
||||
|
||||
difference() {
|
||||
union() {
|
||||
// ── Vertical arm (extends +X from deck edge) ──
|
||||
translate([0, -BRKT_W/2, -BRKT_H_BELOW])
|
||||
cube([BRKT_REACH + BRKT_T, BRKT_W, BRKT_TOTAL_H]);
|
||||
|
||||
// ── Vertical flange (sits against deck edge, -X side) ──
|
||||
translate([-FLANGE_DEPTH, -BRKT_W/2, -BRKT_H_BELOW])
|
||||
cube([FLANGE_DEPTH, BRKT_W, BRKT_TOTAL_H]);
|
||||
|
||||
// ── Gusset (arm to flange transition) ──
|
||||
translate([0, -BRKT_W/2, axle_z - GUSSET_T/2])
|
||||
linear_extrude(GUSSET_T)
|
||||
polygon([
|
||||
[0, 0],
|
||||
[BRKT_REACH/2, 0],
|
||||
[0, BRKT_W]
|
||||
]);
|
||||
}
|
||||
|
||||
// ── Axle dropout slot (open at bottom, centered on arm tip) ──
|
||||
translate([BRKT_REACH, -DROP_W/2, -BRKT_H_BELOW - e])
|
||||
cube([BRKT_T + e, DROP_W, AXLE_H + 2*e]);
|
||||
|
||||
// ── Axle round bore at slot top ──
|
||||
translate([BRKT_REACH, 0, axle_z])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d=AXLE_D + 1.0, h=BRKT_T + 2*e);
|
||||
|
||||
// ── D-cut anti-rotation flat (matches axle flat chord) ──
|
||||
translate([BRKT_REACH - e, -AXLE_FLAT/2, axle_z - AXLE_D/2 - e])
|
||||
cube([BRKT_T + 2*e, AXLE_FLAT, AXLE_D/2 + e]);
|
||||
|
||||
// ── Bearing seat recess (inboard face, X=BRKT_T) ──
|
||||
translate([BRKT_REACH + BRKT_T - BEARING_H, 0, axle_z])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d=BEARING_D, h=BEARING_H + e);
|
||||
|
||||
// ── Clamping plate bolt holes (×2, retain axle) ──
|
||||
for (dz=[-CLAMP_BOLT_SPC/2, CLAMP_BOLT_SPC/2])
|
||||
translate([BRKT_REACH - e, 0, axle_z + dz])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d=CLAMP_BOLT_D, h=BRKT_T + 2*e);
|
||||
|
||||
// ── Deck attachment holes through flange (M5 × 2) ──
|
||||
for (bz=[-MOT_BOLT_SPC/2, MOT_BOLT_SPC/2])
|
||||
translate([-FLANGE_DEPTH - e, 0, bz])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d=M5_D, h=FLANGE_DEPTH + 2*e);
|
||||
|
||||
// ── Lightening pocket on arm (inboard face) ──
|
||||
translate([BRKT_T, -BRKT_W/2 + 8, -BRKT_H_BELOW/2])
|
||||
cube([BRKT_REACH - BRKT_T - 8,
|
||||
BRKT_W - 16,
|
||||
BRKT_H_BELOW/2 - 4]);
|
||||
}
|
||||
}
|
||||
|
||||
// ─────────────────────────────────────────────────────────
|
||||
// axle_clamp_plate()
|
||||
// Bolts to bracket outboard face to close dropout slot.
|
||||
// Print flat face down. 1× per bracket.
|
||||
// ─────────────────────────────────────────────────────────
|
||||
module axle_clamp_plate() {
|
||||
axle_z = AXLE_ABOVE_DECK;
|
||||
|
||||
difference() {
|
||||
translate([-CLAMP_W/2, 0, axle_z - CLAMP_H/2])
|
||||
cube([CLAMP_W, CLAMP_T, CLAMP_H]);
|
||||
|
||||
// Axle bore
|
||||
translate([0, -e, axle_z])
|
||||
rotate([-90, 0, 0])
|
||||
cylinder(d=AXLE_D + 0.5, h=CLAMP_T + 2*e);
|
||||
|
||||
// Bearing seat relief
|
||||
translate([0, CLAMP_T - BEARING_H + e, axle_z])
|
||||
rotate([-90, 0, 0])
|
||||
cylinder(d=BEARING_D, h=BEARING_H + e);
|
||||
|
||||
// M4 bolt holes (×2)
|
||||
for (dz=[-CLAMP_BOLT_SPC/2, CLAMP_BOLT_SPC/2])
|
||||
translate([0, -e, axle_z + dz])
|
||||
rotate([-90, 0, 0])
|
||||
cylinder(d=CLAMP_BOLT_D, h=CLAMP_T + 2*e);
|
||||
}
|
||||
}
|
||||
|
||||
// ─────────────────────────────────────────────────────────
|
||||
// Render selector
|
||||
// ─────────────────────────────────────────────────────────
|
||||
if (RENDER == "assembly") {
|
||||
color("DimGray", 0.90) motor_bracket();
|
||||
color("SteelBlue", 0.85)
|
||||
translate([BRKT_REACH + BRKT_T, 0, 0])
|
||||
axle_clamp_plate();
|
||||
|
||||
// Phantom axle stub
|
||||
color("Silver", 0.4)
|
||||
translate([BRKT_REACH, 0, AXLE_ABOVE_DECK])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d=AXLE_D, h=80);
|
||||
|
||||
// Phantom tire (semi-transparent outline)
|
||||
color("Black", 0.12)
|
||||
translate([BRKT_REACH + 40, 0, AXLE_ABOVE_DECK])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d=TIRE_OD, h=54);
|
||||
|
||||
// Reference deck edge
|
||||
color("Gold", 0.25)
|
||||
translate([-FLANGE_DEPTH - 20, -BRKT_W/2, -BRKT_H_BELOW])
|
||||
cube([20, BRKT_W, BRKT_TOTAL_H]);
|
||||
|
||||
} else if (RENDER == "bracket") {
|
||||
motor_bracket();
|
||||
|
||||
} else if (RENDER == "clamp_plate") {
|
||||
// Orient for printing: lay flat, rotate upright
|
||||
translate([0, CLAMP_T, -AXLE_ABOVE_DECK])
|
||||
rotate([90, 0, 0])
|
||||
axle_clamp_plate();
|
||||
|
||||
} else if (RENDER == "bracket_2d") {
|
||||
projection(cut=true)
|
||||
translate([0, 0, -BRKT_H_BELOW - BRKT_T/2])
|
||||
motor_bracket();
|
||||
}
|
||||
202
chassis/rover_stem_adapter.scad
Normal file
202
chassis/rover_stem_adapter.scad
Normal file
@ -0,0 +1,202 @@
|
||||
// ============================================================
|
||||
// rover_stem_adapter.scad — SaltyRover Vertical Stem Adapter
|
||||
// Rev A 2026-03-01 sl-mechanical
|
||||
// ============================================================
|
||||
// Secures the 25 mm OD vertical stem to the rover deck.
|
||||
//
|
||||
// Two-part system:
|
||||
// base_flange() — annular flange plate bolts to deck top
|
||||
// (4× M4 SHCS into deck; deck already has
|
||||
// 25 mm stem bore through its centre collar)
|
||||
// stem_clamp() — split collar clamps on stem above the
|
||||
// deck collar; two M4 clamping bolts
|
||||
// lock stem position / rotation
|
||||
//
|
||||
// Reuses the existing sensor head, RPLIDAR, camera mounts, and
|
||||
// roll cage from SaltyLab — stem OD 25 mm is unchanged.
|
||||
//
|
||||
// Stem length options:
|
||||
// SaltyLab 1000 mm (balance robot — tall for CG)
|
||||
// SaltyRover 550 mm (rover — sensors visible, compact)
|
||||
// The adapter is identical; only the purchased tube differs.
|
||||
//
|
||||
// ⚠ Ensure the deck stem collar (saltyrover_chassis.scad,
|
||||
// STEM_COLLAR_OD=50 mm, H=22 mm) is the primary lateral
|
||||
// support. The flange + clamp provide torque/axial lock.
|
||||
//
|
||||
// RENDER options:
|
||||
// "assembly" flange + clamp + stem stub
|
||||
// "base_flange" flange plate for printing
|
||||
// "clamp_front" clamp front half for printing
|
||||
// "clamp_rear" clamp rear half for printing
|
||||
// "flange_2d" flange projection → DXF
|
||||
// ============================================================
|
||||
|
||||
RENDER = "assembly";
|
||||
|
||||
// ── Stem ─────────────────────────────────────────────────
|
||||
STEM_OD = 25.0;
|
||||
STEM_BORE = 25.4; // +0.4 clearance (same as sensor mounts)
|
||||
STEM_L_ROVER = 550; // recommended rover stem length (mm)
|
||||
|
||||
// ── Base flange ───────────────────────────────────────────
|
||||
// Sits on deck collar top. 4× M4 bolt through flange into deck.
|
||||
FLANGE_OD = 80.0; // outer diameter
|
||||
FLANGE_T = 6.0; // plate thickness
|
||||
FLANGE_BOLT_BC = 65.0; // M4 bolt circle diameter
|
||||
FLANGE_BOLT_D = 4.3; // M4 clearance
|
||||
FLANGE_BOLT_N = 4; // number of bolts (at 90°)
|
||||
// Deck collar height (must match saltyrover_chassis.scad STEM_COLLAR_H)
|
||||
DECK_COLLAR_H = 22.0;
|
||||
|
||||
// ── Split stem clamp ─────────────────────────────────────
|
||||
// Sits on top of flange; clamped M4 bolts lock stem.
|
||||
COL_OD = 52.0; // clamp outer diameter
|
||||
COL_H = 28.0; // clamp height (above flange)
|
||||
COL_BOLT_X = 19.0; // M4 clamping bolt CL from stem axis
|
||||
COL_BOLT_D = 4.5; // M4 clearance hole
|
||||
COL_NUT_W = 7.0; // M4 hex nut across-flats
|
||||
COL_NUT_H = 3.4; // hex nut height
|
||||
|
||||
// Set screw for rotation lock (front half)
|
||||
SET_SCREW_D = 4.5; // M4 set screw
|
||||
|
||||
// ── Fasteners ─────────────────────────────────────────────
|
||||
M4_D = 4.3;
|
||||
|
||||
$fn = 64;
|
||||
e = 0.01;
|
||||
|
||||
// ─────────────────────────────────────────────────────────
|
||||
// base_flange()
|
||||
// Sits on top of the deck stem collar.
|
||||
// Z=0 at deck top (collar rises from here).
|
||||
// ─────────────────────────────────────────────────────────
|
||||
module base_flange() {
|
||||
difference() {
|
||||
union() {
|
||||
// Annular flange plate (sits on collar top)
|
||||
translate([0, 0, DECK_COLLAR_H])
|
||||
cylinder(d=FLANGE_OD, h=FLANGE_T);
|
||||
// Short skirt that drops inside/over collar
|
||||
translate([0, 0, DECK_COLLAR_H - 4])
|
||||
cylinder(d=FLANGE_OD - 8, h=4);
|
||||
}
|
||||
|
||||
// Stem bore
|
||||
translate([0, 0, DECK_COLLAR_H - 4 - e])
|
||||
cylinder(d=STEM_BORE, h=FLANGE_T + 4 + 2*e);
|
||||
|
||||
// M4 bolts through flange + down into deck (×4)
|
||||
for (ang=[0, 90, 180, 270])
|
||||
rotate([0, 0, ang])
|
||||
translate([FLANGE_BOLT_BC/2, 0, DECK_COLLAR_H - e])
|
||||
cylinder(d=FLANGE_BOLT_D, h=FLANGE_T + 2*e);
|
||||
|
||||
// Countersink on top face
|
||||
for (ang=[0, 90, 180, 270])
|
||||
rotate([0, 0, ang])
|
||||
translate([FLANGE_BOLT_BC/2, 0, DECK_COLLAR_H + FLANGE_T - 3.5])
|
||||
cylinder(d1=FLANGE_BOLT_D, d2=8.5, h=3.5 + e);
|
||||
}
|
||||
}
|
||||
|
||||
// ─────────────────────────────────────────────────────────
|
||||
// stem_clamp_half(side)
|
||||
// Split collar clamps on stem above the flange.
|
||||
// Print flat-face-down. side = "front" | "rear"
|
||||
// ─────────────────────────────────────────────────────────
|
||||
module stem_clamp_half(side="front") {
|
||||
y_front = (side == "front");
|
||||
|
||||
// Clamp Z origin: on top of flange
|
||||
clamp_z0 = DECK_COLLAR_H + FLANGE_T;
|
||||
|
||||
difference() {
|
||||
// D-shaped half
|
||||
intersection() {
|
||||
translate([0, 0, clamp_z0])
|
||||
cylinder(d=COL_OD, h=COL_H);
|
||||
translate([-COL_OD/2,
|
||||
y_front ? 0 : -COL_OD/2,
|
||||
clamp_z0])
|
||||
cube([COL_OD, COL_OD/2, COL_H]);
|
||||
}
|
||||
|
||||
// Stem bore
|
||||
translate([0, 0, clamp_z0 - e])
|
||||
cylinder(d=STEM_BORE, h=COL_H + 2*e);
|
||||
|
||||
// M4 clamping bolt holes (Y direction, through front half)
|
||||
for (bx=[-COL_BOLT_X, COL_BOLT_X])
|
||||
translate([bx,
|
||||
y_front ? COL_OD/2 : 0,
|
||||
clamp_z0 + COL_H/2])
|
||||
rotate([90, 0, 0])
|
||||
cylinder(d=COL_BOLT_D, h=COL_OD/2 + e);
|
||||
|
||||
// M4 hex nut pockets in rear half
|
||||
if (!y_front)
|
||||
for (bx=[-COL_BOLT_X, COL_BOLT_X])
|
||||
translate([bx,
|
||||
-(COL_OD/4 + e),
|
||||
clamp_z0 + COL_H/2])
|
||||
rotate([90, 0, 0])
|
||||
cylinder(d=COL_NUT_W/cos(30),
|
||||
h=COL_NUT_H + e, $fn=6);
|
||||
|
||||
// M4 set screw hole (front half, mid-height, horizontal)
|
||||
if (y_front)
|
||||
translate([0, COL_OD/2,
|
||||
clamp_z0 + COL_H * 0.65])
|
||||
rotate([90, 0, 0])
|
||||
cylinder(d=SET_SCREW_D,
|
||||
h=COL_OD/2 - STEM_BORE/2 + e);
|
||||
|
||||
// Mating face chamfer (0.2 mm, prevents elephant-foot binding)
|
||||
translate([0, 0, clamp_z0 - e])
|
||||
rotate([0, 0, y_front ? 0 : 180])
|
||||
translate([-COL_OD/2, -0.2, 0])
|
||||
cube([COL_OD, 0.2, COL_H + 2*e]);
|
||||
}
|
||||
}
|
||||
|
||||
// ─────────────────────────────────────────────────────────
|
||||
// Render selector
|
||||
// ─────────────────────────────────────────────────────────
|
||||
if (RENDER == "assembly") {
|
||||
// Phantom deck collar reference
|
||||
color("Gray", 0.15)
|
||||
difference() {
|
||||
cylinder(d=50, h=DECK_COLLAR_H);
|
||||
translate([0,0,-e]) cylinder(d=STEM_BORE, h=DECK_COLLAR_H+2*e);
|
||||
}
|
||||
|
||||
color("SteelBlue", 0.90) base_flange();
|
||||
color("CornflowerBlue", 0.90) stem_clamp_half("front");
|
||||
color("SlateBlue", 0.90)
|
||||
mirror([0,1,0]) stem_clamp_half("rear");
|
||||
|
||||
// Phantom stem stub
|
||||
color("Silver", 0.30)
|
||||
translate([0, 0, DECK_COLLAR_H + FLANGE_T + COL_H])
|
||||
cylinder(d=STEM_OD, h=STEM_L_ROVER);
|
||||
|
||||
} else if (RENDER == "base_flange") {
|
||||
// Print flat; rotate flange down
|
||||
translate([0, 0, -(DECK_COLLAR_H - 4)])
|
||||
base_flange();
|
||||
|
||||
} else if (RENDER == "clamp_front") {
|
||||
translate([0, 0, -(DECK_COLLAR_H + FLANGE_T)])
|
||||
stem_clamp_half("front");
|
||||
|
||||
} else if (RENDER == "clamp_rear") {
|
||||
translate([0, 0, -(DECK_COLLAR_H + FLANGE_T)])
|
||||
stem_clamp_half("rear");
|
||||
|
||||
} else if (RENDER == "flange_2d") {
|
||||
projection(cut=true)
|
||||
translate([0, 0, -(DECK_COLLAR_H + FLANGE_T/2)])
|
||||
base_flange();
|
||||
}
|
||||
218
chassis/saltyrover_chassis.scad
Normal file
218
chassis/saltyrover_chassis.scad
Normal file
@ -0,0 +1,218 @@
|
||||
// ============================================================
|
||||
// saltyrover_chassis.scad — SaltyRover 4-Wheel Base Plate
|
||||
// Rev A 2026-03-01 sl-mechanical
|
||||
// ============================================================
|
||||
// Parametric deck plate for the stable 4-wheel SaltyRover.
|
||||
// Low/wide/stable design — reuses sensor head + 25 mm stem.
|
||||
//
|
||||
// Coordinate convention:
|
||||
// Z = 0 deck top face
|
||||
// +Y forward
|
||||
// +X right (motor side)
|
||||
//
|
||||
// Battery orientation:
|
||||
// Packs laid FLAT (56 mm tall, 420 mm running left-right,
|
||||
// 88 mm per pack fore-aft). N packs arranged fore-aft.
|
||||
// 2-pack tray: 430 × 186 mm opening.
|
||||
// 4-pack tray: 430 × 362 mm opening.
|
||||
//
|
||||
// Motor positions are at ±TRACK_W/2 (X), ±AXLE_BASE/2 (Y).
|
||||
// Motor brackets (rover_motor_mount.scad) bolt to deck edge
|
||||
// and extend BRKT_REACH mm outward to hold axle at AXLE_H.
|
||||
//
|
||||
// RENDER options:
|
||||
// "assembly" full deck + phantom batteries + stem stub
|
||||
// "deck" deck plate only (review)
|
||||
// "deck_2d" floor projection → DXF for waterjet/CNC
|
||||
// ============================================================
|
||||
|
||||
RENDER = "assembly";
|
||||
|
||||
// ── Deck footprint ────────────────────────────────────────
|
||||
ROVER_L = 500.0; // fore-aft (Y)
|
||||
ROVER_W = 480.0; // left-right (X) — sized around battery width
|
||||
DECK_T = 8.0; // deck thickness
|
||||
DECK_R = 15.0; // corner fillet radius
|
||||
|
||||
// ── Drive geometry (caliper-verified motor data from BOM.md) ──
|
||||
TRACK_W = 540.0; // motor axle CL to CL, left-right
|
||||
AXLE_BASE = 340.0; // motor axle CL to CL, fore-aft
|
||||
AXLE_H = 127.0; // axle CL above ground (10×2.125" wheel, caliper)
|
||||
GND_CLR = 50.0; // minimum chassis ground clearance
|
||||
|
||||
// Height of deck bottom above ground:
|
||||
// GND_CLR + battery_tray_floor + battery_height
|
||||
BATT_FLOOR_T = 3.0;
|
||||
BATT_PACK_H = 56.0; // flat-laid pack height
|
||||
DECK_BOT_H = GND_CLR + BATT_FLOOR_T + BATT_PACK_H; // 109 mm
|
||||
// Axle above deck top = AXLE_H - (DECK_BOT_H + DECK_T) ≈ +10 mm
|
||||
|
||||
// ── Battery packs (BOM.md caliper-verified: 420×88×56 mm) ──
|
||||
// Laid flat, 420 mm running left-right (X), 88 mm per pack (Y),
|
||||
// 56 mm tall (Z). BATT_N packs arranged side-by-side fore-aft.
|
||||
BATT_X_DIM = 420.0;
|
||||
BATT_Y_DIM = 88.0; // per-pack fore-aft depth
|
||||
BATT_N = 2; // packs fore-aft (2 = 176 mm; 4 = 352 mm)
|
||||
TRAY_MARGIN = 5.0; // extra margin on each side of tray opening
|
||||
|
||||
// ── Stem socket (deck centre) ─────────────────────────────
|
||||
STEM_BORE = 25.5; // 25 mm tube + 0.5 clearance
|
||||
STEM_COLLAR_OD = 50.0;
|
||||
STEM_COLLAR_H = 22.0; // raised boss height above deck top
|
||||
|
||||
// ── FC mount (MAMBA F722S, 30.5×30.5 M3) ─────────────────
|
||||
FC_SPACING = 30.5;
|
||||
FC_HOLE_D = 3.2;
|
||||
FC_STANDOFF_H= 6.0;
|
||||
// FC centred forward of battery tray
|
||||
FC_X = 0.0;
|
||||
FC_Y = ROVER_L/2 - 60.0; // near front edge
|
||||
|
||||
// ── Jetson Orin NX mount ─────────────────────────────────
|
||||
// ⚠ Verify hole pattern against your carrier board.
|
||||
// Default: 58×49 mm M3 (same as RPi HAT pattern used on Orin NX cards).
|
||||
ORIN_HOLE_X = 58.0;
|
||||
ORIN_HOLE_Y = 49.0;
|
||||
ORIN_HOLE_D = 3.2;
|
||||
ORIN_STANDOFF = 8.0;
|
||||
ORIN_X = 0.0;
|
||||
ORIN_Y = -(ROVER_L/2 - 55.0); // near rear edge
|
||||
|
||||
// ── Motor bracket attachment bolt pattern ────────────────
|
||||
// 2× M5 bolts through deck edge at each motor corner.
|
||||
MOT_BOLT_D = 5.3; // M5 clearance
|
||||
MOT_BOLT_SPC = 30.0; // bolt spacing fore-aft at each motor position
|
||||
|
||||
// ── Lightening holes ──────────────────────────────────────
|
||||
LH_D = 55.0;
|
||||
|
||||
// ── Fasteners ─────────────────────────────────────────────
|
||||
M3_D = 3.2;
|
||||
M4_D = 4.3;
|
||||
M5_D = 5.3;
|
||||
|
||||
$fn = 64;
|
||||
e = 0.01;
|
||||
|
||||
// ─────────────────────────────────────────────────────────
|
||||
// rounded_plate_2d()
|
||||
// ─────────────────────────────────────────────────────────
|
||||
module rounded_plate_2d(l, w, r) {
|
||||
offset(r=r, $fn=32) offset(r=-r)
|
||||
square([l, w], center=true);
|
||||
}
|
||||
|
||||
// ─────────────────────────────────────────────────────────
|
||||
// rover_deck()
|
||||
// ─────────────────────────────────────────────────────────
|
||||
module rover_deck() {
|
||||
tray_w = BATT_X_DIM + 2*TRAY_MARGIN; // 430 mm
|
||||
tray_l = BATT_Y_DIM * BATT_N + 2*TRAY_MARGIN; // 186 or 362 mm
|
||||
|
||||
difference() {
|
||||
// ── Deck plate ────────────────────────────────
|
||||
translate([0, 0, -DECK_T])
|
||||
linear_extrude(DECK_T)
|
||||
rounded_plate_2d(ROVER_W, ROVER_L, DECK_R);
|
||||
|
||||
// ── Battery tray opening (centred, through deck) ──
|
||||
translate([-tray_w/2, -tray_l/2, -DECK_T - e])
|
||||
cube([tray_w, tray_l, DECK_T + 2*e]);
|
||||
|
||||
// ── Lightening holes ──────────────────────────
|
||||
// 3 columns × 2 rows, outside battery opening
|
||||
for (hx = [-ROVER_W/4, 0, ROVER_W/4])
|
||||
for (hy = [-(tray_l/2 + LH_D/2 + 20),
|
||||
(tray_l/2 + LH_D/2 + 20)])
|
||||
if (abs(hy) < ROVER_L/2 - LH_D/2 - 10)
|
||||
translate([hx, hy, -DECK_T - e])
|
||||
cylinder(d=LH_D, h=DECK_T + 2*e);
|
||||
|
||||
// ── FC mount holes ────────────────────────────
|
||||
for (fx=[-FC_SPACING/2, FC_SPACING/2])
|
||||
for (fy=[-FC_SPACING/2, FC_SPACING/2])
|
||||
translate([FC_X + fx, FC_Y + fy, -DECK_T - e])
|
||||
cylinder(d=FC_HOLE_D, h=DECK_T + 2*e);
|
||||
|
||||
// ── Orin mount holes ──────────────────────────
|
||||
for (ox=[-ORIN_HOLE_X/2, ORIN_HOLE_X/2])
|
||||
for (oy=[-ORIN_HOLE_Y/2, ORIN_HOLE_Y/2])
|
||||
translate([ORIN_X + ox, ORIN_Y + oy, -DECK_T - e])
|
||||
cylinder(d=ORIN_HOLE_D, h=DECK_T + 2*e);
|
||||
|
||||
// ── Motor bracket bolt holes (4 corners × 2 bolts) ──
|
||||
for (sx=[-1,1], sy=[-1,1])
|
||||
for (by=[-MOT_BOLT_SPC/2, MOT_BOLT_SPC/2])
|
||||
translate([sx*ROVER_W/2 - sx*(DECK_T/2 + e),
|
||||
sy*AXLE_BASE/2 + by,
|
||||
-DECK_T - e])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d=MOT_BOLT_D, h=DECK_T + 2*e);
|
||||
|
||||
// ── Stem bore through deck ────────────────────
|
||||
translate([0, 0, -DECK_T - e])
|
||||
cylinder(d=STEM_BORE, h=DECK_T + 2*e);
|
||||
|
||||
// ── Cable routing slots (×2 per side, fore and aft) ──
|
||||
for (cy=[-ROVER_L/2 - e, ROVER_L/2 - 16])
|
||||
for (cx=[-55, 55])
|
||||
translate([cx - 7, cy, -DECK_T - e])
|
||||
cube([14, 16, DECK_T + 2*e]);
|
||||
}
|
||||
|
||||
// ── Stem collar (raised boss) ─────────────────────
|
||||
difference() {
|
||||
cylinder(d=STEM_COLLAR_OD, h=STEM_COLLAR_H);
|
||||
translate([0, 0, -e])
|
||||
cylinder(d=STEM_BORE, h=STEM_COLLAR_H + 2*e);
|
||||
}
|
||||
|
||||
// ── FC standoff posts (×4) ────────────────────────
|
||||
for (fx=[-FC_SPACING/2, FC_SPACING/2])
|
||||
for (fy=[-FC_SPACING/2, FC_SPACING/2])
|
||||
translate([FC_X + fx, FC_Y + fy, 0])
|
||||
cylinder(d=6, h=FC_STANDOFF_H);
|
||||
|
||||
// ── Orin standoff posts (×4) ──────────────────────
|
||||
for (ox=[-ORIN_HOLE_X/2, ORIN_HOLE_X/2])
|
||||
for (oy=[-ORIN_HOLE_Y/2, ORIN_HOLE_Y/2])
|
||||
translate([ORIN_X + ox, ORIN_Y + oy, 0])
|
||||
cylinder(d=6, h=ORIN_STANDOFF);
|
||||
}
|
||||
|
||||
// ─────────────────────────────────────────────────────────
|
||||
// Render selector
|
||||
// ─────────────────────────────────────────────────────────
|
||||
if (RENDER == "assembly") {
|
||||
color("Silver", 0.90) rover_deck();
|
||||
|
||||
// Phantom battery packs (2 × laid flat)
|
||||
tray_w = BATT_X_DIM + 2*TRAY_MARGIN;
|
||||
tray_l = BATT_Y_DIM * BATT_N + 2*TRAY_MARGIN;
|
||||
for (pn = [0 : BATT_N - 1])
|
||||
color("Gold", 0.30)
|
||||
translate([-BATT_X_DIM/2,
|
||||
-BATT_Y_DIM*BATT_N/2 + pn*BATT_Y_DIM,
|
||||
-(DECK_T + BATT_FLOOR_T + BATT_PACK_H)])
|
||||
cube([BATT_X_DIM, BATT_Y_DIM, BATT_PACK_H]);
|
||||
|
||||
// Phantom stem stub
|
||||
color("Silver", 0.25)
|
||||
translate([0, 0, STEM_COLLAR_H])
|
||||
cylinder(d=25, h=400);
|
||||
|
||||
// Motor position markers (spheres at axle CLs)
|
||||
axle_z = AXLE_H - (DECK_BOT_H + DECK_T); // above deck top
|
||||
for (sx=[-1,1], sy=[-1,1])
|
||||
color("Tomato", 0.5)
|
||||
translate([sx*TRACK_W/2, sy*AXLE_BASE/2, axle_z])
|
||||
sphere(d=20);
|
||||
|
||||
} else if (RENDER == "deck") {
|
||||
rover_deck();
|
||||
|
||||
} else if (RENDER == "deck_2d") {
|
||||
projection(cut=true)
|
||||
translate([0, 0, -DECK_T/2])
|
||||
rover_deck();
|
||||
}
|
||||
47
jetson/ros2_ws/src/saltyrover_drive/config/rover_params.yaml
Normal file
47
jetson/ros2_ws/src/saltyrover_drive/config/rover_params.yaml
Normal file
@ -0,0 +1,47 @@
|
||||
# rover_params.yaml
|
||||
# SaltyRover 4-wheel differential drive parameters.
|
||||
#
|
||||
# SaltyRover is a stable 4-wheel chassis (no balance PID).
|
||||
# Left side (FL + RL) and right side (FR + RR) run at the same speed per side.
|
||||
# STM32 command: {"cmd":"drive4","fl":N,"fr":N,"rl":N,"rr":N} N ∈ [-1000, 1000]
|
||||
#
|
||||
# Run with:
|
||||
# ros2 launch saltyrover_drive rover_drive.launch.py
|
||||
#
|
||||
# Data flow:
|
||||
# /cmd_vel → [rover_drive_node] → /rover/wheel_speeds → STM32
|
||||
|
||||
# ── Chassis geometry ───────────────────────────────────────────────────────────
|
||||
# Measure wheel_separation centre-to-centre between L and R drive wheels.
|
||||
# wheel_base is front-to-rear axle distance (used for future encoder odometry).
|
||||
wheel_separation: 0.45 # m (adjustable per chassis build)
|
||||
wheel_base: 0.40 # m
|
||||
wheel_radius: 0.075 # m (75 mm hub motor wheels)
|
||||
|
||||
# ── Speed ─────────────────────────────────────────────────────────────────────
|
||||
# max_speed is the m/s value that maps to ±1000 STM32 ticks.
|
||||
# Higher than SaltyLab (no tipping risk from balance).
|
||||
max_speed: 3.0 # m/s (= 10.8 km/h) — maps to ±1000
|
||||
max_linear_vel: 3.0 # m/s — cmd_vel hard clamp (same as max_speed initially)
|
||||
max_angular_vel: 2.5 # rad/s
|
||||
|
||||
# ── Acceleration limits ────────────────────────────────────────────────────────
|
||||
# 4-wheel stable chassis can handle faster ramps than SaltyLab balance robot.
|
||||
accel_limit: 2.0 # m/s² — still gentle for person-following smoothness
|
||||
decel_limit: 4.0 # m/s² — braking can be faster than accelerating
|
||||
|
||||
# ── Control ───────────────────────────────────────────────────────────────────
|
||||
control_rate: 50.0 # Hz — 20ms tick
|
||||
|
||||
# ── Watchdog ──────────────────────────────────────────────────────────────────
|
||||
# Zero wheel speeds if no cmd_vel received within cmd_timeout seconds.
|
||||
cmd_timeout: 0.5 # s
|
||||
|
||||
# ── Safety ────────────────────────────────────────────────────────────────────
|
||||
drive_enabled: true # runtime on/off: ros2 param set /rover_drive drive_enabled false
|
||||
|
||||
# ── Odometry ──────────────────────────────────────────────────────────────────
|
||||
publish_tf: true # broadcast odom → base_link TF
|
||||
odom_frame_id: odom
|
||||
base_frame_id: base_link
|
||||
publish_rate: 50.0 # Hz
|
||||
120
jetson/ros2_ws/src/saltyrover_drive/launch/rover_drive.launch.py
Normal file
120
jetson/ros2_ws/src/saltyrover_drive/launch/rover_drive.launch.py
Normal file
@ -0,0 +1,120 @@
|
||||
"""
|
||||
rover_drive.launch.py — SaltyRover 4-wheel differential drive launch.
|
||||
|
||||
Launches:
|
||||
rover_drive_node : /cmd_vel → diff-drive kinematics → /rover/wheel_speeds (STM32 JSON)
|
||||
rover_odom_node : /cmd_vel → dead-reckoning pose → /odom + TF odom→base_link
|
||||
|
||||
Usage:
|
||||
# Defaults from rover_params.yaml:
|
||||
ros2 launch saltyrover_drive rover_drive.launch.py
|
||||
|
||||
# Override wheel separation (wider chassis build):
|
||||
ros2 launch saltyrover_drive rover_drive.launch.py wheel_separation:=0.50
|
||||
|
||||
# Higher speed for open-area testing:
|
||||
ros2 launch saltyrover_drive rover_drive.launch.py max_speed:=4.0 max_linear_vel:=4.0
|
||||
|
||||
# Disable TF (if using robot_localization EKF instead):
|
||||
ros2 launch saltyrover_drive rover_drive.launch.py publish_tf:=false
|
||||
|
||||
Topics:
|
||||
/cmd_vel — Input (from person_follower, Nav2, or teleop)
|
||||
/rover/wheel_speeds — Output to STM32 bridge (JSON drive4 command)
|
||||
/rover/drive_status — Speed + enabled status (JSON String)
|
||||
/odom — Odometry (nav_msgs/Odometry)
|
||||
"""
|
||||
|
||||
import os
|
||||
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument, OpaqueFunction
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def _launch_nodes(context, *args, **kwargs):
|
||||
params_file = LaunchConfiguration("params_file").perform(context)
|
||||
|
||||
def _s(k): return LaunchConfiguration(k).perform(context)
|
||||
def _f(k): return float(LaunchConfiguration(k).perform(context))
|
||||
def _b(k): return LaunchConfiguration(k).perform(context).lower() == "true"
|
||||
|
||||
drive_params = {
|
||||
"wheel_separation": _f("wheel_separation"),
|
||||
"wheel_radius": _f("wheel_radius"),
|
||||
"max_speed": _f("max_speed"),
|
||||
"max_linear_vel": _f("max_linear_vel"),
|
||||
"max_angular_vel": _f("max_angular_vel"),
|
||||
"accel_limit": _f("accel_limit"),
|
||||
"decel_limit": _f("decel_limit"),
|
||||
"control_rate": _f("control_rate"),
|
||||
"cmd_timeout": _f("cmd_timeout"),
|
||||
"drive_enabled": _b("drive_enabled"),
|
||||
}
|
||||
odom_params = {
|
||||
"wheel_separation": _f("wheel_separation"),
|
||||
"publish_tf": _b("publish_tf"),
|
||||
"odom_frame_id": _s("odom_frame_id"),
|
||||
"base_frame_id": _s("base_frame_id"),
|
||||
"publish_rate": _f("control_rate"),
|
||||
}
|
||||
|
||||
node_params_drive = [params_file, drive_params] if params_file else [drive_params]
|
||||
node_params_odom = [params_file, odom_params] if params_file else [odom_params]
|
||||
|
||||
return [
|
||||
Node(
|
||||
package="saltyrover_drive",
|
||||
executable="rover_drive_node",
|
||||
name="rover_drive",
|
||||
output="screen",
|
||||
parameters=node_params_drive,
|
||||
),
|
||||
Node(
|
||||
package="saltyrover_drive",
|
||||
executable="rover_odom_node",
|
||||
name="rover_odom",
|
||||
output="screen",
|
||||
parameters=node_params_odom,
|
||||
),
|
||||
]
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
pkg_share = get_package_share_directory("saltyrover_drive")
|
||||
default_yaml = os.path.join(pkg_share, "config", "rover_params.yaml")
|
||||
|
||||
return LaunchDescription([
|
||||
DeclareLaunchArgument(
|
||||
"params_file", default_value=default_yaml,
|
||||
description="Full path to rover_params.yaml"),
|
||||
|
||||
# Chassis geometry
|
||||
DeclareLaunchArgument("wheel_separation", default_value="0.45",
|
||||
description="L/R wheel centre-to-centre distance (m)"),
|
||||
DeclareLaunchArgument("wheel_radius", default_value="0.075"),
|
||||
|
||||
# Speed
|
||||
DeclareLaunchArgument("max_speed", default_value="3.0",
|
||||
description="m/s corresponding to ±1000 STM32 ticks"),
|
||||
DeclareLaunchArgument("max_linear_vel", default_value="3.0"),
|
||||
DeclareLaunchArgument("max_angular_vel", default_value="2.5"),
|
||||
|
||||
# Accel
|
||||
DeclareLaunchArgument("accel_limit", default_value="2.0"),
|
||||
DeclareLaunchArgument("decel_limit", default_value="4.0"),
|
||||
|
||||
# Control
|
||||
DeclareLaunchArgument("control_rate", default_value="50.0"),
|
||||
DeclareLaunchArgument("cmd_timeout", default_value="0.5"),
|
||||
DeclareLaunchArgument("drive_enabled", default_value="true"),
|
||||
|
||||
# Odometry
|
||||
DeclareLaunchArgument("publish_tf", default_value="true"),
|
||||
DeclareLaunchArgument("odom_frame_id", default_value="odom"),
|
||||
DeclareLaunchArgument("base_frame_id", default_value="base_link"),
|
||||
|
||||
OpaqueFunction(function=_launch_nodes),
|
||||
])
|
||||
30
jetson/ros2_ws/src/saltyrover_drive/package.xml
Normal file
30
jetson/ros2_ws/src/saltyrover_drive/package.xml
Normal file
@ -0,0 +1,30 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>saltyrover_drive</name>
|
||||
<version>0.1.0</version>
|
||||
<description>
|
||||
4-wheel differential drive controller for SaltyRover variant.
|
||||
Converts /cmd_vel (Twist) to per-wheel speeds for STM32 via JSON drive4 command.
|
||||
Includes dead-reckoning odometry node. No balance PID required — stable chassis.
|
||||
</description>
|
||||
<maintainer email="sl-controls@saltylab.local">sl-controls</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>geometry_msgs</depend>
|
||||
<depend>nav_msgs</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>tf2_ros</depend>
|
||||
|
||||
<buildtool_depend>ament_python</buildtool_depend>
|
||||
|
||||
<test_depend>ament_copyright</test_depend>
|
||||
<test_depend>ament_flake8</test_depend>
|
||||
<test_depend>ament_pep257</test_depend>
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -0,0 +1,266 @@
|
||||
"""
|
||||
rover_drive_node.py — 4-wheel differential drive controller for SaltyRover.
|
||||
|
||||
SaltyRover is a stable 4-wheel chassis (no balance PID needed).
|
||||
Two independently-driven axles: left (FL + RL) and right (FR + RR) run
|
||||
at the same speed per side — classic tank/skid-steer kinematics.
|
||||
|
||||
Subscribes:
|
||||
/cmd_vel (geometry_msgs/Twist) — standard ROS2 velocity command
|
||||
|
||||
Publishes:
|
||||
/rover/wheel_speeds (std_msgs/String) — JSON drive command for STM32:
|
||||
{"cmd":"drive4","fl":N,"fr":N,"rl":N,"rr":N} N ∈ [-1000, 1000]
|
||||
/rover/drive_status (std_msgs/String) — JSON status: speeds + profile
|
||||
|
||||
Kinematics
|
||||
----------
|
||||
v_left = linear.x - angular.z × (wheel_separation / 2)
|
||||
v_right = linear.x + angular.z × (wheel_separation / 2)
|
||||
|
||||
Speed is scaled to integer ±1000 for STM32:
|
||||
wheel_int = clamp(v / max_speed, -1.0, 1.0) × 1000
|
||||
|
||||
Acceleration limiting
|
||||
---------------------
|
||||
Each wheel speed ramps toward the target at accel_limit m/s² (50 Hz tick).
|
||||
Deceleration uses a higher limit for responsive braking.
|
||||
|
||||
Safety
|
||||
------
|
||||
* cmd_vel input is hard-clamped to max_linear_vel / max_angular_vel.
|
||||
* A watchdog zeroes wheel speeds if no cmd_vel received in cmd_timeout s.
|
||||
* follow_enabled param for runtime on/off.
|
||||
|
||||
Usage
|
||||
-----
|
||||
ros2 launch saltyrover_drive rover_drive.launch.py
|
||||
ros2 launch saltyrover_drive rover_drive.launch.py max_speed:=3.0
|
||||
"""
|
||||
|
||||
import json
|
||||
import math
|
||||
import time
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from geometry_msgs.msg import Twist
|
||||
from std_msgs.msg import String
|
||||
|
||||
|
||||
# ── Pure kinematics functions ─────────────────────────────────────────────────
|
||||
|
||||
def clamp(v: float, lo: float, hi: float) -> float:
|
||||
return max(lo, min(hi, v))
|
||||
|
||||
|
||||
def diff_drive_kinematics(linear_x: float, angular_z: float,
|
||||
wheel_separation: float) -> tuple:
|
||||
"""
|
||||
Convert Twist to left/right wheel velocities (m/s).
|
||||
|
||||
wheel_separation: distance between left and right wheels (m)
|
||||
|
||||
Returns (v_left, v_right).
|
||||
"""
|
||||
half_sep = wheel_separation / 2.0
|
||||
v_left = linear_x - angular_z * half_sep
|
||||
v_right = linear_x + angular_z * half_sep
|
||||
return v_left, v_right
|
||||
|
||||
|
||||
def velocity_to_wheel_int(v: float, max_speed: float) -> int:
|
||||
"""
|
||||
Scale velocity (m/s) to integer wheel speed [-1000, +1000] for STM32.
|
||||
|
||||
max_speed : m/s corresponding to ±1000
|
||||
Returns int clamped to [-1000, +1000].
|
||||
"""
|
||||
if max_speed <= 0:
|
||||
return 0
|
||||
scaled = clamp(v / max_speed, -1.0, 1.0)
|
||||
return int(round(scaled * 1000))
|
||||
|
||||
|
||||
def build_drive4_cmd(fl: int, fr: int, rl: int, rr: int) -> str:
|
||||
"""Build STM32 JSON drive command for 4-wheel rover."""
|
||||
return json.dumps({"cmd": "drive4", "fl": fl, "fr": fr, "rl": rl, "rr": rr})
|
||||
|
||||
|
||||
def apply_ramp(current: float, target: float,
|
||||
accel_limit: float, decel_limit: float,
|
||||
dt: float) -> float:
|
||||
"""
|
||||
Trapezoidal ramp toward target.
|
||||
|
||||
Uses decel_limit when reducing magnitude (moving toward zero),
|
||||
accel_limit when increasing magnitude.
|
||||
"""
|
||||
delta = target - current
|
||||
decelerating = (current > 0 and delta < 0) or (current < 0 and delta > 0)
|
||||
limit = decel_limit if decelerating else accel_limit
|
||||
if delta > 0:
|
||||
step = min(delta, limit * dt)
|
||||
else:
|
||||
step = max(delta, -limit * dt)
|
||||
return current + step
|
||||
|
||||
|
||||
def compute_wheel_speeds(linear_x: float, angular_z: float,
|
||||
wheel_separation: float,
|
||||
max_linear_vel: float, max_angular_vel: float,
|
||||
max_speed: float) -> tuple:
|
||||
"""
|
||||
Full pipeline: Twist → clamped → kinematics → int wheel speeds.
|
||||
|
||||
Returns (fl, fr, rl, rr) as ints in [-1000, +1000].
|
||||
"""
|
||||
# Clamp input
|
||||
lx = clamp(linear_x, -max_linear_vel, max_linear_vel)
|
||||
az = clamp(angular_z, -max_angular_vel, max_angular_vel)
|
||||
|
||||
v_left, v_right = diff_drive_kinematics(lx, az, wheel_separation)
|
||||
|
||||
fl = velocity_to_wheel_int(v_left, max_speed)
|
||||
fr = velocity_to_wheel_int(v_right, max_speed)
|
||||
rl = fl # rear mirrors front (paired L/R)
|
||||
rr = fr
|
||||
|
||||
return fl, fr, rl, rr
|
||||
|
||||
|
||||
# ── ROS2 Node ─────────────────────────────────────────────────────────────────
|
||||
|
||||
class RoverDriveNode(Node):
|
||||
|
||||
def __init__(self):
|
||||
super().__init__("rover_drive")
|
||||
|
||||
# -- Parameters --------------------------------------------------------
|
||||
self.declare_parameter("wheel_separation", 0.45) # m — L/R wheel distance
|
||||
self.declare_parameter("wheel_radius", 0.075) # m
|
||||
self.declare_parameter("max_speed", 2.0) # m/s → maps to ±1000
|
||||
self.declare_parameter("max_linear_vel", 2.0) # cmd_vel clamp (m/s)
|
||||
self.declare_parameter("max_angular_vel", 2.0) # cmd_vel clamp (rad/s)
|
||||
self.declare_parameter("accel_limit", 1.5) # m/s²
|
||||
self.declare_parameter("decel_limit", 3.0) # m/s² (braking faster)
|
||||
self.declare_parameter("control_rate", 50.0) # Hz
|
||||
self.declare_parameter("cmd_timeout", 0.5) # s — watchdog
|
||||
self.declare_parameter("drive_enabled", True)
|
||||
|
||||
self._reload_params()
|
||||
|
||||
# -- State -------------------------------------------------------------
|
||||
self._target_vel_left = 0.0
|
||||
self._target_vel_right = 0.0
|
||||
self._current_vel_left = 0.0
|
||||
self._current_vel_right = 0.0
|
||||
self._last_cmd_t = 0.0 # monotonic; 0 = never received
|
||||
|
||||
# -- Subscriptions -----------------------------------------------------
|
||||
self.create_subscription(Twist, "/cmd_vel", self._cmd_vel_cb, 10)
|
||||
|
||||
# -- Publishers --------------------------------------------------------
|
||||
self._drive_pub = self.create_publisher(String, "/rover/wheel_speeds", 10)
|
||||
self._status_pub = self.create_publisher(String, "/rover/drive_status", 10)
|
||||
|
||||
# -- Control timer -----------------------------------------------------
|
||||
dt = 1.0 / self._p["control_rate"]
|
||||
self._timer = self.create_timer(dt, self._control_cb)
|
||||
|
||||
self.get_logger().info(
|
||||
f"RoverDrive ready sep={self._p['wheel_separation']}m "
|
||||
f"max={self._p['max_speed']}m/s "
|
||||
f"rate={self._p['control_rate']}Hz"
|
||||
)
|
||||
|
||||
def _reload_params(self):
|
||||
self._p = {
|
||||
"wheel_separation": self.get_parameter("wheel_separation").value,
|
||||
"wheel_radius": self.get_parameter("wheel_radius").value,
|
||||
"max_speed": self.get_parameter("max_speed").value,
|
||||
"max_linear_vel": self.get_parameter("max_linear_vel").value,
|
||||
"max_angular_vel": self.get_parameter("max_angular_vel").value,
|
||||
"accel_limit": self.get_parameter("accel_limit").value,
|
||||
"decel_limit": self.get_parameter("decel_limit").value,
|
||||
"control_rate": self.get_parameter("control_rate").value,
|
||||
"cmd_timeout": self.get_parameter("cmd_timeout").value,
|
||||
"drive_enabled": self.get_parameter("drive_enabled").value,
|
||||
}
|
||||
|
||||
def _cmd_vel_cb(self, msg: Twist):
|
||||
self._reload_params()
|
||||
p = self._p
|
||||
if not p["drive_enabled"]:
|
||||
self._target_vel_left = 0.0
|
||||
self._target_vel_right = 0.0
|
||||
return
|
||||
|
||||
lx = clamp(msg.linear.x, -p["max_linear_vel"], p["max_linear_vel"])
|
||||
az = clamp(msg.angular.z, -p["max_angular_vel"], p["max_angular_vel"])
|
||||
v_left, v_right = diff_drive_kinematics(lx, az, p["wheel_separation"])
|
||||
|
||||
# Clamp wheel velocities to max_speed
|
||||
self._target_vel_left = clamp(v_left, -p["max_speed"], p["max_speed"])
|
||||
self._target_vel_right = clamp(v_right, -p["max_speed"], p["max_speed"])
|
||||
self._last_cmd_t = time.monotonic()
|
||||
|
||||
def _control_cb(self):
|
||||
self._reload_params()
|
||||
p = self._p
|
||||
dt = 1.0 / p["control_rate"]
|
||||
|
||||
# Watchdog: zero target if no cmd_vel received recently
|
||||
if self._last_cmd_t > 0.0:
|
||||
age = time.monotonic() - self._last_cmd_t
|
||||
if age > p["cmd_timeout"]:
|
||||
self._target_vel_left = 0.0
|
||||
self._target_vel_right = 0.0
|
||||
|
||||
if not p["drive_enabled"]:
|
||||
self._target_vel_left = 0.0
|
||||
self._target_vel_right = 0.0
|
||||
|
||||
# Ramp toward target
|
||||
self._current_vel_left = apply_ramp(
|
||||
self._current_vel_left, self._target_vel_left,
|
||||
p["accel_limit"], p["decel_limit"], dt)
|
||||
self._current_vel_right = apply_ramp(
|
||||
self._current_vel_right, self._target_vel_right,
|
||||
p["accel_limit"], p["decel_limit"], dt)
|
||||
|
||||
# Convert to int wheel speeds
|
||||
fl = velocity_to_wheel_int(self._current_vel_left, p["max_speed"])
|
||||
fr = velocity_to_wheel_int(self._current_vel_right, p["max_speed"])
|
||||
rl, rr = fl, fr # rear mirrors front
|
||||
|
||||
# Publish STM32 command
|
||||
cmd_str = build_drive4_cmd(fl, fr, rl, rr)
|
||||
self._drive_pub.publish(String(data=cmd_str))
|
||||
|
||||
# Publish status
|
||||
status = {
|
||||
"fl": fl, "fr": fr, "rl": rl, "rr": rr,
|
||||
"v_left": round(self._current_vel_left, 3),
|
||||
"v_right": round(self._current_vel_right, 3),
|
||||
"enabled": p["drive_enabled"],
|
||||
}
|
||||
self._status_pub.publish(String(data=json.dumps(status)))
|
||||
|
||||
|
||||
# ── Entry point ───────────────────────────────────────────────────────────────
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = RoverDriveNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.try_shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@ -0,0 +1,197 @@
|
||||
"""
|
||||
rover_odom_node.py — Wheel odometry for SaltyRover (dead-reckoning from cmd_vel).
|
||||
|
||||
Uses the commanded velocity (dead-reckoning) since the STM32 encoder feedback
|
||||
may not always be available at launch. When encoder feedback is wired up,
|
||||
replace the cmd_vel subscription with actual wheel tick messages.
|
||||
|
||||
Subscribes:
|
||||
/cmd_vel (geometry_msgs/Twist) — commanded velocities (proxy for actual)
|
||||
|
||||
Publishes:
|
||||
/odom (nav_msgs/Odometry) — accumulated pose + velocity estimate
|
||||
TF: odom → base_link
|
||||
|
||||
Kinematics (differential drive):
|
||||
v = (v_right + v_left) / 2 = linear.x
|
||||
omega = (v_right - v_left) / wheel_sep = angular.z
|
||||
x' = x + v × cos(θ) × dt
|
||||
y' = y + v × sin(θ) × dt
|
||||
θ' = θ + omega × dt
|
||||
"""
|
||||
|
||||
import math
|
||||
import time
|
||||
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from geometry_msgs.msg import Twist, TransformStamped
|
||||
from nav_msgs.msg import Odometry
|
||||
|
||||
try:
|
||||
from tf2_ros import TransformBroadcaster
|
||||
_HAVE_TF2 = True
|
||||
except ImportError:
|
||||
_HAVE_TF2 = False
|
||||
|
||||
|
||||
# ── Pure odometry functions ───────────────────────────────────────────────────
|
||||
|
||||
def integrate_pose(x: float, y: float, theta: float,
|
||||
linear_x: float, angular_z: float,
|
||||
dt: float) -> tuple:
|
||||
"""
|
||||
Integrate pose using forward Euler kinematics.
|
||||
|
||||
x, y : position (m)
|
||||
theta : heading (rad), 0 = +X axis
|
||||
linear_x: forward velocity (m/s)
|
||||
angular_z: yaw rate (rad/s)
|
||||
dt : time step (s)
|
||||
|
||||
Returns (x', y', theta').
|
||||
"""
|
||||
x_new = x + linear_x * math.cos(theta) * dt
|
||||
y_new = y + linear_x * math.sin(theta) * dt
|
||||
theta_new = theta + angular_z * dt
|
||||
return x_new, y_new, theta_new
|
||||
|
||||
|
||||
def euler_to_quaternion_z(theta: float) -> tuple:
|
||||
"""
|
||||
Convert a yaw angle to a quaternion (rotation around Z only).
|
||||
|
||||
Returns (qx, qy, qz, qw).
|
||||
"""
|
||||
half = theta / 2.0
|
||||
return 0.0, 0.0, math.sin(half), math.cos(half)
|
||||
|
||||
|
||||
# ── ROS2 Node ─────────────────────────────────────────────────────────────────
|
||||
|
||||
class RoverOdomNode(Node):
|
||||
|
||||
def __init__(self):
|
||||
super().__init__("rover_odom")
|
||||
|
||||
# -- Parameters --------------------------------------------------------
|
||||
self.declare_parameter("wheel_separation", 0.45) # m
|
||||
self.declare_parameter("publish_tf", True)
|
||||
self.declare_parameter("odom_frame_id", "odom")
|
||||
self.declare_parameter("base_frame_id", "base_link")
|
||||
self.declare_parameter("publish_rate", 50.0) # Hz
|
||||
|
||||
p = self._params()
|
||||
self._odom_frame = p["odom_frame_id"]
|
||||
self._base_frame = p["base_frame_id"]
|
||||
self._publish_tf = p["publish_tf"] and _HAVE_TF2
|
||||
|
||||
# -- Pose state --------------------------------------------------------
|
||||
self._x = 0.0
|
||||
self._y = 0.0
|
||||
self._theta = 0.0
|
||||
self._vx = 0.0
|
||||
self._omega = 0.0
|
||||
self._last_t = None # time.monotonic() of last cmd_vel
|
||||
|
||||
# -- TF2 broadcaster ---------------------------------------------------
|
||||
if self._publish_tf:
|
||||
self._tf_broadcaster = TransformBroadcaster(self)
|
||||
|
||||
# -- Subscriptions -----------------------------------------------------
|
||||
self.create_subscription(Twist, "/cmd_vel", self._cmd_vel_cb, 10)
|
||||
|
||||
# -- Publishers --------------------------------------------------------
|
||||
self._odom_pub = self.create_publisher(Odometry, "/odom", 10)
|
||||
|
||||
# -- Timer -------------------------------------------------------------
|
||||
self._timer = self.create_timer(
|
||||
1.0 / p["publish_rate"], self._publish_cb)
|
||||
|
||||
self.get_logger().info("RoverOdom ready (dead-reckoning from /cmd_vel)")
|
||||
|
||||
def _params(self) -> dict:
|
||||
return {
|
||||
"wheel_separation": self.get_parameter("wheel_separation").value,
|
||||
"publish_tf": self.get_parameter("publish_tf").value,
|
||||
"odom_frame_id": self.get_parameter("odom_frame_id").value,
|
||||
"base_frame_id": self.get_parameter("base_frame_id").value,
|
||||
"publish_rate": self.get_parameter("publish_rate").value,
|
||||
}
|
||||
|
||||
def _cmd_vel_cb(self, msg: Twist):
|
||||
now = time.monotonic()
|
||||
if self._last_t is not None:
|
||||
dt = now - self._last_t
|
||||
if 0 < dt < 1.0: # sanity: ignore if gap > 1s
|
||||
self._x, self._y, self._theta = integrate_pose(
|
||||
self._x, self._y, self._theta,
|
||||
msg.linear.x, msg.angular.z, dt)
|
||||
self._last_t = now
|
||||
self._vx = msg.linear.x
|
||||
self._omega = msg.angular.z
|
||||
|
||||
def _publish_cb(self):
|
||||
now_msg = self.get_clock().now().to_msg()
|
||||
qx, qy, qz, qw = euler_to_quaternion_z(self._theta)
|
||||
|
||||
# -- Odometry message --------------------------------------------------
|
||||
odom = Odometry()
|
||||
odom.header.stamp = now_msg
|
||||
odom.header.frame_id = self._odom_frame
|
||||
odom.child_frame_id = self._base_frame
|
||||
|
||||
odom.pose.pose.position.x = self._x
|
||||
odom.pose.pose.position.y = self._y
|
||||
odom.pose.pose.position.z = 0.0
|
||||
odom.pose.pose.orientation.x = qx
|
||||
odom.pose.pose.orientation.y = qy
|
||||
odom.pose.pose.orientation.z = qz
|
||||
odom.pose.pose.orientation.w = qw
|
||||
|
||||
odom.twist.twist.linear.x = self._vx
|
||||
odom.twist.twist.angular.z = self._omega
|
||||
|
||||
# Covariance: dead-reckoning, moderate confidence
|
||||
odom.pose.covariance[0] = 0.1 # x
|
||||
odom.pose.covariance[7] = 0.1 # y
|
||||
odom.pose.covariance[35] = 0.05 # yaw
|
||||
odom.twist.covariance[0] = 0.01
|
||||
odom.twist.covariance[35] = 0.01
|
||||
|
||||
self._odom_pub.publish(odom)
|
||||
|
||||
# -- TF broadcast ------------------------------------------------------
|
||||
if self._publish_tf:
|
||||
tf = TransformStamped()
|
||||
tf.header.stamp = now_msg
|
||||
tf.header.frame_id = self._odom_frame
|
||||
tf.child_frame_id = self._base_frame
|
||||
|
||||
tf.transform.translation.x = self._x
|
||||
tf.transform.translation.y = self._y
|
||||
tf.transform.translation.z = 0.0
|
||||
tf.transform.rotation.x = qx
|
||||
tf.transform.rotation.y = qy
|
||||
tf.transform.rotation.z = qz
|
||||
tf.transform.rotation.w = qw
|
||||
|
||||
self._tf_broadcaster.sendTransform(tf)
|
||||
|
||||
|
||||
# ── Entry point ───────────────────────────────────────────────────────────────
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = RoverOdomNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.try_shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
4
jetson/ros2_ws/src/saltyrover_drive/setup.cfg
Normal file
4
jetson/ros2_ws/src/saltyrover_drive/setup.cfg
Normal file
@ -0,0 +1,4 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/saltyrover_drive
|
||||
[install]
|
||||
install_scripts=$base/lib/saltyrover_drive
|
||||
30
jetson/ros2_ws/src/saltyrover_drive/setup.py
Normal file
30
jetson/ros2_ws/src/saltyrover_drive/setup.py
Normal file
@ -0,0 +1,30 @@
|
||||
from setuptools import setup
|
||||
|
||||
package_name = "saltyrover_drive"
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version="0.1.0",
|
||||
packages=[package_name],
|
||||
data_files=[
|
||||
("share/ament_index/resource_index/packages", [f"resource/{package_name}"]),
|
||||
(f"share/{package_name}", ["package.xml"]),
|
||||
(f"share/{package_name}/launch", ["launch/rover_drive.launch.py"]),
|
||||
(f"share/{package_name}/config", ["config/rover_params.yaml"]),
|
||||
],
|
||||
install_requires=["setuptools"],
|
||||
zip_safe=True,
|
||||
maintainer="sl-controls",
|
||||
maintainer_email="sl-controls@saltylab.local",
|
||||
description="4-wheel diff-drive controller for SaltyRover (tank kinematics + dead-reckoning odom)",
|
||||
license="MIT",
|
||||
tests_require=["pytest"],
|
||||
entry_points={
|
||||
"console_scripts": [
|
||||
# /cmd_vel → diff-drive kinematics → /rover/wheel_speeds (STM32 JSON)
|
||||
"rover_drive_node = saltyrover_drive.rover_drive_node:main",
|
||||
# /cmd_vel → dead-reckoning pose → /odom + TF
|
||||
"rover_odom_node = saltyrover_drive.rover_odom_node:main",
|
||||
],
|
||||
},
|
||||
)
|
||||
414
jetson/ros2_ws/src/saltyrover_drive/test/test_rover_drive.py
Normal file
414
jetson/ros2_ws/src/saltyrover_drive/test/test_rover_drive.py
Normal file
@ -0,0 +1,414 @@
|
||||
"""
|
||||
Unit tests for saltyrover_drive pure functions.
|
||||
No ROS2 runtime required.
|
||||
Run with: pytest jetson/ros2_ws/src/saltyrover_drive/test/test_rover_drive.py
|
||||
"""
|
||||
|
||||
import json
|
||||
import math
|
||||
|
||||
import pytest
|
||||
|
||||
|
||||
# ── Pure function mirrors ─────────────────────────────────────────────────────
|
||||
|
||||
def _clamp(v, lo, hi):
|
||||
return max(lo, min(hi, v))
|
||||
|
||||
|
||||
def _diff_drive_kinematics(linear_x, angular_z, wheel_separation):
|
||||
half_sep = wheel_separation / 2.0
|
||||
v_left = linear_x - angular_z * half_sep
|
||||
v_right = linear_x + angular_z * half_sep
|
||||
return v_left, v_right
|
||||
|
||||
|
||||
def _velocity_to_wheel_int(v, max_speed):
|
||||
if max_speed <= 0:
|
||||
return 0
|
||||
scaled = _clamp(v / max_speed, -1.0, 1.0)
|
||||
return int(round(scaled * 1000))
|
||||
|
||||
|
||||
def _build_drive4_cmd(fl, fr, rl, rr):
|
||||
return json.dumps({"cmd": "drive4", "fl": fl, "fr": fr, "rl": rl, "rr": rr})
|
||||
|
||||
|
||||
def _apply_ramp(current, target, accel_limit, decel_limit, dt):
|
||||
delta = target - current
|
||||
decelerating = (current > 0 and delta < 0) or (current < 0 and delta > 0)
|
||||
limit = decel_limit if decelerating else accel_limit
|
||||
if delta > 0:
|
||||
step = min(delta, limit * dt)
|
||||
else:
|
||||
step = max(delta, -limit * dt)
|
||||
return current + step
|
||||
|
||||
|
||||
def _compute_wheel_speeds(linear_x, angular_z, wheel_separation,
|
||||
max_linear_vel, max_angular_vel, max_speed):
|
||||
lx = _clamp(linear_x, -max_linear_vel, max_linear_vel)
|
||||
az = _clamp(angular_z, -max_angular_vel, max_angular_vel)
|
||||
v_left, v_right = _diff_drive_kinematics(lx, az, wheel_separation)
|
||||
fl = _velocity_to_wheel_int(v_left, max_speed)
|
||||
fr = _velocity_to_wheel_int(v_right, max_speed)
|
||||
rl, rr = fl, fr
|
||||
return fl, fr, rl, rr
|
||||
|
||||
|
||||
def _integrate_pose(x, y, theta, linear_x, angular_z, dt):
|
||||
x_new = x + linear_x * math.cos(theta) * dt
|
||||
y_new = y + linear_x * math.sin(theta) * dt
|
||||
theta_new = theta + angular_z * dt
|
||||
return x_new, y_new, theta_new
|
||||
|
||||
|
||||
def _euler_to_quaternion_z(theta):
|
||||
half = theta / 2.0
|
||||
return 0.0, 0.0, math.sin(half), math.cos(half)
|
||||
|
||||
|
||||
# ── Differential drive kinematics ────────────────────────────────────────────
|
||||
|
||||
class TestDiffDriveKinematics:
|
||||
_SEP = 0.45 # m — standard rover wheel separation
|
||||
|
||||
def test_straight_equal_speeds(self):
|
||||
"""Straight ahead: both wheels same speed."""
|
||||
vl, vr = _diff_drive_kinematics(1.0, 0.0, self._SEP)
|
||||
assert vl == pytest.approx(1.0)
|
||||
assert vr == pytest.approx(1.0)
|
||||
|
||||
def test_turn_left_right_faster(self):
|
||||
"""Turn left (positive angular_z): right wheel faster."""
|
||||
vl, vr = _diff_drive_kinematics(1.0, 1.0, self._SEP)
|
||||
assert vr > vl
|
||||
|
||||
def test_turn_right_left_faster(self):
|
||||
"""Turn right (negative angular_z): left wheel faster."""
|
||||
vl, vr = _diff_drive_kinematics(1.0, -1.0, self._SEP)
|
||||
assert vl > vr
|
||||
|
||||
def test_pivot_left_in_place(self):
|
||||
"""Pivot turn left in place (linear=0): right forward, left backward."""
|
||||
vl, vr = _diff_drive_kinematics(0.0, 1.0, self._SEP)
|
||||
assert vl < 0
|
||||
assert vr > 0
|
||||
assert vl == pytest.approx(-vr) # symmetric
|
||||
|
||||
def test_pivot_speed_proportional_to_separation(self):
|
||||
"""Pivot speed = angular_z × sep/2."""
|
||||
angular_z = 2.0
|
||||
sep = 0.45
|
||||
vl, vr = _diff_drive_kinematics(0.0, angular_z, sep)
|
||||
expected_half = angular_z * sep / 2.0
|
||||
assert vr == pytest.approx( expected_half)
|
||||
assert vl == pytest.approx(-expected_half)
|
||||
|
||||
def test_backward_straight(self):
|
||||
vl, vr = _diff_drive_kinematics(-1.0, 0.0, self._SEP)
|
||||
assert vl == pytest.approx(-1.0)
|
||||
assert vr == pytest.approx(-1.0)
|
||||
|
||||
def test_zero_cmd(self):
|
||||
vl, vr = _diff_drive_kinematics(0.0, 0.0, self._SEP)
|
||||
assert vl == pytest.approx(0.0)
|
||||
assert vr == pytest.approx(0.0)
|
||||
|
||||
def test_turning_radius(self):
|
||||
"""
|
||||
Turning radius R = linear_x / angular_z.
|
||||
Also: v_right = omega × (R + sep/2), v_left = omega × (R - sep/2)
|
||||
Verify ratio: v_right/v_left = (R + sep/2)/(R - sep/2).
|
||||
"""
|
||||
lx, az, sep = 1.0, 0.5, 0.45
|
||||
vl, vr = _diff_drive_kinematics(lx, az, sep)
|
||||
R = lx / az # = 2.0 m
|
||||
expected_ratio = (R + sep / 2) / (R - sep / 2)
|
||||
assert vr / vl == pytest.approx(expected_ratio, rel=1e-5)
|
||||
|
||||
def test_wider_separation_higher_speed_diff(self):
|
||||
"""Wider chassis → larger speed difference for same angular command."""
|
||||
vl_narrow, vr_narrow = _diff_drive_kinematics(1.0, 1.0, 0.30)
|
||||
vl_wide, vr_wide = _diff_drive_kinematics(1.0, 1.0, 0.60)
|
||||
assert (vr_wide - vl_wide) > (vr_narrow - vl_narrow)
|
||||
|
||||
|
||||
# ── Velocity to wheel integer ─────────────────────────────────────────────────
|
||||
|
||||
class TestVelocityToWheelInt:
|
||||
|
||||
def test_full_forward(self):
|
||||
assert _velocity_to_wheel_int(3.0, 3.0) == 1000
|
||||
|
||||
def test_full_reverse(self):
|
||||
assert _velocity_to_wheel_int(-3.0, 3.0) == -1000
|
||||
|
||||
def test_half_speed(self):
|
||||
assert _velocity_to_wheel_int(1.5, 3.0) == 500
|
||||
|
||||
def test_stopped(self):
|
||||
assert _velocity_to_wheel_int(0.0, 3.0) == 0
|
||||
|
||||
def test_clamp_above_max(self):
|
||||
assert _velocity_to_wheel_int(5.0, 3.0) == 1000
|
||||
|
||||
def test_clamp_below_min(self):
|
||||
assert _velocity_to_wheel_int(-5.0, 3.0) == -1000
|
||||
|
||||
def test_zero_max_speed_safe(self):
|
||||
"""max_speed=0 must not divide by zero."""
|
||||
assert _velocity_to_wheel_int(1.0, 0.0) == 0
|
||||
|
||||
def test_rounding(self):
|
||||
"""0.5005 * 1000 = 500.5 → rounds to 501."""
|
||||
# 1.5015 / 3.0 = 0.5005
|
||||
result = _velocity_to_wheel_int(1.5015, 3.0)
|
||||
assert result in (500, 501) # rounding may differ slightly
|
||||
|
||||
def test_one_third(self):
|
||||
"""1.0 / 3.0 m/s → ~333."""
|
||||
result = _velocity_to_wheel_int(1.0, 3.0)
|
||||
assert result == 333
|
||||
|
||||
|
||||
# ── Drive command JSON builder ────────────────────────────────────────────────
|
||||
|
||||
class TestBuildDrive4Cmd:
|
||||
|
||||
def test_valid_json(self):
|
||||
cmd = _build_drive4_cmd(100, 200, 100, 200)
|
||||
parsed = json.loads(cmd)
|
||||
assert parsed["cmd"] == "drive4"
|
||||
|
||||
def test_fields_present(self):
|
||||
cmd = json.loads(_build_drive4_cmd(10, 20, 30, 40))
|
||||
assert cmd["fl"] == 10
|
||||
assert cmd["fr"] == 20
|
||||
assert cmd["rl"] == 30
|
||||
assert cmd["rr"] == 40
|
||||
|
||||
def test_zero_speeds(self):
|
||||
cmd = json.loads(_build_drive4_cmd(0, 0, 0, 0))
|
||||
assert all(cmd[k] == 0 for k in ("fl", "fr", "rl", "rr"))
|
||||
|
||||
def test_negative_values(self):
|
||||
cmd = json.loads(_build_drive4_cmd(-500, -500, -500, -500))
|
||||
assert cmd["fl"] == -500
|
||||
|
||||
def test_rear_mirrors_front_in_pipeline(self):
|
||||
"""In compute_wheel_speeds, rl=fl and rr=fr."""
|
||||
fl, fr, rl, rr = _compute_wheel_speeds(1.0, 0.0, 0.45, 3.0, 2.5, 3.0)
|
||||
assert rl == fl
|
||||
assert rr == fr
|
||||
|
||||
|
||||
# ── Full pipeline (compute_wheel_speeds) ─────────────────────────────────────
|
||||
|
||||
class TestComputeWheelSpeeds:
|
||||
_SEP = 0.45
|
||||
_MAX_LIN = 3.0
|
||||
_MAX_ANG = 2.5
|
||||
_MAX_SPD = 3.0
|
||||
|
||||
def test_straight_full_speed(self):
|
||||
fl, fr, rl, rr = _compute_wheel_speeds(
|
||||
3.0, 0.0, self._SEP, self._MAX_LIN, self._MAX_ANG, self._MAX_SPD)
|
||||
assert fl == fr == rl == rr == 1000
|
||||
|
||||
def test_stopped(self):
|
||||
fl, fr, rl, rr = _compute_wheel_speeds(
|
||||
0.0, 0.0, self._SEP, self._MAX_LIN, self._MAX_ANG, self._MAX_SPD)
|
||||
assert fl == fr == rl == rr == 0
|
||||
|
||||
def test_turn_left_fr_greater(self):
|
||||
fl, fr, rl, rr = _compute_wheel_speeds(
|
||||
1.0, 1.0, self._SEP, self._MAX_LIN, self._MAX_ANG, self._MAX_SPD)
|
||||
assert fr > fl
|
||||
|
||||
def test_clamp_linear_vel(self):
|
||||
"""cmd > max_linear_vel is clamped."""
|
||||
fl1, fr1, _, _ = _compute_wheel_speeds(
|
||||
3.0, 0.0, self._SEP, self._MAX_LIN, self._MAX_ANG, self._MAX_SPD)
|
||||
fl2, fr2, _, _ = _compute_wheel_speeds(
|
||||
10.0, 0.0, self._SEP, self._MAX_LIN, self._MAX_ANG, self._MAX_SPD)
|
||||
assert fl1 == fl2 == 1000
|
||||
|
||||
def test_reverse(self):
|
||||
fl, fr, rl, rr = _compute_wheel_speeds(
|
||||
-3.0, 0.0, self._SEP, self._MAX_LIN, self._MAX_ANG, self._MAX_SPD)
|
||||
assert fl == fr == rl == rr == -1000
|
||||
|
||||
def test_pivot_turn_symmetric(self):
|
||||
"""Pure rotation: FL/RL negative, FR/RR positive, symmetric."""
|
||||
fl, fr, rl, rr = _compute_wheel_speeds(
|
||||
0.0, 2.0, self._SEP, self._MAX_LIN, self._MAX_ANG, self._MAX_SPD)
|
||||
assert fl < 0
|
||||
assert fr > 0
|
||||
assert fl == rl
|
||||
assert fr == rr
|
||||
assert fl == -fr
|
||||
|
||||
|
||||
# ── Ramp / acceleration limiter ──────────────────────────────────────────────
|
||||
|
||||
class TestApplyRamp:
|
||||
_DT = 0.02 # 50 Hz
|
||||
|
||||
def test_accel_step(self):
|
||||
v = _apply_ramp(0.0, 3.0, 2.0, 4.0, self._DT)
|
||||
assert v == pytest.approx(2.0 * self._DT) # 0.04 m/s
|
||||
|
||||
def test_decel_step(self):
|
||||
v = _apply_ramp(3.0, 0.0, 2.0, 4.0, self._DT)
|
||||
assert v == pytest.approx(3.0 - 4.0 * self._DT) # 2.92 m/s
|
||||
|
||||
def test_decel_faster_than_accel(self):
|
||||
"""Decel limit (4.0) > accel limit (2.0) → braking step larger."""
|
||||
accel_step = _apply_ramp(0.0, 1.0, 2.0, 4.0, self._DT) - 0.0
|
||||
decel_step = 3.0 - _apply_ramp(3.0, 0.0, 2.0, 4.0, self._DT)
|
||||
assert decel_step > accel_step
|
||||
|
||||
def test_does_not_overshoot(self):
|
||||
v = _apply_ramp(2.99, 3.0, 2.0, 4.0, self._DT)
|
||||
assert v == pytest.approx(3.0)
|
||||
|
||||
def test_does_not_undershoot(self):
|
||||
v = _apply_ramp(0.01, 0.0, 2.0, 4.0, self._DT)
|
||||
assert v == pytest.approx(0.0)
|
||||
|
||||
def test_negative_to_zero_uses_decel_limit(self):
|
||||
"""From -1.0 toward 0: decelerating → decel_limit applies."""
|
||||
v = _apply_ramp(-1.0, 0.0, 2.0, 4.0, self._DT)
|
||||
assert v == pytest.approx(-1.0 + 4.0 * self._DT) # -0.92
|
||||
|
||||
def test_zero_target(self):
|
||||
assert _apply_ramp(0.0, 0.0, 2.0, 4.0, self._DT) == pytest.approx(0.0)
|
||||
|
||||
def test_time_to_reach_max(self):
|
||||
"""At 2.0 m/s², 3.0 m/s target takes ≈ 3/2/0.02 = 75 ticks."""
|
||||
v = 0.0
|
||||
ticks = 0
|
||||
for _ in range(200):
|
||||
v = _apply_ramp(v, 3.0, 2.0, 4.0, 0.02)
|
||||
ticks += 1
|
||||
if v >= 3.0 - 1e-6:
|
||||
break
|
||||
assert v == pytest.approx(3.0, abs=1e-4)
|
||||
assert 70 <= ticks <= 80
|
||||
|
||||
|
||||
# ── Odometry integration ──────────────────────────────────────────────────────
|
||||
|
||||
class TestIntegratePose:
|
||||
|
||||
def test_straight_along_x(self):
|
||||
"""Moving forward at 1 m/s for 1s → x increases by 1m."""
|
||||
x, y, theta = _integrate_pose(0.0, 0.0, 0.0, 1.0, 0.0, 1.0)
|
||||
assert x == pytest.approx(1.0)
|
||||
assert y == pytest.approx(0.0)
|
||||
assert theta == pytest.approx(0.0)
|
||||
|
||||
def test_straight_along_y(self):
|
||||
"""Facing +Y (theta=pi/2), moving forward 1m → y increases."""
|
||||
x, y, theta = _integrate_pose(0.0, 0.0, math.pi / 2, 1.0, 0.0, 1.0)
|
||||
assert x == pytest.approx(0.0, abs=1e-9)
|
||||
assert y == pytest.approx(1.0)
|
||||
|
||||
def test_pure_rotation(self):
|
||||
"""No linear vel, angular=pi/2 rad/s for 1s → theta = pi/2."""
|
||||
x, y, theta = _integrate_pose(0.0, 0.0, 0.0, 0.0, math.pi / 2, 1.0)
|
||||
assert x == pytest.approx(0.0)
|
||||
assert y == pytest.approx(0.0)
|
||||
assert theta == pytest.approx(math.pi / 2)
|
||||
|
||||
def test_backward(self):
|
||||
"""Reverse (linear=-1) → x decreases."""
|
||||
x, y, theta = _integrate_pose(0.0, 0.0, 0.0, -1.0, 0.0, 1.0)
|
||||
assert x == pytest.approx(-1.0)
|
||||
|
||||
def test_accumulated_over_ticks(self):
|
||||
"""5m at 1m/s with dt=0.02 → 250 ticks."""
|
||||
x, y, theta = 0.0, 0.0, 0.0
|
||||
for _ in range(250):
|
||||
x, y, theta = _integrate_pose(x, y, theta, 1.0, 0.0, 0.02)
|
||||
assert x == pytest.approx(5.0, abs=1e-6)
|
||||
|
||||
def test_small_dt_arc(self):
|
||||
"""Short arc: forward + small turn. Position should shift appropriately."""
|
||||
x, y, theta = _integrate_pose(0.0, 0.0, 0.0, 1.0, 0.1, 0.1)
|
||||
# theta changes by 0.01 rad; x ≈ 0.1, y ≈ 0 (small angle)
|
||||
assert x == pytest.approx(1.0 * 0.1, abs=0.01)
|
||||
assert theta == pytest.approx(0.01)
|
||||
|
||||
|
||||
# ── Quaternion from yaw ───────────────────────────────────────────────────────
|
||||
|
||||
class TestEulerToQuaternionZ:
|
||||
|
||||
def test_zero_yaw_identity(self):
|
||||
qx, qy, qz, qw = _euler_to_quaternion_z(0.0)
|
||||
assert qx == pytest.approx(0.0)
|
||||
assert qy == pytest.approx(0.0)
|
||||
assert qz == pytest.approx(0.0)
|
||||
assert qw == pytest.approx(1.0)
|
||||
|
||||
def test_pi_yaw(self):
|
||||
"""180° rotation: qw ≈ 0, qz ≈ 1."""
|
||||
qx, qy, qz, qw = _euler_to_quaternion_z(math.pi)
|
||||
assert qz == pytest.approx(1.0, abs=1e-6)
|
||||
assert abs(qw) < 1e-6
|
||||
|
||||
def test_half_pi_yaw(self):
|
||||
"""90° rotation: qz = qw = 1/sqrt(2)."""
|
||||
qx, qy, qz, qw = _euler_to_quaternion_z(math.pi / 2)
|
||||
assert qz == pytest.approx(math.sqrt(2) / 2, rel=1e-5)
|
||||
assert qw == pytest.approx(math.sqrt(2) / 2, rel=1e-5)
|
||||
|
||||
def test_unit_quaternion(self):
|
||||
"""All quaternions must be unit length."""
|
||||
for theta in [0, 0.5, 1.0, math.pi / 2, math.pi, -math.pi / 4]:
|
||||
qx, qy, qz, qw = _euler_to_quaternion_z(theta)
|
||||
norm = math.sqrt(qx**2 + qy**2 + qz**2 + qw**2)
|
||||
assert norm == pytest.approx(1.0, rel=1e-6)
|
||||
|
||||
def test_negative_yaw(self):
|
||||
"""Negative yaw → negative qz."""
|
||||
qx, qy, qz, qw = _euler_to_quaternion_z(-math.pi / 2)
|
||||
assert qz < 0
|
||||
|
||||
|
||||
# ── Integration: kinematics + scaling ────────────────────────────────────────
|
||||
|
||||
class TestIntegration:
|
||||
|
||||
def test_full_pipeline_straight(self):
|
||||
"""1 m/s forward → both wheels 333 int (1/3 of max_speed=3)."""
|
||||
fl, fr, rl, rr = _compute_wheel_speeds(
|
||||
1.0, 0.0, 0.45, 3.0, 2.5, 3.0)
|
||||
assert fl == fr == rl == rr == 333
|
||||
|
||||
def test_drive_cmd_round_trip(self):
|
||||
"""Compute speeds → build JSON → parse → verify values."""
|
||||
fl, fr, rl, rr = _compute_wheel_speeds(
|
||||
1.5, 0.5, 0.45, 3.0, 2.5, 3.0)
|
||||
cmd = json.loads(_build_drive4_cmd(fl, fr, rl, rr))
|
||||
assert cmd["fl"] == fl
|
||||
assert cmd["fr"] == fr
|
||||
assert cmd["cmd"] == "drive4"
|
||||
|
||||
def test_ramp_reaches_target(self):
|
||||
"""Starting from rest, ramp reaches commanded speed."""
|
||||
target_vl, _ = _diff_drive_kinematics(2.0, 0.0, 0.45)
|
||||
v = 0.0
|
||||
for _ in range(200):
|
||||
v = _apply_ramp(v, target_vl, 2.0, 4.0, 0.02)
|
||||
assert v == pytest.approx(target_vl, abs=1e-4)
|
||||
|
||||
def test_turn_cmd_produces_different_sides(self):
|
||||
"""Turning command: left and right wheel commands must differ."""
|
||||
fl, fr, rl, rr = _compute_wheel_speeds(
|
||||
1.0, 1.0, 0.45, 3.0, 2.5, 3.0)
|
||||
assert fl != fr # turn → different wheel speeds
|
||||
assert fl == rl # front and rear same per side
|
||||
assert fr == rr
|
||||
Loading…
x
Reference in New Issue
Block a user