BT410 Phase 5.3.2: DRIVABLE mech -- authentic control interpretation + locomotion

The mech now drives from the authentic control chain (faithful route). Verified headlessly: full throttle -> walks straight at top speed (30 u/s) along heading; throttle+turn -> walks a circle (radius = speed/turnRate); neutral -> holds pose; zero Fail. MechControlsMapper::InterpretControls (mechmppr.cpp @004afd10) -- installed as the mapper's per-frame Performance (roster slot 0, ticks before the mech): - speedDemand = topSpeed*throttle*fwdScale (reverse inverts); soft stick (square) / pedal (cube) response; Basic=stick turn, Std/Vet=pedal turn; speed clamped while turning hard. Reads owner stride via Mech accessors. - BT_FORCE_THROTTLE/BT_FORCE_TURN dev hooks for headless verification. Mech::Simulate drive -- consumes the mapper demands: - accelerate currentBodySpeed toward speedDemand (maxBodyAcceleration); - authTurnRate = lerp(walkingTurnRate, runningTurnRate) by ground speed + over-run falloff (mech4.cpp master-perf @0x4aa3d3); - integrate heading via Quaternion::Add(prevPose, (0,turn*rate*dt,0)); - facing = world -Z basis (GetFromAxis(Z_Axis)); worldLinearVelocity = facing*spd; - integrate position (increment-1 core). MECH.HPP: 9 named locomotion members out of reservedState (now [211]) -- walking/runningTurnRate, reverse/walkStrideLength, reverseSpeedMax, forwardThrottleScale, maxBodyAcceleration, body/currentBodySpeed. BRING-UP DEFAULTS (authentic values come from the model resource + LoadLocomotionClips -- next refinement). Deferred: gait-clip-exact advance + leg anim (needs animation subsystem/renderer), model-resource sourcing, terrain drop, torso/free-look aim, telemetry filters. Co-Authored-By: Claude Fable 5 <noreply@anthropic.com>
2026-07-21 14:46:30 -05:00
parent 859529d6f2
commit ebf0d8c23f
6 changed files with 398 additions and 12 deletions
@@ -15,6 +15,10 @@
 #	include <mechmppr.hpp>
 #endif

+#if !defined(MECH_HPP)
+#	include <mech.hpp>
+#endif
+
 Derivation
 	MechControlsMapper::ClassDerivations(
 		Subsystem::ClassDerivations,
@@ -117,6 +121,11 @@ MechControlsMapper::MechControlsMapper(
 			reserved[i] = 0;
 		}
 	}
+	//
+	// Install the per-frame control-interpretation Performance.
+	//
+	SetPerformance(&MechControlsMapper::InterpretControls);
+
 	if (getenv("BT_MECH_LOG"))
 	{
 		DEBUG_STREAM << "[mapper] ctor id=" << subsystem_ID
@@ -131,6 +140,124 @@ MechControlsMapper::~MechControlsMapper()
 {
 }

+//
+//#############################################################################
+// InterpretControls -- the mapper's per-frame Performance.  Reads the raw input
+// attributes (the engine controls push refreshes throttle/stick/pedals/buttons
+// from the RIO/keyboard before this runs) and publishes the locomotion demands
+// (speedDemand in world-u/s, turnDemand [-1..1]) the mech drive consumes.
+//
+// Reconstructs the authentic demand math (mechmppr.cpp @004afd10):
+//   * throttle -> forward speed: speedDemand = topSpeed * throttle * fwdScale
+//     (reverse thrust inverts and drops the forward scale);
+//   * soft stick response: square the yaw (sign preserved), cube the pedals;
+//   * Basic: stick yaw = turn; Standard/Veteran: pedals = turn;
+//   * speed is clamped down while turning hard (max_turn ramp).
+//
+// DEV hook (headless verification, no RIO/keyboard): BT_FORCE_THROTTLE /
+// BT_FORCE_TURN override the pushed raw inputs so the demand math + mech drive
+// are exercisable without hardware.  The torso-aim / free-look interpretation
+// (torso analog axes, look/eyepoint commit) is the aiming wave, deferred.
+//#############################################################################
+//
+void
+	MechControlsMapper::InterpretControls(Scalar time_slice)
+{
+	Check(this);
+
+	Mech *mech = GetMech();
+	Check(mech);
+
+	//
+	// DEV forced-input override.
+	//
+	{
+		const char *force_throttle = getenv("BT_FORCE_THROTTLE");
+		if (force_throttle != NULL)
+		{
+			Scalar t = (Scalar)atof(force_throttle);
+			throttlePosition = (t >= 0.0f) ? t : -t;
+			reverseThrust    = (t < 0.0f) ? 1 : 0;
+		}
+		const char *force_turn = getenv("BT_FORCE_TURN");
+		if (force_turn != NULL)
+		{
+			stickPosition.x = (Scalar)atof(force_turn);
+		}
+	}
+
+	Scalar topSpeed  = mech->GetReverseStrideLength();
+	Scalar walkSpeed = mech->GetWalkStrideLength();
+
+	//
+	// Throttle -> forward speed demand.
+	//
+	if (reverseThrust < 1)
+	{
+		speedDemand = topSpeed * throttlePosition * mech->GetForwardThrottleScale();
+	}
+	else
+	{
+		speedDemand = -topSpeed * throttlePosition;
+	}
+
+	//
+	// Soft response: square the stick yaw (sign preserved), cube the pedals.
+	//
+	Scalar stick_x = stickPosition.x * stickPosition.x;
+	if (stickPosition.x < 0.0f)
+	{
+		stick_x = -stick_x;
+	}
+	Scalar pedal_3 = pedalsPosition * pedalsPosition * pedalsPosition;
+
+	turnDemand = 0.0f;
+
+	if (controlMode == BasicMode)
+	{
+		turnDemand = stick_x;
+	}
+	else
+	{
+		turnDemand = pedal_3;
+	}
+
+	//
+	// Clamp forward speed down while turning hard (except VeteranMode, which has
+	// no speed-dependent turn clamp).  max_turn ramps from topSpeed (no turn) to
+	// walkSpeed (full turn).
+	//
+	if (controlMode != VeteranMode)
+	{
+		Scalar turn_mag = (turnDemand < 0.0f) ? -turnDemand : turnDemand;
+		if (turn_mag > 0.001f)
+		{
+			Scalar max_turn = (topSpeed - walkSpeed) * (1.0f - turn_mag) + walkSpeed;
+			if (speedDemand >  max_turn) speedDemand =  max_turn;
+			if (speedDemand < -max_turn) speedDemand = -max_turn;
+		}
+	}
+
+	if (getenv("BT_MECH_LOG"))
+	{
+		static Scalar reportAccum = 0.0f;
+		reportAccum += time_slice;
+		if (reportAccum >= 1.0f)
+		{
+			reportAccum = 0.0f;
+			DEBUG_STREAM << "[mppr] thr=" << throttlePosition
+				<< " rev=" << reverseThrust
+				<< " stickX=" << stickPosition.x
+				<< " mode=" << controlMode
+				<< " -> speedDemand=" << speedDemand
+				<< " turnDemand=" << turnDemand
+				<< endl << flush;
+		}
+	}
+
+	Check_Fpu();
+}
+
 Logical
 	MechControlsMapper::TestInstance() const
 {