Compare commits
No commits in common. "6ea4b56471107a0086123eb2d5b4256318eff27f" and "914afbc1ca7dda71abaf07b859d253481d9563df" have entirely different histories.
6ea4b56471
...
914afbc1ca
@ -1,339 +1,340 @@
|
||||
// =============================================================================
|
||||
// SaltyBot — Full Chassis Frame (Rev B — correct 270 × 240 mm envelope)
|
||||
// Agent: sl-mechanical | 2026-02-28 | Fixes issue #18
|
||||
// SaltyBot — Parametric Chassis Frame
|
||||
// Task: bd-1iy5
|
||||
// Agent: sl-mechanical
|
||||
// Date: 2026-02-28
|
||||
//
|
||||
// Parametric OpenSCAD model. Plate geometry corrected per issue #18:
|
||||
// Width (axle direction, X): 270 mm ← was wrong 640 mm
|
||||
// Depth (front-to-rear, Y): 240 mm ← was wrong 220 mm
|
||||
// Wheels extend beyond plate edges — expected.
|
||||
//
|
||||
// Motor axle dimensions updated to caliper-verified values (see PR #7 / #11).
|
||||
//
|
||||
// NOTE: For the prototype, use prototype_baseplate.scad + stem_battery_clamp.scad.
|
||||
// This file models the full machined chassis for production reference.
|
||||
// Self-balancing two-wheeled robot chassis
|
||||
// Requirements:
|
||||
// - 600mm wheelbase
|
||||
// - 2x hoverboard hub motors (170mm OD)
|
||||
// - STM32 MAMBA F722S FC mount (30.5x30.5mm pattern)
|
||||
// - Battery tray (24V 4Ah — ~180x70x50mm pack)
|
||||
// - Jetson Nano B01 mount plate (100x80mm, M3 holes)
|
||||
// - Front/rear bumper brackets
|
||||
// =============================================================================
|
||||
|
||||
// ─── RENDER QUALITY ──────────────────────────────────────────────────────────
|
||||
$fn = 64;
|
||||
|
||||
// =============================================================================
|
||||
// PLATE DIMENSIONS ← primary parameters fixed for issue #18
|
||||
// PARAMETERS — edit here to adjust the whole model
|
||||
// =============================================================================
|
||||
|
||||
PLATE_W = 270.0; // mm width (axle / left-right direction)
|
||||
PLATE_D = 240.0; // mm depth (front-to-rear)
|
||||
DECK_THICK = 6.0; // mm deck plate thickness
|
||||
WALL_T = 4.0; // mm general wall / rib thickness
|
||||
// ── Wheelbase / overall geometry ─────────────────────────────────────────────
|
||||
WHEELBASE = 600; // mm, center-to-center axle distance (lateral)
|
||||
FRAME_WIDTH = 220; // mm, front-to-back depth of main deck
|
||||
DECK_THICKNESS = 6; // mm, main deck plate thickness
|
||||
WALL_T = 4; // mm, general wall thickness for brackets/ribs
|
||||
|
||||
// Fork slot geometry — slot opens at plate edge, axle seats at bottom
|
||||
FORK_SLOT_D = 50.0; // mm fork slot depth (inward from plate edge)
|
||||
PLATE_X_HALF = PLATE_W / 2; // 135 mm — plate half-width / edge
|
||||
AXLE_X = PLATE_X_HALF - FORK_SLOT_D; // 85 mm — axle C/L from plate centre
|
||||
// ── Hub motor parameters ─────────────────────────────────────────────────────
|
||||
MOTOR_OD = 170; // mm, motor housing outer diameter
|
||||
MOTOR_AXLE_D = 14; // mm, axle bolt diameter
|
||||
MOTOR_AXLE_FLAT = 10; // mm, axle flat-to-flat (for anti-rotation)
|
||||
MOTOR_FORK_WIDTH = 24; // mm, dropout slot width (fits 10-14mm axle + spacers)
|
||||
MOTOR_FORK_DEPTH = 60; // mm, dropout slot depth from fork tip
|
||||
MOTOR_FORK_H = 80; // mm, total height of motor fork bracket
|
||||
MOTOR_FORK_T = 8; // mm, fork plate thickness
|
||||
AXLE_HEIGHT = 310; // mm, axle CL above ground (motor radius + clearance)
|
||||
|
||||
// ── FC mount (MAMBA F722S — 30.5 × 30.5 mm M3 pattern) ──────────────────────
|
||||
FC_MOUNT_SPACING = 30.5; // mm, hole pattern pitch
|
||||
FC_MOUNT_HOLE_D = 3.2; // mm, M3 clearance
|
||||
FC_STANDOFF_H = 6; // mm, standoff height
|
||||
FC_PAD_T = 3; // mm, mounting pad thickness
|
||||
|
||||
// ── Battery tray (24V 4Ah LiPo / LiFePO4) ───────────────────────────────────
|
||||
BATT_L = 185; // mm, cell pack length
|
||||
BATT_W = 72; // mm, cell pack width
|
||||
BATT_H = 52; // mm, cell pack height
|
||||
BATT_WALL = 3; // mm, tray wall thickness
|
||||
BATT_FLOOR = 4; // mm, tray floor thickness
|
||||
BATT_STRAP_W = 20; // mm, Velcro strap slot width
|
||||
BATT_STRAP_T = 2; // mm, strap slot depth
|
||||
|
||||
// ── Jetson Nano B01 mount plate ──────────────────────────────────────────────
|
||||
// B01 carrier board hole pattern: 58 x 58 mm M3 (inner) + corner pass-throughs
|
||||
JETSON_HOLE_PITCH = 58; // mm, M3 mounting hole pattern
|
||||
JETSON_HOLE_D = 3.2; // mm
|
||||
JETSON_PLATE_L = 105; // mm, plate length
|
||||
JETSON_PLATE_W = 90; // mm, plate width
|
||||
JETSON_PLATE_T = 4; // mm, plate thickness
|
||||
JETSON_STANDOFF_H = 8; // mm
|
||||
|
||||
// ── Bumper bracket ────────────────────────────────────────────────────────────
|
||||
BUMPER_L = WHEELBASE + 60; // mm, bumper rail length (overhangs wheel CL)
|
||||
BUMPER_H = 40; // mm, bracket vertical height
|
||||
BUMPER_T = 5; // mm, bracket plate thickness
|
||||
BUMPER_TUBE_OD = 22; // mm, 3/4" EMT conduit OD for bumper rail
|
||||
|
||||
// ── Rib / gusset parameters ───────────────────────────────────────────────────
|
||||
RIB_W = 20; // mm, longitudinal rib width
|
||||
RIB_H = 40; // mm, rib height below deck
|
||||
RIB_T = 4; // mm, rib plate thickness (laser/router cut)
|
||||
|
||||
// ── Fastener helpers ─────────────────────────────────────────────────────────
|
||||
M3_D = 3.2;
|
||||
M4_D = 4.3;
|
||||
M5_D = 5.3;
|
||||
M6_D = 6.5;
|
||||
|
||||
// =============================================================================
|
||||
// HUB MOTOR — VERIFIED AXLE DIMENSIONS (caliper)
|
||||
// MAIN ASSEMBLY — comment/uncomment parts as needed
|
||||
// =============================================================================
|
||||
|
||||
AXLE_BASE_DIA = 16.11; // mm round section near hub
|
||||
AXLE_BASE_LEN = 15.00;
|
||||
AXLE_DCUT_DIA = 15.95; // mm D-cut round OD
|
||||
AXLE_DCUT_FLAT = 13.00; // mm flat chord
|
||||
AXLE_DCUT_LEN = 43.35;
|
||||
AXLE_TOTAL = 65.50; // mm protrusion from hub face
|
||||
BEARING_SEAT_OD = 37.80; // mm hub centre collar OD
|
||||
TIRE_OD = 254.0; // mm 10 × 2.125" pneumatic
|
||||
TIRE_WIDTH = 54.0; // mm
|
||||
|
||||
AXLE_HEIGHT = TIRE_OD / 2; // 127 mm — axle C/L above ground
|
||||
|
||||
// Fork slot width (base section + clearance)
|
||||
FORK_SLOT_W = AXLE_BASE_DIA + 0.4; // 16.51 mm
|
||||
|
||||
// Vertical motor fork bracket
|
||||
FORK_BKT_H = 100.0; // mm bracket height below deck to axle level
|
||||
FORK_BKT_T = 8.0; // mm bracket plate thickness
|
||||
FORK_BKT_W = 28.0; // mm bracket width (Y)
|
||||
color("Silver", 0.9) main_deck();
|
||||
color("DimGray") motor_fork(side= 1); // right (+X)
|
||||
color("DimGray") motor_fork(side=-1); // left (-X)
|
||||
color("SteelBlue") battery_tray();
|
||||
color("OliveDrab") fc_mount_plate();
|
||||
color("DarkOrange") jetson_mount_plate();
|
||||
color("Tomato") bumper_bracket(front= 1);
|
||||
color("Tomato") bumper_bracket(front=-1);
|
||||
color("WhiteSmoke") longitudinal_ribs();
|
||||
|
||||
// =============================================================================
|
||||
// FC MOUNT — MAMBA F722S 30.5 × 30.5 mm M3
|
||||
// =============================================================================
|
||||
|
||||
FC_PITCH = 30.5;
|
||||
FC_HOLE_D = 3.2;
|
||||
FC_STANDOFF = 6.0;
|
||||
FC_X = -40.0; // mm from plate centre (front of plate)
|
||||
|
||||
// =============================================================================
|
||||
// BATTERY TRAY (legacy flat — superseded by stem carousel architecture)
|
||||
// =============================================================================
|
||||
|
||||
BATT_L = 185.0;
|
||||
BATT_W = 72.0;
|
||||
BATT_H = 52.0;
|
||||
BATT_WALL = 3.0;
|
||||
BATT_FLOOR = 4.0;
|
||||
|
||||
// =============================================================================
|
||||
// JETSON NANO B01 MOUNT
|
||||
// =============================================================================
|
||||
|
||||
JETSON_PITCH = 58.0;
|
||||
JETSON_HOLE_D = 3.2;
|
||||
JETSON_PL = 100.0;
|
||||
JETSON_PW = 86.0;
|
||||
JETSON_PT = 4.0;
|
||||
JETSON_STOFF = 8.0;
|
||||
JETSON_X = 50.0; // mm from plate centre (rear)
|
||||
|
||||
// =============================================================================
|
||||
// BUMPER BRACKETS
|
||||
// =============================================================================
|
||||
|
||||
BUMPER_L = PLATE_D + 60; // 300 mm — spans plate depth + 30 mm each end
|
||||
BUMPER_H = 40.0;
|
||||
BUMPER_T = 5.0;
|
||||
BUMPER_TUBE_OD = 22.0; // 3/4" EMT
|
||||
|
||||
// =============================================================================
|
||||
// LONGITUDINAL RIBS
|
||||
// =============================================================================
|
||||
|
||||
RIB_T = 4.0;
|
||||
RIB_H = 40.0;
|
||||
|
||||
// =============================================================================
|
||||
// FASTENERS
|
||||
// =============================================================================
|
||||
|
||||
M3 = 3.2; M4 = 4.3; M5 = 5.3;
|
||||
|
||||
// =============================================================================
|
||||
// ASSEMBLY
|
||||
// =============================================================================
|
||||
|
||||
color("Silver", 0.90) main_deck();
|
||||
color("DimGray", 1.00) motor_fork_bracket(side= 1);
|
||||
color("DimGray", 1.00) motor_fork_bracket(side=-1);
|
||||
color("SteelBlue", 1.00) battery_tray();
|
||||
color("OliveDrab", 1.00) fc_mount_plate();
|
||||
color("DarkOrange", 1.00) jetson_mount_plate();
|
||||
color("Tomato", 1.00) bumper_bracket(front= 1);
|
||||
color("Tomato", 1.00) bumper_bracket(front=-1);
|
||||
color("WhiteSmoke", 1.00) longitudinal_ribs();
|
||||
|
||||
// =============================================================================
|
||||
// MAIN DECK (270 × 240 mm)
|
||||
// MODULES
|
||||
// =============================================================================
|
||||
|
||||
// ─── Main deck plate ─────────────────────────────────────────────────────────
|
||||
module main_deck() {
|
||||
R = 10;
|
||||
difference() {
|
||||
linear_extrude(DECK_THICK)
|
||||
minkowski() {
|
||||
square([PLATE_W - 2*R, PLATE_D - 2*R], center=true);
|
||||
circle(r=R);
|
||||
}
|
||||
// Deck plate
|
||||
translate([-WHEELBASE/2 - MOTOR_FORK_T, -FRAME_WIDTH/2, 0])
|
||||
cube([WHEELBASE + 2*MOTOR_FORK_T, FRAME_WIDTH, DECK_THICKNESS]);
|
||||
|
||||
// Fork slots (open at ±X plate edges)
|
||||
for (side = [-1, 1]) {
|
||||
translate([side*(PLATE_X_HALF - FORK_SLOT_D), -FORK_SLOT_W/2, -1])
|
||||
cube([FORK_SLOT_D + 1, FORK_SLOT_W, DECK_THICK + 2]);
|
||||
translate([side*(PLATE_X_HALF - FORK_SLOT_D), 0, -1])
|
||||
cylinder(d=FORK_SLOT_W, h=DECK_THICK + 2);
|
||||
}
|
||||
|
||||
// Bearing seat relief
|
||||
for (side = [-1, 1])
|
||||
translate([side*(PLATE_X_HALF - BEARING_SEAT_OD/2 - 1),
|
||||
-BEARING_SEAT_OD/2, -1])
|
||||
cube([BEARING_SEAT_OD/2 + 2, BEARING_SEAT_OD, DECK_THICK + 2]);
|
||||
|
||||
// Fork bracket bolt holes (M5 × 4 per side)
|
||||
for (side = [-1, 1])
|
||||
for (dx = [-15, 15])
|
||||
for (dy = [-12, 12])
|
||||
translate([side * AXLE_X + dx, dy, -1])
|
||||
cylinder(d=M5, h=DECK_THICK + 2);
|
||||
// Lightening holes — 3 rows × 4 cols
|
||||
for (x = [-WHEELBASE/3, 0, WHEELBASE/3])
|
||||
for (y = [-FRAME_WIDTH/4, FRAME_WIDTH/4])
|
||||
translate([x, y, -1])
|
||||
cylinder(d=50, h=DECK_THICKNESS+2);
|
||||
|
||||
// FC mount holes
|
||||
fc_holes(z=-1, h=DECK_THICK+2);
|
||||
fc_mount_holes(z_offset=-1, depth=DECK_THICKNESS+2);
|
||||
|
||||
// Battery tray anchor holes
|
||||
for (x = [-BATT_L/2 + 12, BATT_L/2 - 12])
|
||||
for (y = [-BATT_W/2 + 10, BATT_W/2 - 10])
|
||||
translate([x, y, -1]) cylinder(d=M4, h=DECK_THICK+2);
|
||||
|
||||
// Lightening ovals (front/rear open zones)
|
||||
for (dy = [-80, 80])
|
||||
hull() {
|
||||
translate([ 25, dy, -1]) cylinder(d=22, h=DECK_THICK+2);
|
||||
translate([-25, dy, -1]) cylinder(d=22, h=DECK_THICK+2);
|
||||
}
|
||||
// Battery tray floor cutout for wire pass-through
|
||||
translate([-BATT_L/2 + 40, -BATT_W/2 + 40, -1])
|
||||
cube([BATT_L - 80, BATT_W - 80, DECK_THICKNESS+2]);
|
||||
|
||||
// Cable routing slots
|
||||
for (dy = [-55, 55])
|
||||
hull() {
|
||||
translate([ 12, dy, -1]) cylinder(d=12, h=DECK_THICK+2);
|
||||
translate([-12, dy, -1]) cylinder(d=12, h=DECK_THICK+2);
|
||||
}
|
||||
for (x = [-60, 60])
|
||||
translate([x, -FRAME_WIDTH/2 - 1, -1])
|
||||
cube([14, 18, DECK_THICKNESS+2]);
|
||||
}
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// LONGITUDINAL RIBS (×2)
|
||||
// =============================================================================
|
||||
|
||||
// ─── Longitudinal ribs (×2, symmetric F/R) ───────────────────────────────────
|
||||
module longitudinal_ribs() {
|
||||
rib_y = PLATE_D/2 - WALL_T - RIB_T/2;
|
||||
for (y = [-rib_y, rib_y])
|
||||
translate([-PLATE_W/2, y - RIB_T/2, -RIB_H])
|
||||
cube([PLATE_W, RIB_T, RIB_H]);
|
||||
for (y = [-FRAME_WIDTH/2 + RIB_W/2 + WALL_T,
|
||||
FRAME_WIDTH/2 - RIB_W/2 - WALL_T])
|
||||
translate([-WHEELBASE/2, y - RIB_T/2, -RIB_H])
|
||||
cube([WHEELBASE, RIB_T, RIB_H]);
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// MOTOR FORK BRACKET (vertical, bolts to deck edge at ±PLATE_X_HALF)
|
||||
// =============================================================================
|
||||
// ─── Motor fork dropout bracket ──────────────────────────────────────────────
|
||||
// Mounts to deck edge; provides a CNC-milled or FDM dropout slot for the axle.
|
||||
// `side` = +1 (right/+X) or -1 (left/-X)
|
||||
module motor_fork(side = 1) {
|
||||
x_pos = side * (WHEELBASE/2);
|
||||
|
||||
module motor_fork_bracket(side = 1) {
|
||||
translate([side * PLATE_X_HALF, 0, 0]) {
|
||||
s = side;
|
||||
translate([x_pos, 0, 0]) {
|
||||
difference() {
|
||||
union() {
|
||||
// Main bracket plate (extends below deck)
|
||||
translate([s > 0 ? 0 : -FORK_BKT_T,
|
||||
-FORK_BKT_W/2, -FORK_BKT_H])
|
||||
cube([FORK_BKT_T, FORK_BKT_W, FORK_BKT_H + DECK_THICK]);
|
||||
// Vertical fork body
|
||||
translate([side*(DECK_THICKNESS/2), -MOTOR_FORK_WIDTH/2 - 4, -MOTOR_FORK_H])
|
||||
cube([side * MOTOR_FORK_T, MOTOR_FORK_WIDTH + 8, MOTOR_FORK_H + DECK_THICKNESS]);
|
||||
|
||||
// Diagonal gussets
|
||||
for (gy = [-1, 1])
|
||||
translate([s > 0 ? 0 : -FORK_BKT_T,
|
||||
gy * FORK_BKT_W/2, -FORK_BKT_H/2])
|
||||
linear_extrude(FORK_BKT_T)
|
||||
polygon([[0,0],[0, gy*20],[s * -25, 0]]);
|
||||
// Gusset triangles
|
||||
for (g = [-1, 1])
|
||||
translate([side*(DECK_THICKNESS/2), g*(MOTOR_FORK_WIDTH/2 + 2), -MOTOR_FORK_H])
|
||||
linear_extrude(height=MOTOR_FORK_T)
|
||||
polygon([[0,0],[0, g*20],[side*-30, 0]]);
|
||||
}
|
||||
|
||||
// Axle dropout slot (open at bracket bottom)
|
||||
translate([s > 0 ? -1 : -FORK_BKT_T - 1,
|
||||
-AXLE_BASE_DIA/2 - 0.2, -FORK_BKT_H])
|
||||
cube([FORK_BKT_T + 2, AXLE_BASE_DIA + 0.4, FORK_SLOT_D + 1]);
|
||||
translate([s > 0 ? -1 : -FORK_BKT_T - 1,
|
||||
0, -FORK_BKT_H + FORK_SLOT_D])
|
||||
rotate([-90, 0, 0])
|
||||
cylinder(d=AXLE_BASE_DIA + 0.4,
|
||||
h=FORK_BKT_T + 2, center=true);
|
||||
// Axle dropout slot (open at bottom)
|
||||
translate([side*(DECK_THICKNESS/2) - 1,
|
||||
-MOTOR_AXLE_FLAT/2,
|
||||
-MOTOR_FORK_DEPTH - MOTOR_AXLE_D/2])
|
||||
cube([MOTOR_FORK_T + 2, MOTOR_AXLE_FLAT, MOTOR_FORK_DEPTH + 1]);
|
||||
|
||||
// Deck attachment bolts (M5 × 4)
|
||||
for (dy = [-12, 12])
|
||||
for (dz = [DECK_THICK * 0.3, DECK_THICK * 0.7])
|
||||
translate([s > 0 ? -1 : -FORK_BKT_T - 1, dy, dz])
|
||||
// Axle through-hole at slot top
|
||||
translate([side*(DECK_THICKNESS/2) - 1, 0,
|
||||
-MOTOR_FORK_DEPTH - MOTOR_AXLE_D/2])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d=M5, h=FORK_BKT_T + 2);
|
||||
cylinder(d=MOTOR_AXLE_D + 1, h=MOTOR_FORK_T + 2);
|
||||
|
||||
// Bolt holes for deck attachment (M5 × 4)
|
||||
for (y = [-20, 20])
|
||||
for (z = [4, 12])
|
||||
translate([side*(DECK_THICKNESS/2) - 1, y, z - MOTOR_FORK_H + MOTOR_FORK_H/2])
|
||||
rotate([0, 90, 0])
|
||||
cylinder(d=M5_D, h=MOTOR_FORK_T + 2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// FC MOUNT HELPERS
|
||||
// =============================================================================
|
||||
|
||||
module fc_holes(z=0, h=10) {
|
||||
for (x = [FC_X - FC_PITCH/2, FC_X + FC_PITCH/2])
|
||||
for (y = [-FC_PITCH/2, FC_PITCH/2])
|
||||
translate([x, y, z]) cylinder(d=FC_HOLE_D, h=h);
|
||||
}
|
||||
|
||||
module fc_mount_plate() {
|
||||
translate([FC_X, 0, 0]) {
|
||||
difference() {
|
||||
union() {
|
||||
for (x = [-FC_PITCH/2, FC_PITCH/2])
|
||||
for (y = [-FC_PITCH/2, FC_PITCH/2])
|
||||
translate([x, y, DECK_THICK]) cylinder(d=8, h=FC_STANDOFF);
|
||||
translate([-(FC_PITCH/2+8), -(FC_PITCH/2+8), DECK_THICK])
|
||||
cube([FC_PITCH+16, FC_PITCH+16, 3]);
|
||||
}
|
||||
fc_holes(z=DECK_THICK-1, h=FC_STANDOFF+4);
|
||||
fc_holes(z=-1, h=DECK_THICK+2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// BATTERY TRAY
|
||||
// =============================================================================
|
||||
|
||||
// ─── Battery tray ─────────────────────────────────────────────────────────────
|
||||
// Positioned in centre of deck, recessed 10mm below deck surface
|
||||
module battery_tray() {
|
||||
z_base = -BATT_FLOOR - BATT_H - 5;
|
||||
z_base = -BATT_FLOOR - BATT_H - 5; // hang below deck
|
||||
translate([-BATT_L/2 - BATT_WALL, -BATT_W/2 - BATT_WALL, z_base]) {
|
||||
difference() {
|
||||
cube([BATT_L+2*BATT_WALL, BATT_W+2*BATT_WALL, BATT_H+BATT_FLOOR]);
|
||||
// Outer tray body
|
||||
cube([BATT_L + 2*BATT_WALL,
|
||||
BATT_W + 2*BATT_WALL,
|
||||
BATT_H + BATT_FLOOR]);
|
||||
|
||||
// Inner cavity
|
||||
translate([BATT_WALL, BATT_WALL, BATT_FLOOR])
|
||||
cube([BATT_L, BATT_W, BATT_H+1]);
|
||||
for (x = [BATT_L/2-30, BATT_L/2+8])
|
||||
translate([x, -1, BATT_FLOOR+BATT_H/2-10])
|
||||
cube([20, BATT_W+2*BATT_WALL+2, 20]);
|
||||
for (i = [0:1])
|
||||
translate([BATT_WALL+12+i*68, BATT_WALL+8, -1])
|
||||
cube([38, BATT_W-16, BATT_FLOOR+2]);
|
||||
for (x=[10,BATT_L+BATT_WALL-2]) for (y=[10,BATT_W+BATT_WALL-2])
|
||||
translate([x,y,BATT_H+BATT_FLOOR-1]) cylinder(d=M4,h=BATT_FLOOR+2);
|
||||
cube([BATT_L, BATT_W, BATT_H + 1]);
|
||||
|
||||
// Strap slots (2× longitudinal)
|
||||
for (x = [BATT_L/2 - BATT_STRAP_W*2,
|
||||
BATT_L/2 + BATT_STRAP_W])
|
||||
translate([x, -1, BATT_FLOOR + BATT_H/2 - BATT_STRAP_W/2])
|
||||
cube([BATT_STRAP_W, BATT_W + 2*BATT_WALL + 2, BATT_STRAP_W]);
|
||||
|
||||
// Ventilation slots bottom (3×)
|
||||
for (i = [0:2])
|
||||
translate([BATT_WALL + 20 + i*50, BATT_WALL + 10, -1])
|
||||
cube([30, BATT_W - 20, BATT_FLOOR + 2]);
|
||||
|
||||
// Mount holes to deck (M4 × 4 corners)
|
||||
for (x = [10, BATT_L + BATT_WALL - 2])
|
||||
for (y = [10, BATT_W + BATT_WALL - 2])
|
||||
translate([x, y, BATT_H + BATT_FLOOR - 1])
|
||||
cylinder(d=M4_D, h=BATT_FLOOR + 2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// JETSON NANO B01 MOUNT PLATE
|
||||
// =============================================================================
|
||||
// ─── FC mount holes helper ────────────────────────────────────────────────────
|
||||
module fc_mount_holes(z_offset=0, depth=10) {
|
||||
// MAMBA F722S: 30.5×30.5 mm M3 pattern, centred at origin
|
||||
for (x = [-FC_MOUNT_SPACING/2, FC_MOUNT_SPACING/2])
|
||||
for (y = [-FC_MOUNT_SPACING/2, FC_MOUNT_SPACING/2])
|
||||
translate([x, y, z_offset])
|
||||
cylinder(d=FC_MOUNT_HOLE_D, h=depth);
|
||||
}
|
||||
|
||||
// ─── FC mount plate (raised 40mm above deck centre) ──────────────────────────
|
||||
module fc_mount_plate() {
|
||||
fc_x = -50; // offset toward front from deck centre
|
||||
fc_y = 0;
|
||||
plate_z = DECK_THICKNESS + FC_STANDOFF_H;
|
||||
|
||||
translate([fc_x, fc_y, 0]) {
|
||||
difference() {
|
||||
union() {
|
||||
// Mount pads with standoffs
|
||||
for (x = [-FC_MOUNT_SPACING/2, FC_MOUNT_SPACING/2])
|
||||
for (y = [-FC_MOUNT_SPACING/2, FC_MOUNT_SPACING/2]) {
|
||||
translate([x, y, DECK_THICKNESS])
|
||||
cylinder(d=8, h=FC_STANDOFF_H); // standoff column
|
||||
translate([x - 5, y - 5, DECK_THICKNESS])
|
||||
cube([10, 10, FC_PAD_T]); // pad base
|
||||
}
|
||||
// Base plate
|
||||
translate([-FC_MOUNT_SPACING/2 - 8, -FC_MOUNT_SPACING/2 - 8, DECK_THICKNESS])
|
||||
cube([FC_MOUNT_SPACING + 16, FC_MOUNT_SPACING + 16, FC_PAD_T]);
|
||||
}
|
||||
// M3 through-holes in standoffs
|
||||
fc_mount_holes(z_offset=DECK_THICKNESS - 1, depth=FC_STANDOFF_H + FC_PAD_T + 2);
|
||||
|
||||
// Deck anchor holes
|
||||
fc_mount_holes(z_offset=-1, depth=DECK_THICKNESS + 2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ─── Jetson Nano B01 mount plate ─────────────────────────────────────────────
|
||||
// Positioned rear of deck, elevated on standoffs
|
||||
module jetson_mount_plate() {
|
||||
translate([JETSON_X, 0, 0]) {
|
||||
jet_x = 60; // offset toward rear
|
||||
jet_y = 0;
|
||||
plate_z = DECK_THICKNESS + JETSON_STANDOFF_H;
|
||||
|
||||
translate([jet_x, jet_y, 0]) {
|
||||
difference() {
|
||||
union() {
|
||||
translate([-JETSON_PL/2, -JETSON_PW/2, DECK_THICK+JETSON_STOFF])
|
||||
cube([JETSON_PL, JETSON_PW, JETSON_PT]);
|
||||
for (x=[-JETSON_PITCH/2, JETSON_PITCH/2])
|
||||
for (y=[-JETSON_PITCH/2, JETSON_PITCH/2])
|
||||
translate([x,y,DECK_THICK]) cylinder(d=6, h=JETSON_STOFF);
|
||||
// Mounting plate
|
||||
translate([-JETSON_PLATE_L/2, -JETSON_PLATE_W/2, DECK_THICKNESS + JETSON_STANDOFF_H])
|
||||
cube([JETSON_PLATE_L, JETSON_PLATE_W, JETSON_PLATE_T]);
|
||||
|
||||
// Four standoff columns
|
||||
for (x = [-JETSON_HOLE_PITCH/2, JETSON_HOLE_PITCH/2])
|
||||
for (y = [-JETSON_HOLE_PITCH/2, JETSON_HOLE_PITCH/2])
|
||||
translate([x, y, DECK_THICKNESS])
|
||||
cylinder(d=6, h=JETSON_STANDOFF_H);
|
||||
}
|
||||
for (x=[-JETSON_PITCH/2, JETSON_PITCH/2])
|
||||
for (y=[-JETSON_PITCH/2, JETSON_PITCH/2])
|
||||
translate([x,y,DECK_THICK-1])
|
||||
cylinder(d=JETSON_HOLE_D, h=JETSON_STOFF+JETSON_PT+2);
|
||||
for (i=[-1,0,1])
|
||||
translate([i*22-8,-JETSON_PW/2+8,DECK_THICK+JETSON_STOFF-1])
|
||||
cube([16, JETSON_PW-16, JETSON_PT+2]);
|
||||
for (x=[-JETSON_PL/2+8, JETSON_PL/2-8])
|
||||
for (y=[-JETSON_PW/2+8, JETSON_PW/2-8])
|
||||
translate([x,y,-1]) cylinder(d=M3, h=DECK_THICK+2);
|
||||
|
||||
// B01 M3 hole pattern (58×58 mm)
|
||||
for (x = [-JETSON_HOLE_PITCH/2, JETSON_HOLE_PITCH/2])
|
||||
for (y = [-JETSON_HOLE_PITCH/2, JETSON_HOLE_PITCH/2])
|
||||
translate([x, y, DECK_THICKNESS - 1])
|
||||
cylinder(d=JETSON_HOLE_D, h=JETSON_STANDOFF_H + JETSON_PLATE_T + 2);
|
||||
|
||||
// Ventilation / cable routing slots in plate
|
||||
for (i = [-1, 0, 1])
|
||||
translate([i*25 - 10, -JETSON_PLATE_W/2 + 10,
|
||||
DECK_THICKNESS + JETSON_STANDOFF_H - 1])
|
||||
cube([20, JETSON_PLATE_W - 20, JETSON_PLATE_T + 2]);
|
||||
|
||||
// Deck anchor holes (M3 × 4 corners of plate)
|
||||
for (x = [-JETSON_PLATE_L/2 + 8, JETSON_PLATE_L/2 - 8])
|
||||
for (y = [-JETSON_PLATE_W/2 + 8, JETSON_PLATE_W/2 - 8])
|
||||
translate([x, y, -1])
|
||||
cylinder(d=M3_D, h=DECK_THICKNESS + 2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// BUMPER BRACKETS (front / rear)
|
||||
// =============================================================================
|
||||
|
||||
// ─── Bumper bracket ───────────────────────────────────────────────────────────
|
||||
// `front` = +1 (front) or -1 (rear)
|
||||
module bumper_bracket(front = 1) {
|
||||
yp = front * PLATE_D/2;
|
||||
translate([0, yp, 0]) {
|
||||
y_pos = front * (FRAME_WIDTH/2);
|
||||
|
||||
translate([0, y_pos, 0]) {
|
||||
difference() {
|
||||
union() {
|
||||
// Horizontal mounting rail plate
|
||||
translate([-BUMPER_L/2, front*(BUMPER_T/2), 0])
|
||||
cube([BUMPER_L, BUMPER_T, BUMPER_H]);
|
||||
for (x=[-BUMPER_L/2+4, BUMPER_L/2-BUMPER_T-4])
|
||||
translate([x, front*BUMPER_T, 0]) cube([BUMPER_T, 16, BUMPER_H]);
|
||||
for (x=[-BUMPER_L/3, 0, BUMPER_L/3])
|
||||
translate([x-BUMPER_TUBE_OD/2-2,
|
||||
front*(BUMPER_T+10),
|
||||
BUMPER_H-BUMPER_TUBE_OD/2-4])
|
||||
|
||||
// Vertical gussets at ends
|
||||
for (x = [-BUMPER_L/2 + 5, BUMPER_L/2 - BUMPER_T - 5])
|
||||
translate([x, front*(BUMPER_T), 0])
|
||||
cube([BUMPER_T, 20, BUMPER_H]);
|
||||
|
||||
// Bumper tube saddle clamps (×3 evenly spaced)
|
||||
for (x = [-BUMPER_L/3, 0, BUMPER_L/3])
|
||||
translate([x - BUMPER_TUBE_OD/2 - 2,
|
||||
front*(BUMPER_T + 15),
|
||||
BUMPER_H - BUMPER_TUBE_OD/2 - 5])
|
||||
difference() {
|
||||
cube([BUMPER_TUBE_OD+4, BUMPER_TUBE_OD/2+4, BUMPER_TUBE_OD+4]);
|
||||
translate([BUMPER_TUBE_OD/2+2,-1,BUMPER_TUBE_OD/2+2])
|
||||
rotate([-90,0,0])
|
||||
cylinder(d=BUMPER_TUBE_OD, h=BUMPER_TUBE_OD/2+6);
|
||||
cube([BUMPER_TUBE_OD + 4, BUMPER_TUBE_OD/2 + 4, BUMPER_TUBE_OD + 4]);
|
||||
translate([BUMPER_TUBE_OD/2 + 2, -1, BUMPER_TUBE_OD/2 + 2])
|
||||
rotate([-90, 0, 0])
|
||||
cylinder(d=BUMPER_TUBE_OD, h=BUMPER_TUBE_OD/2 + 6);
|
||||
}
|
||||
}
|
||||
for (x=[-BUMPER_L/2+16, -BUMPER_L/6, BUMPER_L/6, BUMPER_L/2-16])
|
||||
translate([x, yp*0.001, -1]) cylinder(d=M5, h=BUMPER_H+2);
|
||||
|
||||
// Deck mounting holes (M5 × 6 along length)
|
||||
for (x = [-BUMPER_L/2 + 20, -BUMPER_L/6, BUMPER_L/6, BUMPER_L/2 - 20])
|
||||
translate([x, y_pos * 0.001, -1])
|
||||
cylinder(d=M5_D, h=BUMPER_H + 2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// DIMENSIONS / ANNOTATIONS (2D cross-section reference)
|
||||
// =============================================================================
|
||||
// Uncomment to render a side-view dimension guide:
|
||||
//
|
||||
// %translate([0, FRAME_WIDTH/2 + 30, 0]) {
|
||||
// color("Blue") {
|
||||
// cube([WHEELBASE, 1, 1]); // wheelbase span
|
||||
// translate([0,0,0]) text(str("Wheelbase: ", WHEELBASE, "mm"), size=12);
|
||||
// }
|
||||
// }
|
||||
|
||||
@ -1,104 +1,96 @@
|
||||
// =============================================================================
|
||||
// SaltyBot — Prototype Base Plate (Rev D — correct 270 × 240 mm envelope)
|
||||
// Agent: sl-mechanical | 2026-02-28 | Fixes issue #18
|
||||
// SaltyBot — Prototype Base Plate (Rev C — compact, stem-mount)
|
||||
// Agent: sl-mechanical | 2026-02-28
|
||||
//
|
||||
// Laser-cut or CNC-routed flat plate (6 mm Al / 8 mm acrylic).
|
||||
// Uses CALIPER-VERIFIED hub motor axle measurements (see PR #7 / #11).
|
||||
//
|
||||
// PLATE DIMENSIONS (per spec, issue #18)
|
||||
// Width (axle direction, X): 270 mm
|
||||
// Depth (front-to-rear, Y): 240 mm
|
||||
// Wheels extend beyond plate edges — expected.
|
||||
// ARCHITECTURE CHANGE (Rev C):
|
||||
// Batteries are NO LONGER on the base plate.
|
||||
// They stand vertically on a central stem via stem_battery_clamp.scad.
|
||||
// The base plate is now compact — only axle dropouts + stem socket + FC mount.
|
||||
//
|
||||
// MOTOR AXLE GEOMETRY
|
||||
// Plate edge = axle entry point (X = ±135 mm from plate centre)
|
||||
// Fork slot depth = 50 mm inward → axle C/L seated at X = ±85 mm
|
||||
// Axle-to-axle distance = 170 mm
|
||||
// Tires (Ø254 mm, 54 mm wide) at X = ±(85 ± 27) = ±58 – 112 mm ← clear of ±135 edge
|
||||
//
|
||||
// VERIFIED AXLE PROFILE (caliper)
|
||||
// Base section: Ø 16.11 mm, 15 mm long
|
||||
// D-cut zone: Ø 15.95 mm, 13 mm flat chord, 43.35 mm long
|
||||
// Tip: 3 mm
|
||||
// Total: 65.5 mm from hub face
|
||||
// ── AXLE PROFILE (stepped D-cut, caliper-verified) ───────────────────────────
|
||||
// Zone │ Feature │ Ø / Width │ Length
|
||||
// ───────┼──────────────────┼────────────────────┼──────────
|
||||
// Base │ Round (near hub) │ Ø 16.11 mm │ 15.00 mm
|
||||
// D-cut │ Round OD │ Ø 15.95 mm │ 43.35 mm
|
||||
// │ Flat chord │ 13.00 mm │
|
||||
// Tip │ Shoulder/end │ — │ 3.00 mm
|
||||
// Total │ Hub face → tip │ — │ 65.50 mm
|
||||
// Bearing seat collar: Ø 37.8 mm
|
||||
// Tire: 10 × 2.125" pneumatic (Ø 254 mm, 35 PSI)
|
||||
// =============================================================================
|
||||
|
||||
$fn = 128;
|
||||
|
||||
// =============================================================================
|
||||
// PLATE DIMENSIONS ← primary parameters for issue #18 fix
|
||||
// =============================================================================
|
||||
|
||||
PLATE_W = 270.0; // mm width (axle / left-right direction)
|
||||
PLATE_D = 240.0; // mm depth (front-to-rear)
|
||||
PLATE_THICK = 6.0; // mm stock thickness
|
||||
|
||||
// Derived
|
||||
PLATE_X_HALF = PLATE_W / 2; // 135 mm — plate edge
|
||||
FORK_DEPTH = 50.0; // mm fork slot depth (inward from edge)
|
||||
AXLE_X = PLATE_X_HALF - FORK_DEPTH; // 85 mm — axle C/L from plate centre
|
||||
|
||||
// =============================================================================
|
||||
// AXLE PARAMETERS — caliper-verified
|
||||
// =============================================================================
|
||||
|
||||
AXLE_BASE_DIA = 16.11; AXLE_BASE_LEN = 15.00;
|
||||
AXLE_DCUT_DIA = 15.95; AXLE_DCUT_FLAT = 13.00; AXLE_DCUT_LEN = 43.35;
|
||||
AXLE_TIP_LEN = 3.00; AXLE_TOTAL = 65.50;
|
||||
AXLE_BASE_DIA = 16.11;
|
||||
AXLE_BASE_LEN = 15.00;
|
||||
AXLE_DCUT_DIA = 15.95;
|
||||
AXLE_DCUT_FLAT = 13.00;
|
||||
AXLE_DCUT_LEN = 43.35;
|
||||
AXLE_TIP_LEN = 3.00;
|
||||
AXLE_TOTAL = 65.50;
|
||||
BEARING_SEAT_OD = 37.80;
|
||||
TIRE_OD = 254.0; // 10" × 25.4 mm
|
||||
TIRE_OD = 254.0;
|
||||
AXLE_CL_HEIGHT = TIRE_OD / 2; // 127 mm above ground
|
||||
|
||||
// =============================================================================
|
||||
// DROPOUT CLAMP PARAMETERS
|
||||
// PLATE PARAMETERS
|
||||
// =============================================================================
|
||||
|
||||
FORK_W = AXLE_BASE_DIA + 0.4; // 16.51 mm fork slot width
|
||||
WHEELBASE = 600.0; // mm axle C/L to axle C/L
|
||||
// Plate depth now driven only by structural + FC needs (no battery footprint).
|
||||
PLATE_DEPTH = 130.0; // mm front-to-rear
|
||||
PLATE_OVERHANG = 40.0; // mm plate past axle C/L each side
|
||||
PLATE_THICK = 6.0; // mm
|
||||
|
||||
CLAMP_L = 80.0; // mm clamp length along axle axis
|
||||
// outer end at AXLE_X + CLAMP_L/2 = 85+40 = 125 mm
|
||||
// plate edge at 135 mm → 10 mm margin ✓
|
||||
CLAMP_H = 60.0; // mm clamp height (front-to-rear, in Y)
|
||||
CLAMP_THICK = 8.0; // mm each clamp layer
|
||||
CLAMP_BOLT_D = 5.3; // M5 clearance
|
||||
CLAMP_BOLT_DX = 22.0; // mm bolt offset ±X from axle C/L
|
||||
// inner bolt at AXLE_X - 22 = 63 mm ✓
|
||||
// outer bolt at AXLE_X + 22 = 107 mm < 135 mm ✓
|
||||
CLAMP_BOLT_DY = 22.0; // mm bolt offset ±Y from axle C/L
|
||||
CLAMP_ALIGN_D = 4.1; // Ø4 alignment pin clearance
|
||||
// Fork slot
|
||||
FORK_W = AXLE_BASE_DIA + 0.4; // 16.51 mm
|
||||
FORK_DEPTH = 50.0;
|
||||
|
||||
DCUT_CL = 0.3; // mm D-cut bore clearance (all-round)
|
||||
DCUT_R = (AXLE_DCUT_DIA + 2*DCUT_CL) / 2;
|
||||
DCUT_FC = AXLE_DCUT_FLAT + 2*DCUT_CL;
|
||||
// Dropout clamp (two-piece sandwich)
|
||||
CLAMP_L = 80.0;
|
||||
CLAMP_H = 60.0;
|
||||
CLAMP_THICK = 8.0;
|
||||
CLAMP_BOLT_D = 5.3; // M5
|
||||
CLAMP_BOLT_DX = 22.0;
|
||||
CLAMP_BOLT_DY = 22.0;
|
||||
CLAMP_ALIGN_D = 4.1; // Ø4 pin
|
||||
|
||||
// D-cut bore clearance
|
||||
DCUT_CL = 0.3;
|
||||
|
||||
// FC mount — MAMBA F722S 30.5 × 30.5 mm M3
|
||||
FC_PITCH = 30.5;
|
||||
FC_HOLE_D = 3.2;
|
||||
// FC is offset toward front of plate (away from stem)
|
||||
FC_X_OFFSET = -40.0; // mm from plate centre (negative = front/motor side)
|
||||
|
||||
// =============================================================================
|
||||
// STEM SOCKET
|
||||
// STEM SOCKET PARAMETERS
|
||||
// =============================================================================
|
||||
|
||||
STEM_OD = 38.1; // mm 1.5" EMT conduit OD
|
||||
STEM_BORE = STEM_OD + 0.5;
|
||||
STEM_FLANGE_OD = 82.0; // mm flange ring OD
|
||||
STEM_FLANGE_BC = 66.0; // mm M5 bolt circle diameter (r = 33 mm)
|
||||
STEM_FLANGE_T = 6.0; // mm = PLATE_THICK
|
||||
|
||||
// Verify flange bolts clear all other holes:
|
||||
// Flange bolts at (±33, 0) and (0, ±33)
|
||||
// Clamp bolts at (±63, ±22) and (±107, ±22)
|
||||
// FC holes centred at (-40, 0) spanning X = ±55.25, Y = ±15.25
|
||||
// All separations are adequate ✓
|
||||
|
||||
// =============================================================================
|
||||
// FC MOUNT — MAMBA F722S 30.5 × 30.5 mm M3
|
||||
// =============================================================================
|
||||
|
||||
FC_PITCH = 30.5;
|
||||
FC_HOLE_D = 3.2;
|
||||
FC_X_OFFSET = -40.0; // mm front of plate (−X); FC at (−40, 0) within ±135 ✓
|
||||
STEM_BORE = STEM_OD + 0.5; // 38.6 mm with clearance
|
||||
// Flange ring (laser-cut, bolts above + below plate to grip tube):
|
||||
STEM_FLANGE_OD = 82.0; // mm flange outer diameter
|
||||
STEM_FLANGE_BC = 66.0; // mm bolt circle diameter (4× M5 at 90°)
|
||||
STEM_FLANGE_T = 6.0; // mm = PLATE_THICK (flush-mount)
|
||||
// Stem position: at plate centre (X=0, Y=0)
|
||||
|
||||
// =============================================================================
|
||||
// UTILITIES
|
||||
// =============================================================================
|
||||
|
||||
M3 = 3.2; M4 = 4.3; M5 = 5.3;
|
||||
PLATE_X_HALF = WHEELBASE/2 + PLATE_OVERHANG; // ± 340 mm
|
||||
DCUT_R = (AXLE_DCUT_DIA + 2*DCUT_CL) / 2;
|
||||
DCUT_FC = AXLE_DCUT_FLAT + 2*DCUT_CL;
|
||||
|
||||
// =============================================================================
|
||||
// RENDER CONTROL
|
||||
@ -108,15 +100,21 @@ M3 = 3.2; M4 = 4.3; M5 = 5.3;
|
||||
// "plate_2d" DXF — base plate
|
||||
// "clamp_lower_2d" DXF — lower dropout clamp (× 2)
|
||||
// "clamp_upper_2d" DXF — upper dropout clamp (× 2)
|
||||
// "stem_flange_2d" DXF — stem flange ring (× 2)
|
||||
// "stem_flange_2d" DXF — stem flange ring (× 2, one above + one below plate)
|
||||
|
||||
RENDER = "assembly";
|
||||
|
||||
if (RENDER == "assembly") assembly();
|
||||
else if (RENDER == "plate_2d") projection(cut=true) translate([0,0,-PLATE_THICK/2]) base_plate();
|
||||
else if (RENDER == "clamp_lower_2d") projection(cut=true) translate([0,0,-CLAMP_THICK/2]) clamp_lower();
|
||||
else if (RENDER == "clamp_upper_2d") projection(cut=true) translate([0,0,-CLAMP_THICK/2]) clamp_upper();
|
||||
else if (RENDER == "stem_flange_2d") projection(cut=true) translate([0,0,-STEM_FLANGE_T/2]) stem_flange();
|
||||
if (RENDER == "assembly") {
|
||||
assembly();
|
||||
} else if (RENDER == "plate_2d") {
|
||||
projection(cut=true) translate([0,0,-PLATE_THICK/2]) base_plate();
|
||||
} else if (RENDER == "clamp_lower_2d") {
|
||||
projection(cut=true) translate([0,0,-CLAMP_THICK/2]) clamp_lower();
|
||||
} else if (RENDER == "clamp_upper_2d") {
|
||||
projection(cut=true) translate([0,0,-CLAMP_THICK/2]) clamp_upper();
|
||||
} else if (RENDER == "stem_flange_2d") {
|
||||
projection(cut=true) translate([0,0,-STEM_FLANGE_T/2]) stem_flange();
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// ASSEMBLY
|
||||
@ -127,115 +125,119 @@ module assembly() {
|
||||
|
||||
for (side = [-1, 1]) {
|
||||
color("SteelBlue", 0.80)
|
||||
translate([side * AXLE_X, 0, PLATE_THICK])
|
||||
translate([side * WHEELBASE/2, 0, PLATE_THICK])
|
||||
clamp_lower();
|
||||
color("CornflowerBlue", 0.80)
|
||||
translate([side * AXLE_X, 0, PLATE_THICK + CLAMP_THICK])
|
||||
translate([side * WHEELBASE/2, 0, PLATE_THICK + CLAMP_THICK])
|
||||
clamp_upper();
|
||||
}
|
||||
|
||||
// Stem flanges (above and below plate)
|
||||
color("DimGray", 0.70) translate([0, 0, -STEM_FLANGE_T]) stem_flange();
|
||||
color("DimGray", 0.70) translate([0, 0, PLATE_THICK]) stem_flange();
|
||||
// Stem flange rings (above and below plate)
|
||||
color("DimGray", 0.70)
|
||||
translate([0, 0, -STEM_FLANGE_T])
|
||||
stem_flange();
|
||||
color("DimGray", 0.70)
|
||||
translate([0, 0, PLATE_THICK])
|
||||
stem_flange();
|
||||
|
||||
// Ghosts — not exported
|
||||
// Reference ghosts
|
||||
%color("Orange", 0.25)
|
||||
translate([0, 0, PLATE_THICK + STEM_FLANGE_T])
|
||||
cylinder(d=STEM_OD, h=900);
|
||||
|
||||
cylinder(d=STEM_OD, h=800); // vertical stem
|
||||
%for (side = [-1, 1])
|
||||
color("Tomato", 0.20)
|
||||
translate([side * PLATE_X_HALF, 0, 0])
|
||||
rotate([0, side * (-90), 0]) // axle projects inward from plate edge
|
||||
color("Tomato", 0.2)
|
||||
translate([side * WHEELBASE/2, 0, 0])
|
||||
rotate([0, side*90, 0])
|
||||
axle_ghost();
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// BASE PLATE (Part A — 270 × 240 mm)
|
||||
// BASE PLATE (Part A — compact)
|
||||
// =============================================================================
|
||||
|
||||
module base_plate() {
|
||||
R = 10; // corner radius
|
||||
R = 12; // corner radius
|
||||
difference() {
|
||||
// ── Outer profile (minkowski rounded corners) ──────────────────────
|
||||
// ── Outer profile ──
|
||||
linear_extrude(PLATE_THICK)
|
||||
minkowski() {
|
||||
square([PLATE_W - 2*R, PLATE_D - 2*R], center=true);
|
||||
square([2*(PLATE_X_HALF - R), PLATE_DEPTH - 2*R], center=true);
|
||||
circle(r=R);
|
||||
}
|
||||
|
||||
// ── Fork slots (open at ±X plate edges, semicircular tip) ──────────
|
||||
// Slot width FORK_W, depth FORK_DEPTH inward from plate edge.
|
||||
// Axle seats in the semicircle at X = ±(PLATE_X_HALF − FORK_DEPTH) = ±85 mm.
|
||||
// ── Fork slots (open at ±X edges, semicircular tip) ──────────────
|
||||
for (side = [-1, 1]) {
|
||||
// Rectangular slot body
|
||||
translate([side * (PLATE_X_HALF - FORK_DEPTH), -FORK_W/2, -1])
|
||||
cube([side > 0 ? FORK_DEPTH + 1 : FORK_DEPTH + 1,
|
||||
FORK_W, PLATE_THICK + 2]);
|
||||
// Semicircular slot tip (axle seat)
|
||||
translate([side * (PLATE_X_HALF - FORK_DEPTH), 0, -1])
|
||||
translate([side*(PLATE_X_HALF - FORK_DEPTH), -FORK_W/2, -1])
|
||||
cube([FORK_DEPTH + 1, FORK_W, PLATE_THICK + 2]);
|
||||
translate([side*(PLATE_X_HALF - FORK_DEPTH), 0, -1])
|
||||
cylinder(d=FORK_W, h=PLATE_THICK + 2);
|
||||
}
|
||||
|
||||
// ── Bearing seat relief (prevents Ø37.8 mm hub collar contacting edge) ─
|
||||
// ── Bearing seat relief (prevents Ø37.8 mm collar binding on edge) ─
|
||||
for (side = [-1, 1])
|
||||
translate([side * (PLATE_X_HALF - BEARING_SEAT_OD/2 - 1),
|
||||
translate([side*PLATE_X_HALF - side*(BEARING_SEAT_OD/2 + 1),
|
||||
-BEARING_SEAT_OD/2, -1])
|
||||
cube([BEARING_SEAT_OD/2 + 2, BEARING_SEAT_OD, PLATE_THICK + 2]);
|
||||
|
||||
// ── Dropout clamp bolt through-holes ───────────────────────────────
|
||||
// ── Dropout clamp bolt through-holes ─────────────────────────────
|
||||
for (side = [-1, 1])
|
||||
for (dx = [-CLAMP_BOLT_DX, CLAMP_BOLT_DX])
|
||||
for (dy = [-CLAMP_BOLT_DY, CLAMP_BOLT_DY])
|
||||
translate([side * AXLE_X + dx, dy, -1])
|
||||
translate([side*WHEELBASE/2 + dx, dy, -1])
|
||||
cylinder(d=CLAMP_BOLT_D, h=PLATE_THICK + 2);
|
||||
|
||||
// ── Alignment pin holes (Ø4 × 2 per clamp) ─────────────────────────
|
||||
// ── Alignment pin holes (Ø4) ──────────────────────────────────────
|
||||
for (side = [-1, 1])
|
||||
for (dy = [-CLAMP_BOLT_DY + 8, CLAMP_BOLT_DY - 8])
|
||||
translate([side * AXLE_X, dy, -1])
|
||||
translate([side*WHEELBASE/2, dy, -1])
|
||||
cylinder(d=CLAMP_ALIGN_D, h=PLATE_THICK + 2);
|
||||
|
||||
// ── Stem socket bore ─────────────────────────────────────────────────
|
||||
translate([0, 0, -1]) cylinder(d=STEM_BORE, h=PLATE_THICK + 2);
|
||||
// ── Stem socket bore ──────────────────────────────────────────────
|
||||
translate([0, 0, -1])
|
||||
cylinder(d=STEM_BORE, h=PLATE_THICK + 2);
|
||||
|
||||
// ── Stem flange bolt holes (4× M5 at 90° on Ø66 BC) ────────────────
|
||||
// ── Stem flange bolt holes (4× M5, 90° pattern on STEM_FLANGE_BC) ─
|
||||
for (a = [0, 90, 180, 270])
|
||||
rotate([0, 0, a])
|
||||
translate([STEM_FLANGE_BC/2, 0, -1])
|
||||
cylinder(d=M5, h=PLATE_THICK + 2);
|
||||
|
||||
// ── FC mount holes (MAMBA F722S 30.5 × 30.5 M3) ────────────────────
|
||||
// ── FC mount (MAMBA F722S 30.5 × 30.5 M3) ────────────────────────
|
||||
for (x = [FC_X_OFFSET - FC_PITCH/2, FC_X_OFFSET + FC_PITCH/2])
|
||||
for (y = [-FC_PITCH/2, FC_PITCH/2])
|
||||
translate([x, y, -1]) cylinder(d=FC_HOLE_D, h=PLATE_THICK + 2);
|
||||
translate([x, y, -1])
|
||||
cylinder(d=FC_HOLE_D, h=PLATE_THICK + 2);
|
||||
|
||||
// ── Wiring / cable pass-through slots (flanking stem) ───────────────
|
||||
// Placed at Y = ±55 mm (clear of stem flange bolts at ±33 mm and clamp at ±22 mm)
|
||||
for (dy = [-55, 55])
|
||||
// ── Wiring / cable pass-through slots (2×, flanking stem) ─────────
|
||||
for (dy = [-30, 30])
|
||||
hull() {
|
||||
translate([ 12, dy, -1]) cylinder(d=12, h=PLATE_THICK + 2);
|
||||
translate([-12, dy, -1]) cylinder(d=12, h=PLATE_THICK + 2);
|
||||
translate([15, dy, -1]) cylinder(d=14, h=PLATE_THICK + 2);
|
||||
translate([-15, dy, -1]) cylinder(d=14, h=PLATE_THICK + 2);
|
||||
}
|
||||
|
||||
// ── Lightening holes (2× in open areas between stem and clamp zones) ─
|
||||
// At X = 0, Y = ±80 mm (front/rear open corridor)
|
||||
for (dy = [-80, 80])
|
||||
// ── Lightening slots (between FC zone and dropout zones) ──────────
|
||||
for (sx = [-1, 1]) {
|
||||
// One slot each side of stem, in the structural corridor
|
||||
lx = sx * (WHEELBASE/4);
|
||||
hull() {
|
||||
translate([ 20, dy, -1]) cylinder(d=20, h=PLATE_THICK + 2);
|
||||
translate([-20, dy, -1]) cylinder(d=20, h=PLATE_THICK + 2);
|
||||
translate([lx, -(PLATE_DEPTH/2 - 22), -1]) cylinder(d=18, h=PLATE_THICK+2);
|
||||
translate([lx, (PLATE_DEPTH/2 - 22), -1]) cylinder(d=18, h=PLATE_THICK+2);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// STEM FLANGE RING (laser-cut, qty 2)
|
||||
// STEM FLANGE RING (laser-cut, qty 2 — one above, one below plate)
|
||||
// =============================================================================
|
||||
|
||||
module stem_flange() {
|
||||
difference() {
|
||||
cylinder(d=STEM_FLANGE_OD, h=STEM_FLANGE_T);
|
||||
translate([0, 0, -1]) cylinder(d=STEM_BORE, h=STEM_FLANGE_T + 2);
|
||||
// Stem bore (tight fit — tube presses into flange)
|
||||
translate([0, 0, -1])
|
||||
cylinder(d=STEM_BORE, h=STEM_FLANGE_T + 2);
|
||||
// 4× M5 flange bolts
|
||||
for (a = [0, 90, 180, 270])
|
||||
rotate([0, 0, a])
|
||||
translate([STEM_FLANGE_BC/2, 0, -1])
|
||||
@ -244,7 +246,7 @@ module stem_flange() {
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// DROPOUT CLAMP — LOWER (round bore, base section)
|
||||
// DROPOUT CLAMP — LOWER (round bore, base-section diameter)
|
||||
// =============================================================================
|
||||
|
||||
module clamp_lower() {
|
||||
@ -253,13 +255,16 @@ module clamp_lower() {
|
||||
translate([-CLAMP_L/2 + CLAMP_H/2, 0, 0]) cylinder(d=CLAMP_H, h=CLAMP_THICK);
|
||||
translate([ CLAMP_L/2 - CLAMP_H/2, 0, 0]) cylinder(d=CLAMP_H, h=CLAMP_THICK);
|
||||
}
|
||||
translate([0,0,-1]) cylinder(d=AXLE_BASE_DIA + 0.4, h=CLAMP_THICK + 2);
|
||||
// Open entry slot (axle slides in from plate edge)
|
||||
// Round bore (base zone)
|
||||
translate([0, 0, -1]) cylinder(d=AXLE_BASE_DIA + 0.4, h=CLAMP_THICK + 2);
|
||||
// Slide-in slot (open toward wheel side)
|
||||
translate([-CLAMP_L/2 - 1, -FORK_W/2, -1])
|
||||
cube([CLAMP_L/2 + 1, FORK_W, CLAMP_THICK + 2]);
|
||||
// Clamp bolts
|
||||
for (dx = [-CLAMP_BOLT_DX, CLAMP_BOLT_DX])
|
||||
for (dy = [-CLAMP_BOLT_DY, CLAMP_BOLT_DY])
|
||||
translate([dx, dy, -1]) cylinder(d=CLAMP_BOLT_D, h=CLAMP_THICK + 2);
|
||||
// Alignment pins
|
||||
for (dy = [-CLAMP_BOLT_DY + 8, CLAMP_BOLT_DY - 8])
|
||||
translate([0, dy, -1]) cylinder(d=CLAMP_ALIGN_D, h=CLAMP_THICK + 2);
|
||||
}
|
||||
@ -270,31 +275,41 @@ module clamp_lower() {
|
||||
// =============================================================================
|
||||
|
||||
module clamp_upper() {
|
||||
dcut_d = sqrt(pow(DCUT_R, 2) - pow(DCUT_FC/2, 2));
|
||||
dcut_r = DCUT_R;
|
||||
dcut_d = sqrt(pow(dcut_r, 2) - pow(DCUT_FC/2, 2));
|
||||
|
||||
difference() {
|
||||
hull() {
|
||||
translate([-CLAMP_L/2 + CLAMP_H/2, 0, 0]) cylinder(d=CLAMP_H, h=CLAMP_THICK);
|
||||
translate([ CLAMP_L/2 - CLAMP_H/2, 0, 0]) cylinder(d=CLAMP_H, h=CLAMP_THICK);
|
||||
}
|
||||
translate([0,0,-1])
|
||||
linear_extrude(CLAMP_THICK + 2) dcut_profile_2d(DCUT_R, dcut_d);
|
||||
// D-cut bore
|
||||
translate([0, 0, -1])
|
||||
linear_extrude(CLAMP_THICK + 2)
|
||||
dcut_profile_2d(dcut_r, dcut_d);
|
||||
// Slide-in slot
|
||||
translate([-CLAMP_L/2 - 1, -FORK_W/2, -1])
|
||||
cube([CLAMP_L/2 + 1, FORK_W, CLAMP_THICK + 2]);
|
||||
// Clamp bolts
|
||||
for (dx = [-CLAMP_BOLT_DX, CLAMP_BOLT_DX])
|
||||
for (dy = [-CLAMP_BOLT_DY, CLAMP_BOLT_DY])
|
||||
translate([dx, dy, -1]) cylinder(d=CLAMP_BOLT_D, h=CLAMP_THICK + 2);
|
||||
// Alignment pins
|
||||
for (dy = [-CLAMP_BOLT_DY + 8, CLAMP_BOLT_DY - 8])
|
||||
translate([0, dy, -1]) cylinder(d=CLAMP_ALIGN_D, h=CLAMP_THICK + 2);
|
||||
// Orientation marker (triangle pointing toward flat)
|
||||
// Orientation emboss
|
||||
translate([0, dcut_d + 1.5, CLAMP_THICK - 0.8])
|
||||
linear_extrude(1) polygon([[0,0],[-3,-5],[3,-5]]);
|
||||
linear_extrude(1)
|
||||
polygon([[0,0],[-3,-5],[3,-5]]);
|
||||
}
|
||||
}
|
||||
|
||||
// ── D-cut 2D profile helper ───────────────────────────────────────────────────
|
||||
module dcut_profile_2d(r, flat_d) {
|
||||
intersection() {
|
||||
circle(r=r);
|
||||
translate([-r-1, -r-1]) square([2*(r+1), r+1+flat_d]);
|
||||
translate([-r - 1, -r - 1])
|
||||
square([2*(r+1), r + 1 + flat_d]);
|
||||
}
|
||||
}
|
||||
|
||||
@ -309,22 +324,25 @@ module axle_ghost() {
|
||||
}
|
||||
|
||||
// =============================================================================
|
||||
// DXF EXPORT COMMANDS
|
||||
// DXF EXPORT
|
||||
// =============================================================================
|
||||
//
|
||||
// Base plate (270 × 240 mm, 6 mm Al or 8 mm acrylic):
|
||||
// Part 1 — Base plate:
|
||||
// openscad prototype_baseplate.scad -D 'RENDER="plate_2d"' -o baseplate.dxf
|
||||
//
|
||||
// Dropout clamp lower (× 2):
|
||||
// Part 2 — Dropout clamp, lower (× 2):
|
||||
// openscad prototype_baseplate.scad -D 'RENDER="clamp_lower_2d"' -o clamp_lower.dxf
|
||||
//
|
||||
// Dropout clamp upper (× 2):
|
||||
// Part 3 — Dropout clamp, upper (× 2):
|
||||
// openscad prototype_baseplate.scad -D 'RENDER="clamp_upper_2d"' -o clamp_upper.dxf
|
||||
//
|
||||
// Stem flange ring (× 2):
|
||||
// Part 4 — Stem flange ring (× 2, one each side of plate):
|
||||
// openscad prototype_baseplate.scad -D 'RENDER="stem_flange_2d"' -o stem_flange.dxf
|
||||
//
|
||||
// PLATE BLANK: 280 × 250 mm (10 mm oversized each side for datum + clamping)
|
||||
// CLAMP BLANKS: 90 × 70 mm each
|
||||
// FLANGE BLANKS: Ø90 mm disc each
|
||||
// Materials:
|
||||
// Plate + flanges : 6 mm 5052-H32 aluminium (preferred)
|
||||
// 8 mm clear acrylic (quick proto)
|
||||
// Dropout clamps : 8 mm 6061-T6 aluminium
|
||||
// Stem tube : 38.1 mm OD × 1.5 mm wall 6061-T6 (or 1.5" EMT)
|
||||
// Cut stem to ~1050 mm — allows batteries from ~100 mm to ~950 mm height.
|
||||
// =============================================================================
|
||||
|
||||
@ -140,8 +140,4 @@
|
||||
#define MAX_SPEED_LIMIT 100
|
||||
#define WATCHDOG_TIMEOUT_MS 50
|
||||
|
||||
// --- Motor Driver ---
|
||||
#define MOTOR_CMD_MAX 1000 /* ESC range: -1000..+1000 */
|
||||
#define MOTOR_STEER_RAMP_RATE 20 /* counts/ms — steer ramp only */
|
||||
|
||||
#endif // CONFIG_H
|
||||
|
||||
@ -1,47 +0,0 @@
|
||||
#ifndef MOTOR_DRIVER_H
|
||||
#define MOTOR_DRIVER_H
|
||||
|
||||
#include <stdint.h>
|
||||
#include <stdbool.h>
|
||||
|
||||
/*
|
||||
* SaltyLab Motor Driver
|
||||
*
|
||||
* Sits between the balance PID and the raw hoverboard UART driver.
|
||||
* Responsibilities:
|
||||
* - Differential drive mixing: speed = balance_cmd, steer mixed in
|
||||
* - Steer ramping to avoid sudden yaw torque disturbing balance
|
||||
* - Headroom clamping: |speed| + |steer| <= MOTOR_CMD_MAX
|
||||
* - Emergency stop: immediate zero + latch until explicitly cleared
|
||||
*
|
||||
* Balance PID output is NOT ramped — it needs full immediate authority.
|
||||
* Only the steer channel is ramped.
|
||||
*
|
||||
* Call motor_driver_update() at the ESC send rate (50Hz / every 20ms).
|
||||
*/
|
||||
|
||||
typedef struct {
|
||||
int16_t steer_actual; /* Ramped steer command currently sent */
|
||||
bool estop; /* Emergency stop latched */
|
||||
} motor_driver_t;
|
||||
|
||||
void motor_driver_init(motor_driver_t *m);
|
||||
|
||||
/*
|
||||
* Update and send to ESC.
|
||||
* balance_cmd : PID output, -1000..+1000
|
||||
* steer_cmd : desired yaw/steer, -1000..+1000 (future RC/autonomous input)
|
||||
* now : HAL_GetTick() timestamp (ms) for ramp delta-time
|
||||
*/
|
||||
void motor_driver_update(motor_driver_t *m,
|
||||
int16_t balance_cmd,
|
||||
int16_t steer_cmd,
|
||||
uint32_t now);
|
||||
|
||||
/* Latch emergency stop — sends zero immediately */
|
||||
void motor_driver_estop(motor_driver_t *m);
|
||||
|
||||
/* Clear emergency stop latch (only call from armed/ready context) */
|
||||
void motor_driver_estop_clear(motor_driver_t *m);
|
||||
|
||||
#endif
|
||||
@ -5,10 +5,8 @@
|
||||
#include <stdbool.h>
|
||||
|
||||
typedef struct {
|
||||
float pitch; // degrees, filtered (complementary filter)
|
||||
float pitch_rate; // degrees/sec (raw gyro pitch axis)
|
||||
float roll; // degrees, filtered (complementary filter)
|
||||
float yaw; // degrees, gyro-integrated (drifts — no magnetometer)
|
||||
float pitch; // degrees, filtered
|
||||
float pitch_rate; // degrees/sec (raw gyro)
|
||||
float accel_x; // g
|
||||
float accel_z; // g
|
||||
} IMUData;
|
||||
|
||||
@ -1,53 +0,0 @@
|
||||
# RealSense D435i configuration — Jetson Nano (power-budget tuned)
|
||||
#
|
||||
# Profile format: WxHxFPS
|
||||
# Constraint: ~10W total budget. 640x480x15 saves ~0.4W vs 30fps.
|
||||
#
|
||||
# Reference topics at these settings:
|
||||
# /camera/color/image_raw 640x480 15 Hz RGB8
|
||||
# /camera/depth/image_rect_raw 640x480 15 Hz Z16
|
||||
# /camera/aligned_depth_to_color/image_raw 640x480 15 Hz Z16
|
||||
# /camera/imu 6-axis ~200 Hz (accel@100Hz + gyro@400Hz fused)
|
||||
# /camera/color/camera_info 640x480 15 Hz
|
||||
# /camera/depth/camera_info 640x480 15 Hz
|
||||
#
|
||||
# Hardware IMU: Bosch BMI055
|
||||
# Accelerometer native rate: 100 Hz
|
||||
# Gyroscope native rate: 400 Hz
|
||||
# unite_imu_method=2: linearly interpolates accel to match gyro timestamps
|
||||
|
||||
camera:
|
||||
ros__parameters:
|
||||
# ── Streams ──────────────────────────────────────────────────────────────
|
||||
depth_module.profile: "640x480x15"
|
||||
rgb_camera.profile: "640x480x15"
|
||||
|
||||
enable_depth: true
|
||||
enable_color: true
|
||||
enable_infra1: false # not needed for RGB-D SLAM
|
||||
enable_infra2: false
|
||||
|
||||
# ── IMU ──────────────────────────────────────────────────────────────────
|
||||
enable_gyro: true
|
||||
enable_accel: true
|
||||
# 0=none 1=copy 2=linear_interpolation
|
||||
# Use 2: aligns accel timestamps to gyro rate, required for sensor fusion
|
||||
unite_imu_method: 2
|
||||
|
||||
# ── Alignment ────────────────────────────────────────────────────────────
|
||||
# Projects depth pixels into RGB frame — required for rtabmap_ros rgbd input
|
||||
align_depth.enable: true
|
||||
|
||||
# ── Point cloud ──────────────────────────────────────────────────────────
|
||||
# Disabled: rtabmap_ros generates its own from aligned depth.
|
||||
# Maxwell GPU cannot handle both simultaneously at budget.
|
||||
pointcloud.enable: false
|
||||
|
||||
# ── TF ───────────────────────────────────────────────────────────────────
|
||||
publish_tf: true
|
||||
tf_publish_rate: 0.0 # 0 = publish static transforms only (no redundant timer)
|
||||
|
||||
# ── Device ───────────────────────────────────────────────────────────────
|
||||
# Leave serial_no empty to auto-select first found device
|
||||
# serial_no: ''
|
||||
# device_type: d435i
|
||||
@ -1,30 +0,0 @@
|
||||
# RPLIDAR A1M8 configuration
|
||||
#
|
||||
# Hardware specs:
|
||||
# Model: RPLIDAR A1M8 (rev 6)
|
||||
# Interface: USB-UART via CP2102 adapter (VID:10c4 PID:ea60)
|
||||
# Baud rate: 115200
|
||||
# Scan rate: ~5.5 Hz (1440 points/scan @ 8000 samples/s)
|
||||
# Range: 0.15 m – 12 m (reliable to ~8 m indoors)
|
||||
# Angular res: ~0.9° per sample
|
||||
# Scan mode: Standard (A1M8 only supports this mode)
|
||||
#
|
||||
# udev symlink (from jetson/docs/pinout.md 99-saltybot.rules):
|
||||
# /dev/rplidar → ttyUSB* where ATTRS{idVendor}=="10c4" ATTRS{idProduct}=="ea60"
|
||||
#
|
||||
# Published topics:
|
||||
# /scan sensor_msgs/LaserScan ~5.5 Hz
|
||||
# angle_min: -π rad (-180°)
|
||||
# angle_max: +π rad (+180°)
|
||||
# range_min: 0.15 m
|
||||
# range_max: 12.0 m
|
||||
# scan_time: ~0.182 s per revolution
|
||||
|
||||
rplidar_node:
|
||||
ros__parameters:
|
||||
serial_port: "/dev/rplidar" # udev symlink — stable across reboots
|
||||
serial_baudrate: 115200
|
||||
frame_id: "laser" # must match TF in sensors.launch.py
|
||||
inverted: false # scan direction: false = counter-clockwise
|
||||
angle_compensate: true # compensate for motor rotation offset
|
||||
scan_mode: "Standard" # A1M8 only supports Standard mode
|
||||
@ -1,79 +0,0 @@
|
||||
# slam_toolbox — online async SLAM configuration
|
||||
# Tuned for Jetson Nano 4GB (constrained CPU/RAM, indoor mapping)
|
||||
#
|
||||
# Input: /scan (LaserScan from RPLIDAR A1M8, ~5.5 Hz)
|
||||
# Output: /map (OccupancyGrid, updated every map_update_interval seconds)
|
||||
#
|
||||
# Frame assumptions (must match sensors.launch.py static TF):
|
||||
# map → odom → base_link → laser
|
||||
# (odom not yet published — slam_toolbox handles this via scan matching)
|
||||
|
||||
slam_toolbox:
|
||||
ros__parameters:
|
||||
# ── Frames ───────────────────────────────────────────────────────────────
|
||||
odom_frame: odom
|
||||
map_frame: map
|
||||
base_frame: base_link
|
||||
scan_topic: /scan
|
||||
mode: mapping # 'mapping' or 'localization'
|
||||
|
||||
# ── Map params ───────────────────────────────────────────────────────────
|
||||
resolution: 0.05 # 5 cm/cell — good balance for A1M8 angular res
|
||||
max_laser_range: 8.0 # clip to reliable range of A1M8 (spec: 12m)
|
||||
map_update_interval: 5.0 # seconds between full map publishes (saves CPU)
|
||||
minimum_travel_distance: 0.3 # only update after moving 30 cm
|
||||
minimum_travel_heading: 0.3 # or rotating ~17°
|
||||
|
||||
# ── Performance (Nano-specific) ───────────────────────────────────────────
|
||||
# Reduce scan processing rate to stay within ~3.5W CPU budget
|
||||
minimum_time_interval: 0.5 # max 2 Hz scan processing (A1M8 is ~5.5 Hz)
|
||||
transform_timeout: 0.2
|
||||
tf_buffer_duration: 30.0
|
||||
stack_size_to_use: 40000000 # 40 MB stack
|
||||
enable_interactive_mode: false # disable interactive editing (saves CPU)
|
||||
|
||||
# ── Scan matching ─────────────────────────────────────────────────────────
|
||||
use_scan_matching: true
|
||||
use_scan_barycenter: true
|
||||
scan_buffer_size: 10
|
||||
scan_buffer_maximum_scan_distance: 10.0
|
||||
|
||||
# ── Loop closure ──────────────────────────────────────────────────────────
|
||||
do_loop_closing: true
|
||||
loop_match_minimum_chain_size: 10
|
||||
loop_match_maximum_variance_coarse: 3.0
|
||||
loop_match_minimum_response_coarse: 0.35
|
||||
loop_match_minimum_response_fine: 0.45
|
||||
loop_search_maximum_distance: 3.0
|
||||
|
||||
# ── Correlation (coarse scan matching) ───────────────────────────────────
|
||||
correlation_search_space_dimension: 0.5
|
||||
correlation_search_space_resolution: 0.01
|
||||
correlation_search_space_smear_deviation: 0.1
|
||||
|
||||
# ── Loop search space ─────────────────────────────────────────────────────
|
||||
loop_search_space_dimension: 8.0
|
||||
loop_search_space_resolution: 0.05
|
||||
loop_search_space_smear_deviation: 0.03
|
||||
|
||||
# ── Response expansion ────────────────────────────────────────────────────
|
||||
link_match_minimum_response_fine: 0.1
|
||||
link_scan_maximum_distance: 1.5
|
||||
use_response_expansion: true
|
||||
|
||||
# ── Penalties (scan matcher quality thresholds) ───────────────────────────
|
||||
distance_variance_penalty: 0.5
|
||||
angle_variance_penalty: 1.0
|
||||
fine_search_angle_offset: 0.00349 # ~0.2°
|
||||
coarse_search_angle_offset: 0.349 # ~20°
|
||||
coarse_angle_resolution: 0.0349 # ~2°
|
||||
minimum_angle_penalty: 0.9
|
||||
minimum_distance_penalty: 0.5
|
||||
|
||||
# ── Solver ────────────────────────────────────────────────────────────────
|
||||
solver_plugin: solver_plugins::CeresSolver
|
||||
ceres_linear_solver: SPARSE_NORMAL_CHOLESKY
|
||||
ceres_preconditioner: SCHUR_JACOBI
|
||||
ceres_trust_strategy: LEVENBERG_MARQUARDT
|
||||
ceres_dogleg_type: TRADITIONAL_DOGLEG
|
||||
ceres_loss_function: None
|
||||
@ -29,8 +29,7 @@ services:
|
||||
# X11 socket for RViz2
|
||||
- /tmp/.X11-unix:/tmp/.X11-unix:rw
|
||||
# ROS2 workspace (host-mounted for live dev)
|
||||
# Mount src/ subdirectory so host structure mirrors container /ros2_ws/src/
|
||||
- ./ros2_ws/src:/ros2_ws/src:rw
|
||||
- ./ros2_ws:/ros2_ws/src:rw
|
||||
# Persistent SLAM maps
|
||||
- saltybot-maps:/maps
|
||||
# Config files
|
||||
|
||||
@ -1,7 +0,0 @@
|
||||
__pycache__/
|
||||
*.pyc
|
||||
*.pyo
|
||||
*.egg-info/
|
||||
build/
|
||||
install/
|
||||
log/
|
||||
@ -1,8 +0,0 @@
|
||||
stm32_serial_bridge:
|
||||
ros__parameters:
|
||||
# STM32 USB CDC device node
|
||||
# Use /dev/stm32-bridge if udev rule from jetson/docs/pinout.md is applied
|
||||
serial_port: /dev/ttyACM0
|
||||
baud_rate: 921600
|
||||
timeout: 0.1 # serial readline timeout (seconds)
|
||||
reconnect_delay: 2.0 # seconds between reconnect attempts on disconnect
|
||||
@ -1,34 +0,0 @@
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import DeclareLaunchArgument
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from launch_ros.actions import Node
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
serial_port_arg = DeclareLaunchArgument(
|
||||
"serial_port",
|
||||
default_value="/dev/ttyACM0",
|
||||
description="STM32 USB CDC device node",
|
||||
)
|
||||
baud_rate_arg = DeclareLaunchArgument(
|
||||
"baud_rate",
|
||||
default_value="921600",
|
||||
description="Serial baud rate",
|
||||
)
|
||||
|
||||
bridge_node = Node(
|
||||
package="saltybot_bridge",
|
||||
executable="serial_bridge_node",
|
||||
name="stm32_serial_bridge",
|
||||
output="screen",
|
||||
parameters=[
|
||||
{
|
||||
"serial_port": LaunchConfiguration("serial_port"),
|
||||
"baud_rate": LaunchConfiguration("baud_rate"),
|
||||
"timeout": 0.1,
|
||||
"reconnect_delay": 2.0,
|
||||
}
|
||||
],
|
||||
)
|
||||
|
||||
return LaunchDescription([serial_port_arg, baud_rate_arg, bridge_node])
|
||||
@ -1,27 +0,0 @@
|
||||
<?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>saltybot_bridge</name>
|
||||
<version>0.1.0</version>
|
||||
<description>
|
||||
STM32F722 USB CDC serial bridge for saltybot.
|
||||
Reads JSON telemetry from the STM32 flight controller and publishes
|
||||
sensor_msgs/Imu, std_msgs/String (balance state), and diagnostic_msgs/DiagnosticArray.
|
||||
</description>
|
||||
<maintainer email="sl-jetson@saltylab.local">sl-jetson</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<depend>rclpy</depend>
|
||||
<depend>sensor_msgs</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>diagnostic_msgs</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>
|
||||
@ -1,304 +0,0 @@
|
||||
"""
|
||||
saltybot_bridge — serial_bridge_node
|
||||
STM32F722 USB CDC → ROS2 topic publisher
|
||||
|
||||
Telemetry frame (50 Hz, newline-delimited JSON):
|
||||
{"p":<pitch×10>,"r":<roll×10>,"e":<err×10>,"ig":<integral×10>,
|
||||
"m":<motor_cmd -1000..1000>,"s":<state 0-2>,"y":<yaw×10>}
|
||||
|
||||
Error frame (IMU fault):
|
||||
{"err":<imu_errno>}
|
||||
|
||||
State values (balance_state_t):
|
||||
0 = DISARMED 1 = ARMED 2 = TILT_FAULT
|
||||
|
||||
Published topics:
|
||||
/saltybot/imu sensor_msgs/Imu — pitch/roll/yaw as angular velocity
|
||||
/saltybot/balance_state std_msgs/String (JSON) — full PID diagnostics
|
||||
"""
|
||||
|
||||
import json
|
||||
import math
|
||||
import rclpy
|
||||
from rclpy.node import Node
|
||||
from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
|
||||
|
||||
import serial
|
||||
|
||||
from sensor_msgs.msg import Imu
|
||||
from std_msgs.msg import String
|
||||
from diagnostic_msgs.msg import DiagnosticArray, DiagnosticStatus, KeyValue
|
||||
|
||||
# Balance state labels matching STM32 balance_state_t enum
|
||||
_STATE_LABEL = {0: "DISARMED", 1: "ARMED", 2: "TILT_FAULT"}
|
||||
|
||||
# Sensor frame_id published in Imu header
|
||||
IMU_FRAME_ID = "imu_link"
|
||||
|
||||
|
||||
class SerialBridgeNode(Node):
|
||||
def __init__(self):
|
||||
super().__init__("stm32_serial_bridge")
|
||||
|
||||
# ── Parameters ────────────────────────────────────────────────────────
|
||||
self.declare_parameter("serial_port", "/dev/ttyACM0")
|
||||
self.declare_parameter("baud_rate", 921600)
|
||||
self.declare_parameter("timeout", 0.1)
|
||||
self.declare_parameter("reconnect_delay", 2.0)
|
||||
|
||||
port = self.get_parameter("serial_port").value
|
||||
baud = self.get_parameter("baud_rate").value
|
||||
timeout = self.get_parameter("timeout").value
|
||||
self._reconnect_delay = self.get_parameter("reconnect_delay").value
|
||||
|
||||
# ── QoS ───────────────────────────────────────────────────────────────
|
||||
sensor_qos = QoSProfile(
|
||||
reliability=ReliabilityPolicy.BEST_EFFORT,
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=10,
|
||||
)
|
||||
diag_qos = QoSProfile(
|
||||
reliability=ReliabilityPolicy.RELIABLE,
|
||||
history=HistoryPolicy.KEEP_LAST,
|
||||
depth=10,
|
||||
)
|
||||
|
||||
# ── Publishers ────────────────────────────────────────────────────────
|
||||
self._imu_pub = self.create_publisher(Imu, "/saltybot/imu", sensor_qos)
|
||||
self._balance_pub = self.create_publisher(
|
||||
String, "/saltybot/balance_state", diag_qos
|
||||
)
|
||||
self._diag_pub = self.create_publisher(
|
||||
DiagnosticArray, "/diagnostics", diag_qos
|
||||
)
|
||||
|
||||
# ── State ─────────────────────────────────────────────────────────────
|
||||
self._port_name = port
|
||||
self._baud = baud
|
||||
self._timeout = timeout
|
||||
self._ser: serial.Serial | None = None
|
||||
self._frame_count = 0
|
||||
self._error_count = 0
|
||||
self._last_state = -1
|
||||
|
||||
# ── Open serial and start read timer ──────────────────────────────────
|
||||
self._open_serial()
|
||||
# Poll at 100 Hz — STM32 sends at 50 Hz, so we never miss a frame
|
||||
self._timer = self.create_timer(0.01, self._read_cb)
|
||||
|
||||
self.get_logger().info(
|
||||
f"stm32_serial_bridge started — {port} @ {baud} baud"
|
||||
)
|
||||
|
||||
# ── Serial management ─────────────────────────────────────────────────────
|
||||
|
||||
def _open_serial(self) -> bool:
|
||||
try:
|
||||
self._ser = serial.Serial(
|
||||
port=self._port_name,
|
||||
baudrate=self._baud,
|
||||
timeout=self._timeout,
|
||||
)
|
||||
self._ser.reset_input_buffer()
|
||||
self.get_logger().info(f"Serial port open: {self._port_name}")
|
||||
return True
|
||||
except serial.SerialException as exc:
|
||||
self.get_logger().error(f"Cannot open {self._port_name}: {exc}")
|
||||
self._ser = None
|
||||
return False
|
||||
|
||||
def _close_serial(self):
|
||||
if self._ser and self._ser.is_open:
|
||||
try:
|
||||
self._ser.close()
|
||||
except Exception:
|
||||
pass
|
||||
self._ser = None
|
||||
|
||||
# ── Read callback ─────────────────────────────────────────────────────────
|
||||
|
||||
def _read_cb(self):
|
||||
if self._ser is None or not self._ser.is_open:
|
||||
# Attempt reconnect on next timer fire after delay
|
||||
self.get_logger().warn(
|
||||
f"Serial disconnected — retrying in {self._reconnect_delay}s",
|
||||
throttle_duration_sec=self._reconnect_delay,
|
||||
)
|
||||
self._open_serial()
|
||||
return
|
||||
|
||||
try:
|
||||
# Read all lines buffered since last call
|
||||
while self._ser.in_waiting:
|
||||
raw = self._ser.readline()
|
||||
if raw:
|
||||
self._parse_and_publish(raw)
|
||||
except serial.SerialException as exc:
|
||||
self.get_logger().error(f"Serial read error: {exc}")
|
||||
self._close_serial()
|
||||
|
||||
# ── Parsing + publishing ──────────────────────────────────────────────────
|
||||
|
||||
def _parse_and_publish(self, raw: bytes):
|
||||
try:
|
||||
text = raw.decode("ascii", errors="ignore").strip()
|
||||
if not text:
|
||||
return
|
||||
data = json.loads(text)
|
||||
except (ValueError, UnicodeDecodeError) as exc:
|
||||
self.get_logger().debug(f"Parse error ({exc}): {raw!r}")
|
||||
self._error_count += 1
|
||||
return
|
||||
|
||||
now = self.get_clock().now().to_msg()
|
||||
|
||||
# IMU fault frame — {"err": <errno>}
|
||||
if "err" in data:
|
||||
self._publish_imu_fault(data["err"], now)
|
||||
return
|
||||
|
||||
# Normal telemetry frame — validate required keys
|
||||
required = ("p", "r", "e", "ig", "m", "s", "y")
|
||||
if not all(k in data for k in required):
|
||||
self.get_logger().debug(f"Incomplete frame: {text}")
|
||||
return
|
||||
|
||||
# Values are ×10 integers from firmware — convert to float
|
||||
pitch_deg = data["p"] / 10.0
|
||||
roll_deg = data["r"] / 10.0
|
||||
yaw_deg = data["y"] / 10.0
|
||||
error_deg = data["e"] / 10.0
|
||||
integral = data["ig"] / 10.0
|
||||
motor_cmd = float(data["m"]) # -1000..+1000 ESC units
|
||||
state = int(data["s"])
|
||||
|
||||
self._frame_count += 1
|
||||
|
||||
self._publish_imu(pitch_deg, roll_deg, yaw_deg, now)
|
||||
self._publish_balance_state(
|
||||
pitch_deg, roll_deg, yaw_deg,
|
||||
error_deg, integral, motor_cmd, state, now,
|
||||
)
|
||||
|
||||
# Log state transitions
|
||||
if state != self._last_state:
|
||||
label = _STATE_LABEL.get(state, f"UNKNOWN({state})")
|
||||
self.get_logger().info(f"Balance state → {label}")
|
||||
self._last_state = state
|
||||
|
||||
def _publish_imu(self, pitch_deg, roll_deg, yaw_deg, stamp):
|
||||
"""
|
||||
Publish sensor_msgs/Imu.
|
||||
|
||||
The STM32 IMU gives Euler angles (pitch/roll from accelerometer+gyro
|
||||
fusion, yaw from gyro integration). We publish them as angular_velocity
|
||||
for immediate use by slam_toolbox / robot_localization.
|
||||
|
||||
Note: orientation quaternion is left zeroed (covariance [-1,...]) until
|
||||
a proper fusion node (e.g. robot_localization EKF) computes it.
|
||||
"""
|
||||
msg = Imu()
|
||||
msg.header.stamp = stamp
|
||||
msg.header.frame_id = IMU_FRAME_ID
|
||||
|
||||
# orientation unknown — signal with -1 in first covariance element
|
||||
msg.orientation_covariance[0] = -1.0
|
||||
|
||||
# angular_velocity: Euler rates approximation (good enough at low freq)
|
||||
msg.angular_velocity.x = math.radians(pitch_deg) # pitch → x
|
||||
msg.angular_velocity.y = math.radians(roll_deg) # roll → y
|
||||
msg.angular_velocity.z = math.radians(yaw_deg) # yaw → z
|
||||
|
||||
# angular_velocity covariance (diagonal, degrees converted to rad)
|
||||
cov_rad = math.radians(0.5) ** 2 # ±0.5° noise estimate
|
||||
msg.angular_velocity_covariance[0] = cov_rad
|
||||
msg.angular_velocity_covariance[4] = cov_rad
|
||||
msg.angular_velocity_covariance[8] = cov_rad
|
||||
|
||||
# linear_acceleration: unknown from this frame
|
||||
msg.linear_acceleration_covariance[0] = -1.0
|
||||
|
||||
self._imu_pub.publish(msg)
|
||||
|
||||
def _publish_balance_state(
|
||||
self, pitch, roll, yaw, error, integral, motor_cmd, state, stamp
|
||||
):
|
||||
"""Publish full PID diagnostics as JSON string and /diagnostics."""
|
||||
state_label = _STATE_LABEL.get(state, f"UNKNOWN({state})")
|
||||
|
||||
payload = {
|
||||
"stamp": f"{stamp.sec}.{stamp.nanosec:09d}",
|
||||
"state": state_label,
|
||||
"pitch_deg": round(pitch, 1),
|
||||
"roll_deg": round(roll, 1),
|
||||
"yaw_deg": round(yaw, 1),
|
||||
"pid_error_deg": round(error, 1),
|
||||
"integral": round(integral, 1),
|
||||
"motor_cmd": int(motor_cmd),
|
||||
"frames": self._frame_count,
|
||||
"parse_errors": self._error_count,
|
||||
}
|
||||
|
||||
str_msg = String()
|
||||
str_msg.data = json.dumps(payload)
|
||||
self._balance_pub.publish(str_msg)
|
||||
|
||||
# /diagnostics
|
||||
diag = DiagnosticArray()
|
||||
diag.header.stamp = stamp
|
||||
status = DiagnosticStatus()
|
||||
status.name = "saltybot/balance_controller"
|
||||
status.hardware_id = "stm32f722"
|
||||
status.message = state_label
|
||||
|
||||
if state == 1: # ARMED
|
||||
status.level = DiagnosticStatus.OK
|
||||
elif state == 0: # DISARMED
|
||||
status.level = DiagnosticStatus.WARN
|
||||
else: # TILT_FAULT
|
||||
status.level = DiagnosticStatus.ERROR
|
||||
|
||||
status.values = [
|
||||
KeyValue(key="pitch_deg", value=f"{pitch:.1f}"),
|
||||
KeyValue(key="roll_deg", value=f"{roll:.1f}"),
|
||||
KeyValue(key="yaw_deg", value=f"{yaw:.1f}"),
|
||||
KeyValue(key="pid_error_deg", value=f"{error:.1f}"),
|
||||
KeyValue(key="integral", value=f"{integral:.1f}"),
|
||||
KeyValue(key="motor_cmd", value=f"{int(motor_cmd)}"),
|
||||
KeyValue(key="state", value=state_label),
|
||||
]
|
||||
diag.status.append(status)
|
||||
self._diag_pub.publish(diag)
|
||||
|
||||
def _publish_imu_fault(self, errno: int, stamp):
|
||||
"""On IMU fault frame, publish an ERROR diagnostic."""
|
||||
diag = DiagnosticArray()
|
||||
diag.header.stamp = stamp
|
||||
status = DiagnosticStatus()
|
||||
status.level = DiagnosticStatus.ERROR
|
||||
status.name = "saltybot/balance_controller"
|
||||
status.hardware_id = "stm32f722"
|
||||
status.message = f"IMU fault errno={errno}"
|
||||
diag.status.append(status)
|
||||
self._diag_pub.publish(diag)
|
||||
self.get_logger().error(f"STM32 reported IMU fault: errno={errno}")
|
||||
|
||||
def destroy_node(self):
|
||||
self._close_serial()
|
||||
super().destroy_node()
|
||||
|
||||
|
||||
def main(args=None):
|
||||
rclpy.init(args=args)
|
||||
node = SerialBridgeNode()
|
||||
try:
|
||||
rclpy.spin(node)
|
||||
except KeyboardInterrupt:
|
||||
pass
|
||||
finally:
|
||||
node.destroy_node()
|
||||
rclpy.shutdown()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
@ -1,4 +0,0 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/saltybot_bridge
|
||||
[install]
|
||||
install_scripts=$base/lib/saltybot_bridge
|
||||
@ -1,27 +0,0 @@
|
||||
from setuptools import setup
|
||||
|
||||
package_name = "saltybot_bridge"
|
||||
|
||||
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/bridge.launch.py"]),
|
||||
(f"share/{package_name}/config", ["config/bridge_params.yaml"]),
|
||||
],
|
||||
install_requires=["setuptools", "pyserial"],
|
||||
zip_safe=True,
|
||||
maintainer="sl-jetson",
|
||||
maintainer_email="sl-jetson@saltylab.local",
|
||||
description="STM32 USB CDC → ROS2 serial bridge for saltybot",
|
||||
license="MIT",
|
||||
tests_require=["pytest"],
|
||||
entry_points={
|
||||
"console_scripts": [
|
||||
"serial_bridge_node = saltybot_bridge.serial_bridge_node:main",
|
||||
],
|
||||
},
|
||||
)
|
||||
@ -1,97 +0,0 @@
|
||||
"""
|
||||
Unit tests for STM32 telemetry parsing logic.
|
||||
Run with: pytest jetson/ros2_ws/src/saltybot_bridge/test/test_parse.py
|
||||
"""
|
||||
|
||||
import json
|
||||
import pytest
|
||||
|
||||
|
||||
# ── Minimal stub so we can test parsing without a ROS2 runtime ───────────────
|
||||
|
||||
def parse_frame(raw: bytes):
|
||||
"""Mirror of the parsing logic in serial_bridge_node._parse_and_publish."""
|
||||
text = raw.decode("ascii", errors="ignore").strip()
|
||||
if not text:
|
||||
return None
|
||||
data = json.loads(text)
|
||||
|
||||
if "err" in data:
|
||||
return {"type": "fault", "errno": data["err"]}
|
||||
|
||||
required = ("p", "r", "e", "ig", "m", "s", "y")
|
||||
if not all(k in data for k in required):
|
||||
raise ValueError(f"Incomplete frame: {data}")
|
||||
|
||||
return {
|
||||
"type": "telemetry",
|
||||
"pitch_deg": data["p"] / 10.0,
|
||||
"roll_deg": data["r"] / 10.0,
|
||||
"yaw_deg": data["y"] / 10.0,
|
||||
"pid_error_deg": data["e"] / 10.0,
|
||||
"integral": data["ig"] / 10.0,
|
||||
"motor_cmd": int(data["m"]),
|
||||
"state": int(data["s"]),
|
||||
}
|
||||
|
||||
|
||||
# ── Tests ─────────────────────────────────────────────────────────────────────
|
||||
|
||||
def test_normal_frame():
|
||||
raw = b'{"p":125,"r":-30,"e":50,"ig":20,"m":350,"s":1,"y":0}\n'
|
||||
result = parse_frame(raw)
|
||||
assert result["type"] == "telemetry"
|
||||
assert result["pitch_deg"] == pytest.approx(12.5)
|
||||
assert result["roll_deg"] == pytest.approx(-3.0)
|
||||
assert result["yaw_deg"] == pytest.approx(0.0)
|
||||
assert result["pid_error_deg"] == pytest.approx(5.0)
|
||||
assert result["integral"] == pytest.approx(2.0)
|
||||
assert result["motor_cmd"] == 350
|
||||
assert result["state"] == 1 # ARMED
|
||||
|
||||
|
||||
def test_fault_frame():
|
||||
raw = b'{"err":-1}\n'
|
||||
result = parse_frame(raw)
|
||||
assert result["type"] == "fault"
|
||||
assert result["errno"] == -1
|
||||
|
||||
|
||||
def test_zero_frame():
|
||||
raw = b'{"p":0,"r":0,"e":0,"ig":0,"m":0,"s":0,"y":0}\n'
|
||||
result = parse_frame(raw)
|
||||
assert result["type"] == "telemetry"
|
||||
assert result["pitch_deg"] == pytest.approx(0.0)
|
||||
assert result["state"] == 0 # DISARMED
|
||||
|
||||
|
||||
def test_tilt_fault_state():
|
||||
raw = b'{"p":450,"r":0,"e":400,"ig":999,"m":1000,"s":2,"y":0}\n'
|
||||
result = parse_frame(raw)
|
||||
assert result["state"] == 2 # TILT_FAULT
|
||||
assert result["pitch_deg"] == pytest.approx(45.0)
|
||||
assert result["motor_cmd"] == 1000
|
||||
|
||||
|
||||
def test_negative_motor_cmd():
|
||||
raw = b'{"p":-100,"r":0,"e":-100,"ig":-50,"m":-750,"s":1,"y":10}\n'
|
||||
result = parse_frame(raw)
|
||||
assert result["motor_cmd"] == -750
|
||||
assert result["pitch_deg"] == pytest.approx(-10.0)
|
||||
|
||||
|
||||
def test_incomplete_frame_raises():
|
||||
raw = b'{"p":100,"r":0}\n'
|
||||
with pytest.raises(ValueError, match="Incomplete frame"):
|
||||
parse_frame(raw)
|
||||
|
||||
|
||||
def test_empty_line_returns_none():
|
||||
assert parse_frame(b"\n") is None
|
||||
assert parse_frame(b" \n") is None
|
||||
|
||||
|
||||
def test_corrupt_frame_raises_json_error():
|
||||
raw = b'not_json\n'
|
||||
with pytest.raises(json.JSONDecodeError):
|
||||
parse_frame(raw)
|
||||
@ -1,131 +0,0 @@
|
||||
# saltybot_bringup — Sensor Topic Reference
|
||||
|
||||
ROS2 Humble | ROS_DOMAIN_ID=42 | DDS: CycloneDDS
|
||||
|
||||
---
|
||||
|
||||
## Build & Run
|
||||
|
||||
```bash
|
||||
# From inside the container (or ros2_ws root on host after sourcing ROS2):
|
||||
cd /ros2_ws
|
||||
colcon build --packages-select saltybot_bringup --symlink-install
|
||||
source install/local_setup.bash
|
||||
|
||||
# Sensors only (verify data before SLAM):
|
||||
ros2 launch saltybot_bringup sensors.launch.py
|
||||
|
||||
# Full SLAM stack (saltybot-ros2 docker-compose service uses this):
|
||||
ros2 launch saltybot_bringup slam.launch.py
|
||||
|
||||
# Individual drivers:
|
||||
ros2 launch saltybot_bringup realsense.launch.py
|
||||
ros2 launch saltybot_bringup rplidar.launch.py
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Topic Reference
|
||||
|
||||
### RPLIDAR A1M8
|
||||
|
||||
| Topic | Type | Rate | Notes |
|
||||
|-------|------|------|-------|
|
||||
| `/scan` | `sensor_msgs/LaserScan` | ~5.5 Hz | 360°, 1440 pts/scan, 12m range |
|
||||
|
||||
```
|
||||
angle_min: -π (-180°)
|
||||
angle_max: +π (+180°)
|
||||
range_min: 0.15 m
|
||||
range_max: 12.00 m
|
||||
scan_time: ~0.182 s
|
||||
frame_id: laser
|
||||
```
|
||||
|
||||
### Intel RealSense D435i
|
||||
|
||||
| Topic | Type | Rate | Notes |
|
||||
|-------|------|------|-------|
|
||||
| `/camera/color/image_raw` | `sensor_msgs/Image` | 15 Hz | RGB8, 640×480 |
|
||||
| `/camera/color/camera_info` | `sensor_msgs/CameraInfo` | 15 Hz | Intrinsics |
|
||||
| `/camera/depth/image_rect_raw` | `sensor_msgs/Image` | 15 Hz | Z16, 640×480 |
|
||||
| `/camera/depth/camera_info` | `sensor_msgs/CameraInfo` | 15 Hz | Intrinsics |
|
||||
| `/camera/aligned_depth_to_color/image_raw` | `sensor_msgs/Image` | 15 Hz | Depth in color frame |
|
||||
| `/camera/imu` | `sensor_msgs/Imu` | ~200 Hz | Accel + gyro fused (linear_interpolation) |
|
||||
|
||||
### TF Tree
|
||||
|
||||
```
|
||||
map
|
||||
└── odom (published by slam_toolbox once moving)
|
||||
└── base_link (robot body frame)
|
||||
├── laser (RPLIDAR A1M8) ← static, 10cm above base
|
||||
└── camera_link (RealSense D435i) ← static, 5cm fwd, 15cm up
|
||||
├── camera_depth_frame
|
||||
├── camera_color_frame
|
||||
└── camera_imu_frame
|
||||
```
|
||||
|
||||
**Note:** Static TF values in `sensors.launch.py` are placeholders.
|
||||
Update after physical mount with real measurements.
|
||||
|
||||
---
|
||||
|
||||
## Verification Commands
|
||||
|
||||
```bash
|
||||
# List all active topics
|
||||
ros2 topic list
|
||||
|
||||
# Check data rates
|
||||
ros2 topic hz /scan
|
||||
ros2 topic hz /camera/color/image_raw
|
||||
ros2 topic hz /camera/imu
|
||||
|
||||
# Spot-check data
|
||||
ros2 topic echo /scan --once
|
||||
ros2 topic echo /camera/imu --once
|
||||
|
||||
# TF tree
|
||||
ros2 run tf2_tools view_frames
|
||||
ros2 run tf2_ros tf2_echo base_link laser
|
||||
|
||||
# SLAM map topic (after slam.launch.py)
|
||||
ros2 topic hz /map
|
||||
ros2 topic echo /map --no-arr # metadata without raw data flood
|
||||
```
|
||||
|
||||
---
|
||||
|
||||
## Troubleshooting
|
||||
|
||||
### RPLIDAR not found
|
||||
```bash
|
||||
# Check udev symlink
|
||||
ls -la /dev/rplidar
|
||||
# If missing, fall back to:
|
||||
ros2 launch saltybot_bringup rplidar.launch.py serial_port:=/dev/ttyUSB0
|
||||
# Install udev rule (host, not container):
|
||||
sudo cp /path/to/jetson/docs/99-saltybot.rules /etc/udev/rules.d/
|
||||
sudo udevadm control --reload-rules && sudo udevadm trigger
|
||||
```
|
||||
|
||||
### RealSense not detected
|
||||
```bash
|
||||
# Check USB3 port (blue port on Nano)
|
||||
lsusb | grep "8086:0b3a"
|
||||
# Expected: Bus 002 Device 00X: ID 8086:0b3a Intel Corp. Intel RealSense D435i
|
||||
|
||||
# Install udev rules (host, not container):
|
||||
sudo cp /etc/udev/rules.d/99-realsense-libusb.rules /etc/udev/rules.d/
|
||||
sudo udevadm control --reload-rules && sudo udevadm trigger
|
||||
```
|
||||
|
||||
### IMU not publishing
|
||||
```bash
|
||||
# Verify enable_gyro and enable_accel are true
|
||||
ros2 param get /camera/camera enable_gyro
|
||||
ros2 param get /camera/camera enable_accel
|
||||
# Check unite_imu_method (should be 2)
|
||||
ros2 param get /camera/camera unite_imu_method
|
||||
```
|
||||
@ -1,85 +0,0 @@
|
||||
"""
|
||||
realsense.launch.py — Intel RealSense D435i driver (standalone)
|
||||
|
||||
Launches realsense2_camera_node with Jetson Nano power-budget settings:
|
||||
- 640×480 @ 15fps (depth + RGB) — saves ~0.4W vs 30fps
|
||||
- IMU enabled with linear interpolation (unified /camera/imu topic)
|
||||
- Depth aligned to color frame
|
||||
- Point cloud disabled (expensive on Maxwell GPU)
|
||||
|
||||
Published topics:
|
||||
/camera/color/image_raw sensor_msgs/Image 15 Hz
|
||||
/camera/color/camera_info sensor_msgs/CameraInfo 15 Hz
|
||||
/camera/depth/image_rect_raw sensor_msgs/Image 15 Hz
|
||||
/camera/depth/camera_info sensor_msgs/CameraInfo 15 Hz
|
||||
/camera/aligned_depth_to_color/image_raw sensor_msgs/Image 15 Hz
|
||||
/camera/imu sensor_msgs/Imu ~200 Hz
|
||||
|
||||
Verify:
|
||||
ros2 topic list | grep camera
|
||||
ros2 topic hz /camera/color/image_raw
|
||||
ros2 topic hz /camera/imu
|
||||
"""
|
||||
|
||||
import os
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
# Allow overriding camera serial number at launch time (useful with multiple cameras)
|
||||
serial_no_arg = DeclareLaunchArgument(
|
||||
'serial_no',
|
||||
default_value="''",
|
||||
description='RealSense device serial number (empty = first found)',
|
||||
)
|
||||
|
||||
realsense_share = get_package_share_directory('realsense2_camera')
|
||||
|
||||
realsense_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(realsense_share, 'launch', 'rs_launch.py')
|
||||
),
|
||||
launch_arguments={
|
||||
# Camera identity
|
||||
'serial_no': LaunchConfiguration('serial_no'),
|
||||
'camera_name': 'camera',
|
||||
'camera_namespace': 'camera',
|
||||
|
||||
# Depth stream — 640×480 @ 15fps saves power on Nano
|
||||
'depth_module.profile': '640x480x15',
|
||||
'enable_depth': 'true',
|
||||
|
||||
# RGB stream — matched resolution/fps to depth
|
||||
'rgb_camera.profile': '640x480x15',
|
||||
'enable_color': 'true',
|
||||
|
||||
# IR streams — disabled (not needed for RTAB-Map RGB-D mode)
|
||||
'enable_infra1': 'false',
|
||||
'enable_infra2': 'false',
|
||||
|
||||
# IMU — enable both sensors, publish unified topic
|
||||
'enable_gyro': 'true',
|
||||
'enable_accel': 'true',
|
||||
# 2 = linear_interpolation: aligns accel+gyro timestamps
|
||||
'unite_imu_method': '2',
|
||||
|
||||
# Align depth to color frame (required for rtabmap_ros RGB-D input)
|
||||
'align_depth.enable': 'true',
|
||||
|
||||
# Point cloud — disabled, too expensive for Maxwell GPU during SLAM
|
||||
'pointcloud.enable': 'false',
|
||||
|
||||
# TF — publish camera→IMU extrinsics
|
||||
'publish_tf': 'true',
|
||||
'tf_publish_rate': '0.0', # static only, no redundant updates
|
||||
}.items(),
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
serial_no_arg,
|
||||
realsense_launch,
|
||||
])
|
||||
@ -1,60 +0,0 @@
|
||||
"""
|
||||
rplidar.launch.py — RPLIDAR A1M8 driver (standalone)
|
||||
|
||||
Launches rplidar_ros with udev symlink (/dev/rplidar) set in 99-saltybot.rules.
|
||||
Falls back to /dev/ttyUSB0 if the symlink is not present.
|
||||
|
||||
RPLIDAR A1M8 specs:
|
||||
- 360° omnidirectional scan
|
||||
- 8000 samples/s, ~5.5 Hz scan rate at 1440 points/scan
|
||||
- 12m range (reliable to ~8m indoors)
|
||||
- 115200 baud via CP2102 USB-UART adapter
|
||||
|
||||
Published topics:
|
||||
/scan sensor_msgs/LaserScan ~5.5 Hz
|
||||
|
||||
TF frame: laser → matches static_transform_publisher in sensors.launch.py
|
||||
frame_id = 'laser'
|
||||
|
||||
Verify:
|
||||
ros2 topic hz /scan
|
||||
ros2 run tf2_ros tf2_echo base_link laser
|
||||
"""
|
||||
|
||||
import os
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch.substitutions import LaunchConfiguration
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
serial_port_arg = DeclareLaunchArgument(
|
||||
'serial_port',
|
||||
default_value='/dev/rplidar',
|
||||
description='RPLIDAR serial port (udev symlink preferred over /dev/ttyUSB0)',
|
||||
)
|
||||
|
||||
rplidar_share = get_package_share_directory('rplidar_ros')
|
||||
|
||||
rplidar_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(rplidar_share, 'launch', 'rplidar_a1_launch.py')
|
||||
),
|
||||
launch_arguments={
|
||||
'serial_port': LaunchConfiguration('serial_port'),
|
||||
'serial_baudrate': '115200',
|
||||
# 'laser' matches the TF frame in sensors.launch.py and slam config
|
||||
'frame_id': 'laser',
|
||||
# Compensate for motor rotation angle offset
|
||||
'angle_compensate': 'true',
|
||||
# A1M8 only supports Standard scan mode
|
||||
'scan_mode': 'Standard',
|
||||
}.items(),
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
serial_port_arg,
|
||||
rplidar_launch,
|
||||
])
|
||||
@ -1,92 +0,0 @@
|
||||
"""
|
||||
sensors.launch.py — All sensor drivers + static TF
|
||||
|
||||
Brings up both sensors together with placeholder static transforms.
|
||||
Use this to verify sensor data before running full SLAM stack.
|
||||
|
||||
Static TF (placeholder — update with real measurements after physical mount):
|
||||
base_link → laser x=0.00 y=0.00 z=0.10 (RPLIDAR: 10cm above base)
|
||||
base_link → camera_link x=0.05 y=0.00 z=0.15 (D435i: 5cm fwd, 15cm up)
|
||||
|
||||
Published topics summary:
|
||||
/scan LaserScan ~5.5 Hz (RPLIDAR)
|
||||
/camera/color/image_raw Image 15 Hz (D435i RGB)
|
||||
/camera/depth/image_rect_raw Image 15 Hz (D435i depth)
|
||||
/camera/aligned_depth_to_color/image_raw Image 15 Hz (D435i aligned)
|
||||
/camera/imu Imu ~200 Hz (D435i IMU)
|
||||
|
||||
Quick verification:
|
||||
ros2 topic list
|
||||
ros2 topic hz /scan # expect ~5.5 Hz
|
||||
ros2 topic hz /camera/color/image_raw # expect 15 Hz
|
||||
ros2 topic hz /camera/imu # expect ~200 Hz
|
||||
ros2 run tf2_tools view_frames # check TF tree
|
||||
"""
|
||||
|
||||
import os
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import IncludeLaunchDescription
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from launch_ros.actions import Node
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
bringup_share = get_package_share_directory('saltybot_bringup')
|
||||
|
||||
realsense_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(bringup_share, 'launch', 'realsense.launch.py')
|
||||
),
|
||||
)
|
||||
|
||||
rplidar_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(bringup_share, 'launch', 'rplidar.launch.py')
|
||||
),
|
||||
)
|
||||
|
||||
# Static TF: base_link → laser (RPLIDAR A1M8 mount position)
|
||||
# TODO: update x/y/z/yaw with real measurements from robot chassis
|
||||
laser_tf = Node(
|
||||
package='tf2_ros',
|
||||
executable='static_transform_publisher',
|
||||
name='base_link_to_laser',
|
||||
arguments=[
|
||||
'--x', '0.00',
|
||||
'--y', '0.00',
|
||||
'--z', '0.10', # 10cm above base_link
|
||||
'--roll', '0',
|
||||
'--pitch', '0',
|
||||
'--yaw', '0',
|
||||
'--frame-id', 'base_link',
|
||||
'--child-frame-id', 'laser',
|
||||
],
|
||||
output='screen',
|
||||
)
|
||||
|
||||
# Static TF: base_link → camera_link (RealSense D435i mount position)
|
||||
# TODO: update x/y/z with real measurements from robot chassis
|
||||
camera_tf = Node(
|
||||
package='tf2_ros',
|
||||
executable='static_transform_publisher',
|
||||
name='base_link_to_camera',
|
||||
arguments=[
|
||||
'--x', '0.05', # 5cm forward of base_link center
|
||||
'--y', '0.00',
|
||||
'--z', '0.15', # 15cm above base_link
|
||||
'--roll', '0',
|
||||
'--pitch', '0',
|
||||
'--yaw', '0',
|
||||
'--frame-id', 'base_link',
|
||||
'--child-frame-id', 'camera_link',
|
||||
],
|
||||
output='screen',
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
realsense_launch,
|
||||
rplidar_launch,
|
||||
laser_tf,
|
||||
camera_tf,
|
||||
])
|
||||
@ -1,62 +0,0 @@
|
||||
"""
|
||||
slam.launch.py — Full SLAM stack (sensors + slam_toolbox)
|
||||
|
||||
Entry point referenced by docker-compose.yml saltybot-ros2 service.
|
||||
|
||||
Stack:
|
||||
sensors.launch.py (RPLIDAR + RealSense + static TF)
|
||||
slam_toolbox online_async — 2D LIDAR SLAM from /scan
|
||||
|
||||
SLAM input:
|
||||
/scan LaserScan from RPLIDAR A1M8
|
||||
/tf base_link → laser (from sensors.launch.py)
|
||||
|
||||
SLAM output:
|
||||
/map OccupancyGrid (2D occupancy map)
|
||||
/slam_toolbox/... Internal slam_toolbox topics
|
||||
|
||||
Note: slam_toolbox uses LIDAR-only SLAM (no RGB-D from D435i at this stage).
|
||||
For RGB-D SLAM (RTAB-Map), use a separate launch file once slam_toolbox
|
||||
mapping is validated — see SLAM-SETUP-PLAN.md (bd-wax) for full plan.
|
||||
|
||||
Config: /config/slam_toolbox_params.yaml (mounted from jetson/config/)
|
||||
|
||||
Verify:
|
||||
ros2 topic hz /map # expect update every ~5s (map_update_interval)
|
||||
ros2 run rviz2 rviz2 # visualize map + scan
|
||||
"""
|
||||
|
||||
import os
|
||||
from launch import LaunchDescription
|
||||
from launch.actions import IncludeLaunchDescription
|
||||
from launch.launch_description_sources import PythonLaunchDescriptionSource
|
||||
from ament_index_python.packages import get_package_share_directory
|
||||
|
||||
|
||||
SLAM_PARAMS_FILE = '/config/slam_toolbox_params.yaml'
|
||||
|
||||
|
||||
def generate_launch_description():
|
||||
bringup_share = get_package_share_directory('saltybot_bringup')
|
||||
slam_share = get_package_share_directory('slam_toolbox')
|
||||
|
||||
sensors_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(bringup_share, 'launch', 'sensors.launch.py')
|
||||
),
|
||||
)
|
||||
|
||||
slam_launch = IncludeLaunchDescription(
|
||||
PythonLaunchDescriptionSource(
|
||||
os.path.join(slam_share, 'launch', 'online_async_launch.py')
|
||||
),
|
||||
launch_arguments={
|
||||
'params_file': SLAM_PARAMS_FILE,
|
||||
'use_sim_time': 'false',
|
||||
}.items(),
|
||||
)
|
||||
|
||||
return LaunchDescription([
|
||||
sensors_launch,
|
||||
slam_launch,
|
||||
])
|
||||
@ -1,26 +0,0 @@
|
||||
<?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>saltybot_bringup</name>
|
||||
<version>0.1.0</version>
|
||||
<description>SaltyBot launch files — RealSense D435i + RPLIDAR A1M8 sensor bringup and SLAM integration</description>
|
||||
<maintainer email="sl-perception@saltylab.local">sl-perception</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<exec_depend>rplidar_ros</exec_depend>
|
||||
<exec_depend>realsense2_camera</exec_depend>
|
||||
<exec_depend>realsense2_description</exec_depend>
|
||||
<exec_depend>slam_toolbox</exec_depend>
|
||||
<exec_depend>robot_state_publisher</exec_depend>
|
||||
<exec_depend>tf2_ros</exec_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>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@ -1,4 +0,0 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/saltybot_bringup
|
||||
[install]
|
||||
install_scripts=$base/lib/saltybot_bringup
|
||||
@ -1,30 +0,0 @@
|
||||
from setuptools import setup
|
||||
import os
|
||||
from glob import glob
|
||||
|
||||
package_name = 'saltybot_bringup'
|
||||
|
||||
setup(
|
||||
name=package_name,
|
||||
version='0.1.0',
|
||||
packages=[package_name],
|
||||
data_files=[
|
||||
('share/ament_index/resource_index/packages',
|
||||
['resource/' + package_name]),
|
||||
('share/' + package_name, ['package.xml']),
|
||||
(os.path.join('share', package_name, 'launch'),
|
||||
glob('launch/*.launch.py')),
|
||||
(os.path.join('share', package_name, 'config'),
|
||||
glob('config/*.yaml')),
|
||||
],
|
||||
install_requires=['setuptools'],
|
||||
zip_safe=True,
|
||||
maintainer='sl-perception',
|
||||
maintainer_email='sl-perception@saltylab.local',
|
||||
description='SaltyBot sensor bringup and SLAM launch files',
|
||||
license='MIT',
|
||||
tests_require=['pytest'],
|
||||
entry_points={
|
||||
'console_scripts': [],
|
||||
},
|
||||
)
|
||||
@ -16,21 +16,8 @@ volatile uint8_t cdc_disarm_request = 0; /* set by D command */
|
||||
volatile uint8_t cdc_cmd_ready = 0;
|
||||
volatile char cdc_cmd_buf[32];
|
||||
|
||||
/*
|
||||
* USB TX/RX buffers grouped into a single 512-byte aligned struct so that
|
||||
* one MPU region (configured in usbd_conf.c) can mark them non-cacheable.
|
||||
* Size must be a power-of-2 >= total size for MPU RASR SIZE encoding.
|
||||
*/
|
||||
static struct {
|
||||
uint8_t tx[256];
|
||||
uint8_t rx[256];
|
||||
} __attribute__((aligned(512))) usb_nc_buf;
|
||||
|
||||
#define UserTxBuffer usb_nc_buf.tx
|
||||
#define UserRxBuffer usb_nc_buf.rx
|
||||
|
||||
/* Exported so usbd_conf.c USB_NC_MPU_Config() can set the region base */
|
||||
void * const usb_nc_buf_base = &usb_nc_buf;
|
||||
static uint8_t UserRxBuffer[256];
|
||||
static uint8_t UserTxBuffer[256];
|
||||
|
||||
/*
|
||||
* Betaflight-proven DFU reboot:
|
||||
|
||||
@ -5,33 +5,6 @@
|
||||
#include "usbd_cdc.h"
|
||||
#include "usbd_conf.h"
|
||||
|
||||
/*
|
||||
* Mark USB TX/RX buffers non-cacheable via MPU Region 0.
|
||||
* Cortex-M7 TEX=1, C=0, B=0 → Normal Non-cacheable.
|
||||
* Called before HAL_PCD_Init() so the region is active before the USB
|
||||
* hardware ever touches the buffers.
|
||||
*/
|
||||
extern void * const usb_nc_buf_base; /* defined in usbd_cdc_if.c */
|
||||
|
||||
static void USB_NC_MPU_Config(void)
|
||||
{
|
||||
MPU_Region_InitTypeDef r = {0};
|
||||
HAL_MPU_Disable();
|
||||
r.Enable = MPU_REGION_ENABLE;
|
||||
r.Number = MPU_REGION_NUMBER0;
|
||||
r.BaseAddress = (uint32_t)usb_nc_buf_base;
|
||||
r.Size = MPU_REGION_SIZE_512B;
|
||||
r.SubRegionDisable = 0x00;
|
||||
r.TypeExtField = MPU_TEX_LEVEL1; /* TEX=1 */
|
||||
r.AccessPermission = MPU_REGION_FULL_ACCESS;
|
||||
r.DisableExec = MPU_INSTRUCTION_ACCESS_DISABLE;
|
||||
r.IsShareable = MPU_ACCESS_NOT_SHAREABLE;
|
||||
r.IsCacheable = MPU_ACCESS_NOT_CACHEABLE; /* C=0 */
|
||||
r.IsBufferable = MPU_ACCESS_NOT_BUFFERABLE; /* B=0 */
|
||||
HAL_MPU_ConfigRegion(&r);
|
||||
HAL_MPU_Enable(MPU_PRIVILEGED_DEFAULT);
|
||||
}
|
||||
|
||||
PCD_HandleTypeDef hpcd;
|
||||
|
||||
void HAL_PCD_MspInit(PCD_HandleTypeDef *hpcd)
|
||||
@ -95,7 +68,6 @@ USBD_StatusTypeDef USBD_LL_Init(USBD_HandleTypeDef *pdev)
|
||||
hpcd.pData = pdev;
|
||||
pdev->pData = &hpcd;
|
||||
|
||||
USB_NC_MPU_Config(); /* Mark USB buffers non-cacheable before USB hardware init */
|
||||
HAL_PCD_Init(&hpcd);
|
||||
|
||||
HAL_PCDEx_SetRxFiFo(&hpcd, 0x80);
|
||||
|
||||
19
src/crsf.c
19
src/crsf.c
@ -1,19 +0,0 @@
|
||||
/*
|
||||
* crsf.c — CRSF RC receiver stub (UART not yet wired on MAMBA F722S).
|
||||
* crsf_state.last_rx_ms stays 0 so safety_rc_alive() always returns false,
|
||||
* which keeps USB-only mode active (RC timeout disarm is commented out in main.c).
|
||||
*/
|
||||
#include "crsf.h"
|
||||
|
||||
volatile CRSFState crsf_state = {0};
|
||||
|
||||
void crsf_init(void) {}
|
||||
|
||||
void crsf_parse_byte(uint8_t byte) { (void)byte; }
|
||||
|
||||
int16_t crsf_to_range(uint16_t val, int16_t min, int16_t max)
|
||||
{
|
||||
/* CRSF range 172-1811 → linear map to [min, max] */
|
||||
int32_t v = (int32_t)val;
|
||||
return (int16_t)(min + (v - 172) * (max - min) / (1811 - 172));
|
||||
}
|
||||
45
src/main.c
45
src/main.c
@ -6,7 +6,6 @@
|
||||
#include "mpu6000.h"
|
||||
#include "balance.h"
|
||||
#include "hoverboard.h"
|
||||
#include "motor_driver.h"
|
||||
#include "config.h"
|
||||
#include "status.h"
|
||||
#include "safety.h"
|
||||
@ -91,7 +90,7 @@ void SysTick_Handler(void) { HAL_IncTick(); }
|
||||
|
||||
int main(void) {
|
||||
SCB_EnableICache();
|
||||
/* DCache stays ON — MPU Region 0 in usbd_conf.c marks USB buffers non-cacheable. */
|
||||
SCB_DisableDCache(); /* Must be off before USB starts — STM32F7 DCache/USB coherency */
|
||||
checkForBootloader(); /* Check RTC magic BEFORE HAL_Init — must be first thing */
|
||||
HAL_Init();
|
||||
SystemClock_Config();
|
||||
@ -105,6 +104,14 @@ int main(void) {
|
||||
status_init();
|
||||
HAL_Delay(3000); /* Wait for USB host to enumerate */
|
||||
|
||||
/*
|
||||
* IWDG starts after the USB enumeration delay — avoids spurious reset
|
||||
* during the 3s startup hold. Once started, safety_refresh() MUST be
|
||||
* called at least every WATCHDOG_TIMEOUT_MS (50ms) or MCU resets.
|
||||
* IWDG cannot be stopped once started (hardware enforced).
|
||||
*/
|
||||
safety_init();
|
||||
|
||||
/* Init IMU (MPU6000 via SPI1 — mpu6000.c wraps icm42688 + complementary filter) */
|
||||
int imu_ret = mpu6000_init() ? 0 : -1;
|
||||
|
||||
@ -115,18 +122,6 @@ int main(void) {
|
||||
balance_t bal;
|
||||
balance_init(&bal);
|
||||
|
||||
/* Init motor driver */
|
||||
motor_driver_t motors;
|
||||
motor_driver_init(&motors);
|
||||
|
||||
/*
|
||||
* IWDG starts AFTER all peripheral inits — avoids reset during mpu6000_init()
|
||||
* which takes ~510ms (well above the 50ms WATCHDOG_TIMEOUT_MS).
|
||||
* Once started, safety_refresh() MUST be called every WATCHDOG_TIMEOUT_MS
|
||||
* or MCU resets. IWDG cannot be stopped once started (hardware enforced).
|
||||
*/
|
||||
safety_init();
|
||||
|
||||
char buf[256];
|
||||
int len;
|
||||
|
||||
@ -186,21 +181,14 @@ int main(void) {
|
||||
balance_update(&bal, &imu, dt);
|
||||
}
|
||||
|
||||
/* Latch estop on tilt fault or disarm */
|
||||
if (bal.state == BALANCE_TILT_FAULT) {
|
||||
motor_driver_estop(&motors);
|
||||
} else if (bal.state == BALANCE_DISARMED && motors.estop) {
|
||||
motor_driver_estop_clear(&motors);
|
||||
}
|
||||
|
||||
/* Send to hoverboard ESC at 50Hz (every 20ms) */
|
||||
/* Send to hoverboard ESC at 50Hz (every 20ms)
|
||||
* Both wheels get same speed for balance, steer=0 for now */
|
||||
if (now - esc_tick >= 20) {
|
||||
esc_tick = now;
|
||||
if (bal.state == BALANCE_ARMED) {
|
||||
motor_driver_update(&motors, bal.motor_cmd, 0, now);
|
||||
hoverboard_send(bal.motor_cmd, 0);
|
||||
} else {
|
||||
/* Always send zero while disarmed to prevent ESC timeout */
|
||||
motor_driver_update(&motors, 0, 0, now);
|
||||
hoverboard_send(0, 0); /* Always send to prevent ESC timeout */
|
||||
}
|
||||
}
|
||||
|
||||
@ -210,14 +198,13 @@ int main(void) {
|
||||
if (imu_ret == 0) {
|
||||
float err = bal.setpoint - bal.pitch_deg;
|
||||
len = snprintf(buf, sizeof(buf),
|
||||
"{\"p\":%d,\"r\":%d,\"e\":%d,\"ig\":%d,\"m\":%d,\"s\":%d,\"y\":%d}\n",
|
||||
"{\"p\":%d,\"r\":%d,\"e\":%d,\"ig\":%d,\"m\":%d,\"s\":%d}\n",
|
||||
(int)(bal.pitch_deg * 10), /* pitch degrees x10 */
|
||||
(int)(imu.roll * 10), /* roll degrees x10 */
|
||||
(int)(imu.pitch_rate * 10), /* pitch rate °/s x10 */
|
||||
(int)(err * 10), /* PID error x10 */
|
||||
(int)(bal.integral * 10), /* integral x10 (windup monitor) */
|
||||
(int)bal.motor_cmd, /* ESC command -1000..+1000 */
|
||||
(int)bal.state,
|
||||
(int)(imu.yaw * 10)); /* yaw degrees x10 (gyro-integrated, drifts) */
|
||||
(int)bal.state);
|
||||
} else {
|
||||
len = snprintf(buf, sizeof(buf), "{\"err\":%d}\n", imu_ret);
|
||||
}
|
||||
|
||||
@ -1,58 +0,0 @@
|
||||
#include "motor_driver.h"
|
||||
#include "hoverboard.h"
|
||||
#include "config.h"
|
||||
#include <stdlib.h>
|
||||
|
||||
void motor_driver_init(motor_driver_t *m) {
|
||||
m->steer_actual = 0;
|
||||
m->estop = false;
|
||||
}
|
||||
|
||||
void motor_driver_update(motor_driver_t *m,
|
||||
int16_t balance_cmd,
|
||||
int16_t steer_cmd,
|
||||
uint32_t now) {
|
||||
static uint32_t s_last_tick = 0;
|
||||
|
||||
/* Delta-time for ramp (cap at 100ms to handle first call / long gaps).
|
||||
* Always update tick so ramp starts fresh when estop clears. */
|
||||
int32_t dt_ms = (int32_t)(now - s_last_tick);
|
||||
if (dt_ms > 100) dt_ms = 100;
|
||||
s_last_tick = now;
|
||||
|
||||
/* Emergency stop: send zero, hold latch */
|
||||
if (m->estop) {
|
||||
hoverboard_send(0, 0);
|
||||
return;
|
||||
}
|
||||
|
||||
/* Ramp steer toward target at MOTOR_STEER_RAMP_RATE counts/ms */
|
||||
int32_t max_step = (int32_t)MOTOR_STEER_RAMP_RATE * dt_ms;
|
||||
int32_t steer_error = (int32_t)steer_cmd - (int32_t)m->steer_actual;
|
||||
if (steer_error > max_step) steer_error = max_step;
|
||||
else if (steer_error < -max_step) steer_error = -max_step;
|
||||
m->steer_actual = (int16_t)((int32_t)m->steer_actual + steer_error);
|
||||
|
||||
/* Headroom clamp: ensure |speed| + |steer| <= MOTOR_CMD_MAX
|
||||
* Reduce steer (not balance) to preserve PID authority. */
|
||||
int32_t speed = (int32_t)balance_cmd;
|
||||
int32_t steer = (int32_t)m->steer_actual;
|
||||
int32_t headroom = MOTOR_CMD_MAX - abs((int)speed);
|
||||
if (headroom < 0) headroom = 0;
|
||||
if (steer > headroom) steer = headroom;
|
||||
if (steer < -headroom) steer = -headroom;
|
||||
|
||||
hoverboard_send((int16_t)speed, (int16_t)steer);
|
||||
}
|
||||
|
||||
void motor_driver_estop(motor_driver_t *m) {
|
||||
m->estop = true;
|
||||
m->steer_actual = 0;
|
||||
/* Don't call hoverboard_send here — caller must drive sends via
|
||||
* motor_driver_update() at the normal 50Hz ESC rate to avoid
|
||||
* flooding the UART with 1kHz zero packets. */
|
||||
}
|
||||
|
||||
void motor_driver_estop_clear(motor_driver_t *m) {
|
||||
m->estop = false;
|
||||
}
|
||||
@ -27,16 +27,12 @@
|
||||
|
||||
/* Filter state */
|
||||
static float s_pitch = 0.0f;
|
||||
static float s_roll = 0.0f;
|
||||
static float s_yaw = 0.0f;
|
||||
static uint32_t s_last_tick = 0;
|
||||
|
||||
bool mpu6000_init(void) {
|
||||
int ret = icm42688_init();
|
||||
if (ret == 0) {
|
||||
s_pitch = 0.0f;
|
||||
s_roll = 0.0f;
|
||||
s_yaw = 0.0f;
|
||||
s_last_tick = HAL_GetTick();
|
||||
}
|
||||
return (ret == 0);
|
||||
@ -56,43 +52,34 @@ void mpu6000_read(IMUData *data) {
|
||||
|
||||
/* Convert raw to physical units */
|
||||
float ax = raw.ax * ACCEL_SCALE; /* g */
|
||||
float ay = raw.ay * ACCEL_SCALE; /* g */
|
||||
float az = raw.az * ACCEL_SCALE; /* g */
|
||||
|
||||
/*
|
||||
* Gyro axes with CW270 alignment:
|
||||
* pitch rate = gx, roll rate = gy, yaw rate = gz
|
||||
* Signs may need inverting depending on mounting orientation.
|
||||
* Gyro pitch axis with CW270 alignment: pitch rate = gx.
|
||||
* Sign may need inverting depending on mounting orientation —
|
||||
* verify during bench test (positive nose-up should be positive).
|
||||
*/
|
||||
float gyro_pitch_rate = raw.gx * GYRO_SCALE; /* °/s */
|
||||
float gyro_roll_rate = raw.gy * GYRO_SCALE; /* °/s */
|
||||
float gyro_yaw_rate = raw.gz * GYRO_SCALE; /* °/s */
|
||||
|
||||
/*
|
||||
* Accel-derived pitch and roll (degrees).
|
||||
* CW270 alignment: pitch = atan2(ax, az), roll = atan2(ay, az).
|
||||
* Valid while total accel ≈ 1g (low linear acceleration).
|
||||
* Accel pitch angle (degrees).
|
||||
* CW270 alignment: pitch = atan2(ax, az).
|
||||
* Valid while ax² + az² ≈ 1g (i.e., low linear acceleration).
|
||||
*/
|
||||
float accel_pitch = atan2f(ax, az) * (180.0f / 3.14159265358979f);
|
||||
float accel_roll = atan2f(ay, az) * (180.0f / 3.14159265358979f);
|
||||
|
||||
/*
|
||||
* Complementary filter for pitch and roll:
|
||||
* angle = α * (angle + ω*dt) + (1−α) * accel_angle
|
||||
* Gyro integration tracks fast dynamics; accel corrects drift.
|
||||
* Complementary filter:
|
||||
* pitch = α * (pitch + ω*dt) + (1−α) * accel_pitch
|
||||
*
|
||||
* Gyro integration tracks fast dynamics; accel correction
|
||||
* prevents long-term drift.
|
||||
*/
|
||||
s_pitch = COMP_ALPHA * (s_pitch + gyro_pitch_rate * dt)
|
||||
+ (1.0f - COMP_ALPHA) * accel_pitch;
|
||||
s_roll = COMP_ALPHA * (s_roll + gyro_roll_rate * dt)
|
||||
+ (1.0f - COMP_ALPHA) * accel_roll;
|
||||
|
||||
/* Yaw: pure gyro integration — no accel correction, drifts over time */
|
||||
s_yaw += gyro_yaw_rate * dt;
|
||||
|
||||
data->pitch = s_pitch;
|
||||
data->pitch_rate = gyro_pitch_rate;
|
||||
data->roll = s_roll;
|
||||
data->yaw = s_yaw;
|
||||
data->accel_x = ax;
|
||||
data->accel_z = az;
|
||||
}
|
||||
|
||||
@ -16,9 +16,8 @@
|
||||
|
||||
/* IWDG prescaler 32 → LSI(40kHz)/32 = 1250 ticks/sec → 0.8ms/tick */
|
||||
#define IWDG_PRESCALER IWDG_PRESCALER_32
|
||||
/* Integer formula: timeout_ms * LSI_HZ / (prescaler * 1000)
|
||||
* = WATCHDOG_TIMEOUT_MS * 40000 / (32 * 1000) = WATCHDOG_TIMEOUT_MS * 40 / 32 */
|
||||
#define IWDG_RELOAD (WATCHDOG_TIMEOUT_MS * 40UL / 32UL)
|
||||
#define IWDG_TICK_MS (32.0f / 40.0f) /* 0.8ms per tick */
|
||||
#define IWDG_RELOAD ((uint32_t)(WATCHDOG_TIMEOUT_MS / IWDG_TICK_MS))
|
||||
|
||||
#if IWDG_RELOAD > 4095
|
||||
# error "WATCHDOG_TIMEOUT_MS too large for IWDG_PRESCALER_32 — increase prescaler"
|
||||
|
||||
@ -49,14 +49,14 @@
|
||||
<h1>⚡ SALTYLAB <span id="state-badge" class="state-disarmed">DISARMED</span></h1>
|
||||
<div class="stat"><span class="label">PITCH</span> <span class="val" id="v-pitch">--</span>°</div>
|
||||
<div class="stat"><span class="label">ROLL</span> <span class="val" id="v-roll">--</span>°</div>
|
||||
<div class="stat"><span class="label">YAW</span> <span class="val" id="v-yaw">--</span>°</div>
|
||||
<div class="stat"><span class="label">MOTOR</span> <span class="val" id="v-motor">--</span></div>
|
||||
<div class="stat"><span class="label">ACCEL</span> <span class="val" id="v-accel">--</span></div>
|
||||
<div class="stat"><span class="label">GYRO</span> <span class="val" id="v-gyro">--</span></div>
|
||||
<div class="stat"><span class="label">HZ</span> <span class="val" id="v-hz">--</span></div>
|
||||
<div>
|
||||
<button class="btn" id="connect-btn" onclick="toggleSerial()">CONNECT USB</button>
|
||||
<button class="btn" id="arm-btn" onclick="toggleArm()">ARM</button>
|
||||
<button class="btn" id="dfu-btn" onclick="enterDFU()">DFU</button>
|
||||
<button class="btn" id="yaw-btn" onclick="resetYaw()" style="background:#335533;display:none">YAW RESET</button>
|
||||
</div>
|
||||
<div id="status">WebSerial ready</div>
|
||||
</div>
|
||||
@ -170,34 +170,52 @@ boardGroup.add(arrow);
|
||||
scene.add(boardGroup);
|
||||
|
||||
// --- State ---
|
||||
let targetPitch = 0, targetRoll = 0, targetYaw = 0;
|
||||
let currentPitch = 0, currentRoll = 0, currentYaw = 0;
|
||||
let yawOffset = 0; // subtracted from firmware yaw for reset-to-zero
|
||||
let targetPitch = 0, targetRoll = 0;
|
||||
let currentPitch = 0, currentRoll = 0;
|
||||
|
||||
// Complementary filter (client-side, for roll — firmware sends pitch)
|
||||
let roll = 0, lastTime = 0;
|
||||
|
||||
const stateNames = ['DISARMED', 'ARMED', 'TILT FAULT'];
|
||||
const stateClasses = ['state-disarmed', 'state-armed', 'state-fault'];
|
||||
|
||||
window.updateIMU = function(data) {
|
||||
// Firmware sends: p=pitch×10, r=roll×10, y=yaw×10, m=motor, s=state
|
||||
const pitch = (data.p !== undefined) ? data.p / 10.0 : 0;
|
||||
const roll = (data.r !== undefined) ? data.r / 10.0 : 0;
|
||||
const yawRaw = (data.y !== undefined) ? data.y / 10.0 : 0;
|
||||
const yaw = yawRaw - yawOffset;
|
||||
const ax = data.ax || 0, ay = data.ay || 0, az = data.az || 0;
|
||||
const gx = data.gx || 0, gy = data.gy || 0, gz = data.gz || 0;
|
||||
|
||||
// Firmware sends pitch as p (x10), motor as m, state as s
|
||||
const fwPitch = (data.p !== undefined) ? data.p / 10.0 : null;
|
||||
const motorCmd = data.m || 0;
|
||||
const state = data.s || 0;
|
||||
|
||||
// Three.js rotation targets (radians):
|
||||
// pitch → rotation.x (tipping forward/back around left-right axis)
|
||||
// roll → rotation.z (banking left/right around forward axis)
|
||||
// yaw → rotation.y (spinning on vertical axis)
|
||||
const now = performance.now();
|
||||
const dt = lastTime ? (now - lastTime) / 1000 : 0.02;
|
||||
lastTime = now;
|
||||
|
||||
// Use firmware pitch if available, otherwise compute from accel
|
||||
let pitch;
|
||||
if (fwPitch !== null) {
|
||||
pitch = fwPitch;
|
||||
} else {
|
||||
pitch = Math.atan2(-ax, Math.sqrt(ay*ay + az*az)) * 180 / Math.PI;
|
||||
}
|
||||
|
||||
// Roll from client-side complementary filter
|
||||
const accelRoll = Math.atan2(ay, az) * 180 / Math.PI;
|
||||
if (Math.abs(roll) < 0.01) {
|
||||
roll = accelRoll;
|
||||
} else {
|
||||
roll = 0.98 * (roll + (gx / 131.0) * dt) + 0.02 * accelRoll;
|
||||
}
|
||||
|
||||
targetPitch = pitch * Math.PI / 180;
|
||||
targetRoll = roll * Math.PI / 180;
|
||||
targetYaw = yaw * Math.PI / 180;
|
||||
|
||||
document.getElementById('v-pitch').textContent = pitch.toFixed(1);
|
||||
document.getElementById('v-roll').textContent = roll.toFixed(1);
|
||||
document.getElementById('v-yaw').textContent = yaw.toFixed(1);
|
||||
document.getElementById('v-motor').textContent = motorCmd;
|
||||
document.getElementById('v-accel').textContent = `${ax},${ay},${az}`;
|
||||
document.getElementById('v-gyro').textContent = `${gx},${gy},${gz}`;
|
||||
|
||||
// Pitch bar: center at 50%, ±90°
|
||||
document.getElementById('bar-pitch').style.width = ((pitch + 90) / 180 * 100) + '%';
|
||||
@ -236,21 +254,12 @@ function animate() {
|
||||
requestAnimationFrame(animate);
|
||||
currentPitch += (targetPitch - currentPitch) * 0.15;
|
||||
currentRoll += (targetRoll - currentRoll) * 0.15;
|
||||
currentYaw += (targetYaw - currentYaw) * 0.15;
|
||||
boardGroup.rotation.x = currentPitch;
|
||||
boardGroup.rotation.z = currentRoll;
|
||||
boardGroup.rotation.y = currentYaw;
|
||||
renderer.render(scene, camera);
|
||||
}
|
||||
animate();
|
||||
|
||||
window.resetYaw = function() {
|
||||
// Capture current firmware yaw as new zero reference
|
||||
const currentFirmwareYaw = targetYaw * 180 / Math.PI + yawOffset;
|
||||
yawOffset = currentFirmwareYaw;
|
||||
targetYaw = 0;
|
||||
};
|
||||
|
||||
window.addEventListener('resize', () => {
|
||||
camera.aspect = window.innerWidth / window.innerHeight;
|
||||
camera.updateProjectionMatrix();
|
||||
@ -284,7 +293,6 @@ window.toggleSerial = async function() {
|
||||
document.getElementById('connect-btn').classList.remove('connected');
|
||||
document.getElementById('arm-btn').style.display = 'none';
|
||||
document.getElementById('dfu-btn').style.display = 'none';
|
||||
document.getElementById('yaw-btn').style.display = 'none';
|
||||
document.getElementById('status').textContent = 'Disconnected';
|
||||
return;
|
||||
}
|
||||
@ -298,7 +306,6 @@ window.toggleSerial = async function() {
|
||||
document.getElementById('connect-btn').classList.add('connected');
|
||||
document.getElementById('arm-btn').style.display = 'inline-block';
|
||||
document.getElementById('dfu-btn').style.display = 'inline-block';
|
||||
document.getElementById('yaw-btn').style.display = 'inline-block';
|
||||
document.getElementById('status').textContent = 'Connected — streaming';
|
||||
|
||||
readLoop();
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user