diff --git a/CommonLib/xil/driver.py b/CommonLib/xil/driver.py index 7a4df63d..ae2cc656 100644 --- a/CommonLib/xil/driver.py +++ b/CommonLib/xil/driver.py @@ -15,9 +15,12 @@ a coupling. """ +from collections import deque + class RobotDriver(object): - """PI on speed error, producing one pedal split into throttle and brake. + """PI on speed error (``mode='speed'``) or on acceleration error + (``mode='accel'``), producing one pedal split into throttle and brake. One axis, not two: positive is throttle, negative is brake, and they are never both non-zero. That is what a driver does and what a VehicleControl @@ -27,7 +30,13 @@ class RobotDriver(object): def __init__(self, kp=0.45, ki=0.25, max_throttle=1.0, max_brake=1.0, standstill_ref_mps=0.05, standstill_brake=0.3, max_accel_mps2=None, max_decel_mps2=None, - stop_speed_mps=0.0, stop_accel_mps2=0.0): + stop_speed_mps=0.0, stop_accel_mps2=0.0, delay_s=0.0, + actuator=None, mode='speed', kp_accel=0.05, ki_accel=0.5, + accel_filter_s=0.1): + if mode not in ('speed', 'accel'): + raise ValueError("mode must be 'speed' or 'accel', got %r" % (mode,)) + #: 'speed' chases v_ref; 'accel' chases a_ref and ignores v_ref. + self.mode = mode self.kp = kp self.ki = ki self.max_throttle = max_throttle @@ -58,8 +67,24 @@ def __init__(self, kp=0.45, ki=0.25, max_throttle=1.0, max_brake=1.0, #: less than stop_accel_mps2 (openpilot: 0.3 and 0.1). 0.0 is off. self.stop_speed_mps = stop_speed_mps self.stop_accel_mps2 = stop_accel_mps2 + #: Transport delay between the robot's command and the pedal, in s; 0.0 is + #: off. Held per step, so it resolves to whole steps of the caller's dt. + self.delay_s = delay_s + #: Your pedal dynamics, after the delay: actuator(throttle, brake, dt) + #: -> (throttle, brake). Its reset(), if it has one, runs with ours. + self.actuator = actuator + #: Accel mode: PI gains on a_ref - a_meas, no feedforward. + self.kp_accel = kp_accel + self.ki_accel = ki_accel + #: Time constant of the first-order filter on dv/dt that gives a_meas. + self.accel_filter_s = accel_filter_s + self.a_meas = 0.0 + self._v_prev = None self.integral = 0.0 self.pedal = 0.0 + self._clock = 0.0 + self._sent = deque() + self._applied = (0.0, 0.0) #: How far the ramp may sit from the measured speed. Anti-windup for the #: setpoint itself: without it, a ramp advancing past a vehicle that cannot @@ -70,14 +95,42 @@ def reset(self): self.integral = 0.0 self.pedal = 0.0 self._ramp = None + self._clock = 0.0 + self._sent.clear() + self._applied = (0.0, 0.0) + self.a_meas = 0.0 + self._v_prev = None + if hasattr(self.actuator, 'reset'): + self.actuator.reset() def step(self, v_ref, v_measured, dt, a_ref=None): - """Returns (throttle, brake), each in [0, 1], never both positive. + """Returns (throttle, brake), each in [0, 1], never both positive, + unless an ``actuator`` says otherwise. - ``a_ref`` is the acceleration command, read only by the hold below. + ``a_ref`` is the acceleration command: the reference in accel mode, + otherwise read only by the hold. With ``delay_s`` set, this is the command of ``delay_s`` ago, and + (0, 0) until there is one; ``pedal`` stays the command just made. """ if dt <= 0.0: raise ValueError('dt must be positive') + command = self._command(v_ref, v_measured, dt, a_ref) + if self.delay_s > 0.0: + self._sent.append((self._clock, command)) + due = self._clock - self.delay_s + 1e-9 + self._clock += dt + while self._sent and self._sent[0][0] <= due: + self._applied = self._sent.popleft()[1] + command = self._applied + if self.actuator is not None: + command = self.actuator(command[0], command[1], dt) + return command + + def _command(self, v_ref, v_measured, dt, a_ref): + if self.mode == 'accel' and a_ref is None: + raise ValueError("mode 'accel' needs a_ref") + if self._v_prev is not None: + self.a_meas += ((v_measured - self._v_prev) / dt - self.a_meas) * dt / (self.accel_filter_s + dt) + self._v_prev = v_measured if a_ref is not None and v_measured < self.stop_speed_mps \ and a_ref < self.stop_accel_mps2: @@ -85,6 +138,9 @@ def step(self, v_ref, v_measured, dt, a_ref=None): self.pedal = -self.standstill_brake return 0.0, min(self.max_brake, self.standstill_brake) + if self.mode == 'accel': + return self._accel(a_ref, v_measured, dt) + # THE ENVELOPE. A RAMPED setpoint, advanced from its own last value -- # not the incoming reference clamped to the measured speed. # @@ -119,11 +175,23 @@ def step(self, v_ref, v_measured, dt, a_ref=None): self.pedal = -self.standstill_brake return 0.0, min(self.max_brake, self.standstill_brake) - err = v_ref - v_measured - raw = self.kp * err + self.ki * self.integral + return self._pi(v_ref - v_measured, self.kp, self.ki, dt) + + def _accel(self, a_ref, v_measured, dt): + if self.max_accel_mps2 is not None: + a_ref = min(a_ref, self.max_accel_mps2) + if self.max_decel_mps2 is not None: + a_ref = max(a_ref, -self.max_decel_mps2) + if a_ref <= 0.0 and v_measured <= self.standstill_ref_mps: + self.integral = 0.0 + self.pedal = -self.standstill_brake + return 0.0, min(self.max_brake, self.standstill_brake) + return self._pi(a_ref - self.a_meas, self.kp_accel, self.ki_accel, dt) + + def _pi(self, err, kp, ki, dt): + raw = kp * err + ki * self.integral if -self.max_brake < raw < self.max_throttle: self.integral += err * dt # anti-windup: freeze at the rail self.pedal = max(-self.max_brake, - min(self.max_throttle, - self.kp * err + self.ki * self.integral)) + min(self.max_throttle, kp * err + ki * self.integral)) return (self.pedal, 0.0) if self.pedal >= 0.0 else (0.0, -self.pedal) diff --git a/tests/Python/unit/test_xil_dyno.py b/tests/Python/unit/test_xil_dyno.py index 6bdd568c..0b924f00 100644 --- a/tests/Python/unit/test_xil_dyno.py +++ b/tests/Python/unit/test_xil_dyno.py @@ -294,6 +294,98 @@ def test_separate_throttle_and_brake_ceilings(): assert thr == pytest.approx(0.25) +def test_a_delay_applies_the_command_of_that_long_ago(): + plain, late = RobotDriver(), RobotDriver(delay_s=0.1) + sent = [plain.step(20.0, 0.0, 0.05) for _ in range(6)] + got = [late.step(20.0, 0.0, 0.05) for _ in range(6)] + assert got[:2] == [(0.0, 0.0)] * 2 + assert got[2:] == sent[:4] + assert late.pedal == plain.pedal + + +def test_reset_empties_the_delay_line(): + driver = RobotDriver(delay_s=0.1) + for _ in range(5): + driver.step(20.0, 0.0, 0.05) + driver.reset() + assert driver.step(20.0, 0.0, 0.05) == (0.0, 0.0) + + +def test_the_actuator_gets_the_delayed_command_and_has_the_last_word(): + seen = [] + + def half(thr, brk, dt): + seen.append((thr, brk, dt)) + return 0.5 * thr, brk + + plain = RobotDriver() + late = RobotDriver(delay_s=0.05, actuator=half) + sent = [plain.step(20.0, 0.0, 0.05) for _ in range(3)] + got = [late.step(20.0, 0.0, 0.05) for _ in range(3)] + assert seen == [(0.0, 0.0, 0.05)] + [c + (0.05,) for c in sent[:2]] + assert got == [(0.0, 0.0)] + [(0.5 * t, b) for t, b in sent[:2]] + + +def test_reset_resets_the_actuator_if_it_can(): + class Lag(object): + resets = 0 + + def __call__(self, thr, brk, dt): + return thr, brk + + def reset(self): + self.resets += 1 + + lag = Lag() + DynoSim(driver=RobotDriver(actuator=lag)).reset() + assert lag.resets == 1 + + +def test_a_delayed_driver_still_settles_on_the_reference(): + sim = DynoSim(driver=RobotDriver(delay_s=0.2)) + for _ in range(int(60.0 / 0.05)): + state = sim.step(15.0, 0.05) + assert state.speed == pytest.approx(15.0, abs=0.05) + + +def test_an_unknown_mode_is_rejected(): + with pytest.raises(ValueError): + RobotDriver(mode='torque') + + +def test_accel_mode_needs_an_acceleration_command(): + with pytest.raises(ValueError): + RobotDriver(mode='accel').step(5.0, 0.0, 0.05) + + +def test_accel_mode_tracks_a_constant_acceleration(): + sim = DynoSim(driver=RobotDriver(mode='accel')) + speeds = [sim.step(0.0, 0.05, a_ref=1.0).speed for _ in range(160)] + assert speeds[-1] == pytest.approx(8.0, abs=0.3) + assert (speeds[-1] - speeds[-81]) / 4.0 == pytest.approx(1.0, abs=0.02) + + +def test_accel_mode_ignores_the_speed_reference(): + a, b = RobotDriver(mode='accel'), RobotDriver(mode='accel') + for k in range(20): + v = 0.1 * k + assert a.step(0.0, v, 0.05, a_ref=0.5) == b.step(30.0, v, 0.05, a_ref=0.5) + + +def test_accel_mode_caps_the_command_at_the_envelope(): + sim = DynoSim(driver=RobotDriver(mode='accel', max_accel_mps2=1.8)) + speeds = [sim.step(0.0, 0.05, a_ref=5.0).speed for _ in range(160)] + assert (speeds[-1] - speeds[-81]) / 4.0 == pytest.approx(1.8, abs=0.05) + + +def test_accel_mode_holds_the_brake_at_rest_when_not_asked_to_move(): + driver = RobotDriver(mode='accel') + for _ in range(20): + thr, brk = driver.step(0.0, 0.0, 0.05, a_ref=0.0) + assert thr == 0.0 and brk == pytest.approx(0.3) + assert driver.integral == 0.0 + + def test_the_bench_can_fail_to_reach_the_reference(): """The gap between asked and achieved is the point of having a plant.""" sim = DynoSim()