Initial import: robot firmware + PlatformIO two-env scaffold

Robot-side firmware (Teensy 4.0) imported unchanged as extracted.
platformio.ini defines [env:robot] (teensy40) and [env:remote]
(teensy41); scripts/select_src_dir.py points each env at its own
sketch folder so robot/ and remote/ stay Arduino-style sketches.

Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>

2026-current/robot/BodyPose.h

@@ -0,0 +1,36 @@
+#pragma once
+#include "SmoothedValue.h"
+#include "Kinematics.h"
+#include "Config.h"
+
+struct PoseCommand {
+  float roll_deg = 0, pitch_deg = 0, yaw_deg = 0;
+  float x_mm = 0, y_mm = 0, z_mm = 0;
+};
+
+class BodyPose {
+public:
+  void begin();
+  void setCommand(const PoseCommand& c) { command_ = c; }
+  // From RobotIMU stabilization; pass 0,0 when the robot-gyro toggle is off.
+  void setCorrection(float roll_deg, float pitch_deg) {
+    correction_roll_ = roll_deg;
+    correction_pitch_ = pitch_deg;
+  }
+
+  void update(float dt);
+
+  // Applies the current (smoothed) pose transform to a body-frame foot target.
+  FootTarget apply(const FootTarget& in) const;
+
+private:
+  PoseCommand command_;
+  float correction_roll_ = 0, correction_pitch_ = 0;
+
+  SmoothedValue s_roll_{POSE_TILT_MAX_RATE_DEG};
+  SmoothedValue s_pitch_{POSE_TILT_MAX_RATE_DEG};
+  SmoothedValue s_yaw_{POSE_TILT_MAX_RATE_DEG};
+  SmoothedValue s_x_{POSE_OFFSET_MAX_RATE_MM};
+  SmoothedValue s_y_{POSE_OFFSET_MAX_RATE_MM};
+  SmoothedValue s_z_{POSE_OFFSET_MAX_RATE_MM};
+};