37 lines
1.5 KiB
Python
37 lines
1.5 KiB
Python
"""Physics-clock command envelope; perception remains asynchronous.
|
|
|
|
Limit wheel-surface acceleration without delaying a safety reduction. Scaling
|
|
both sides together preserves the planner's requested curvature. Stops bypass
|
|
the ramp, as required by the already qualified collision/braking envelope.
|
|
This is a simulation actuator, not a VESC controller or a motor calibration.
|
|
"""
|
|
|
|
CONTROL_PROFILE = {
|
|
"clock": "physics-pre-step",
|
|
"rate_hz": 60,
|
|
"wheel_surface_acceleration_mps2": 0.2,
|
|
"safety_reductions": "immediate",
|
|
"authority": "simulation-only",
|
|
}
|
|
|
|
|
|
class DriveEnvelope:
|
|
def __init__(self, track_width=0.9, acceleration=0.2):
|
|
self.half_track = track_width / 2
|
|
self.acceleration = acceleration
|
|
self.wheels = (0.0, 0.0)
|
|
|
|
def step(self, velocity, yaw_rate, dt, *, stop=False):
|
|
requested = (velocity - yaw_rate * self.half_track, velocity + yaw_rate * self.half_track)
|
|
if stop or max(map(abs, requested)) < 1e-8:
|
|
self.wheels = (0.0, 0.0)
|
|
return 0.0, 0.0
|
|
scale = 1.0
|
|
for old, new in zip(self.wheels, requested, strict=True):
|
|
# Reversal starts from zero; never retain motion in the old direction.
|
|
prior = abs(old) if old * new >= 0 else 0.0
|
|
if abs(new) > prior:
|
|
scale = min(scale, (prior + self.acceleration * max(0.0, dt)) / abs(new))
|
|
self.wheels = tuple(value * scale for value in requested)
|
|
return velocity * scale, yaw_rate * scale
|