PAROL6 Firmware (Teensy 4.1)

Motion firmware for the PAROL6 6-axis desktop robot arm. It runs on a Teensy 4.1 and talks to a host PC over micro-ROS (ROS 2 Humble). The PC plans smooth trajectories; the Teensy executes them deterministically and handles homing, limits, the gripper, the E-stop and driver power.

It is the low-level counterpart to the Helyx Commander GUI (NiceGUI + ROS 2

• MoveIt2), which streams the trajectories this firmware plays back.

---

Hardware

Part	Detail
MCU	Teensy 4.1 (600 MHz Cortex-M7)
J1, J2 drivers	TMC5160 over SPI (stealthChop, 1400 mA RMS)
J3, J4, J5, J6 drivers	TMC2209 standalone STEP/DIR
Microstepping	16 µsteps, interpolated to 256 (MicroPlyer)
Gear ratios	J1–J6 = {10, 20, 20, 4, 4, 10}, 3200 steps/rev
Sensors	mechanical limit switches (J2/J3/J5) + inductive GX-F8A (J1/J4/J6)
Safety	hardware E-STOP (latching), 24V rail sense, cooling fan
Gripper	pneumatic 5/2 valve via low-side MOSFET (SOL1)

Motor power (24V/VM) and logic power (3.3V/VCC_IO) are independent: SPI comes up on USB alone, and the coils are enabled only once 24V is detected — so the USB-vs-PSU power-up order doesn't matter and the firmware never needs re-flashing after turning the supply on.

---

Architecture

Director model. The PC sends a whole trajectory in one message (Float32MultiArray on /parol6/traj_full): a flat [N, then per waypoint: pos0..5, vel0..5, t] array (positions in rad, velocities in rad/s). The Teensy buffers it and reconstructs the path with a cubic Hermite interpolator (traj.h). Because the whole path arrives atomically, no waypoints are ever lost mid-figure.

Control in the ISR (~10 kHz). Trajectory sampling and the per-axis moveTo/setSpeed updates run inside the step ISR at CTRL_SAMPLE_US = 100 µs, decoupled from loop()/micro-ROS jitter — no motion micro-stalls.

Per-step PIT scheduler. Steps are generated by a hardware-timer ISR that re-arms the PIT channel to the exact time of the next step (24 MHz), not a fixed tick, so steps aren't quantized → zero jitter, quiet motion. loop() stays free to spin micro-ROS at a high rate.

Homing. Two-touch (fast coarse touch → back off → slow accurate touch), grouped so several axes home in parallel in a collision-free order ({J1,J2,J3}→J4→J6→J5), with a demo fallback for not-yet-calibrated axes and a staged collision-free return to base.

E-STOP. Freezes the arm in place (controlled brake, coils keep holding — no sag) and ignores commands while pressed; micro-ROS stays connected and the position keeps publishing. Engages instantly, releases debounced.

---

ROS 2 interface

Subscriptions

Topic	Type	Purpose
/parol6/traj_full	Float32MultiArray	full trajectory, one message (director model)
/joint_cmds	JointState	legacy JTC setpoint stream (ignored in director mode)
/parol6/set_pose	JointState	calibrate: "I am at position X" (resets counters, no motion)
/parol6/home	Empty	trigger homing (second press cancels)
/parol6/stop	Empty	E-stop / stop, aborts homing
/parol6/gripper	Bool	true = clamp (SOL1 ON)

Publishers

Topic	Type	Purpose
/joint_states_raw	JointState	joint positions, ~20 Hz
/parol6/limit_switches	UInt8	6-bit limit bitmask
/parol6/vm_voltage	Float32	measured 24V rail
/parol6/estop	Bool	E-stop engaged

---

Build & flash

Built with PlatformIO (env teensy41).

pio run -e teensy41            # build
pio run -e teensy41 -t upload  # flash (press the Teensy PROGRAM button if needed)

micro-ROS entity limits are raised in colcon.meta (7 subscriptions need MAX_SUBSCRIPTIONS ≥ 7). After changing it:

pio run -e teensy41 -t clean_microros && pio run -e teensy41

Run a micro-ROS agent on the host to bridge the serial link to ROS 2:

ros2 run micro_ros_agent micro_ros_agent serial --dev /dev/ttyACM0

---

Source layout

File	Contents
src/main.cpp	pins, drivers, control ISR, homing, E-stop, power handshake, micro-ROS
src/traj.h	trajectory buffer + cubic-Hermite sampler (director executor)
src/scurve.h	jerk-limited S-curve profile (dormant, kept behind USE_SCURVE)
src/sensors.h / .cpp	limit/inductive sensor debounce + direction calibration
platformio.ini	build config, libraries, micro-ROS transport

---

Tuning notes

• Per-axis speed/accel that the director path actually runs live in the Helyx app (MAX_JOINT_SPEED / MAX_JOINT_ACCEL); the director ISR runs setSpeed verbatim and is not clamped to NORMAL_SPEED. NORMAL_SPEED here governs homing and the legacy jog path only.
• If a joint loses steps or drifts, lower that joint's acceleration first.
• HOME_AT_LIMIT_RAD, LIMIT_DIRECTION (in sensors.cpp) and HOMING_REAL_ENABLE must be calibrated per axis before enabling real homing.

---

License

MIT — provided "as is", without warranty of any kind. See the license text.