Compare commits
4 Commits
main
...
saltyrover
| Author | SHA1 | Date | |
|---|---|---|---|
| 0d4599e12e | |||
| 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