+The ``DriveSubsystem`` contains a ``TrapezoidProfile`` and a ``Timer``. The ``startRun`` command factory is used, with the first lambda being run when the command is scheduled. It restarts the timer, and for ``profiledDriveDistance``, it resets the encoders; for ``dynamicProfiledDriveDistance``, it sets the initial distances for the left and right encoders. The second lambda passed into ``startRun`` will repeatedly run while the command is scheduled. Inside the lambda, the timer is used along with a ``kDt`` constant of 0.02 seconds to calculate the current and next states from the ``TrapezoidProfile``. The current state is fed to the "smart" motor controller for PID control, while the current and next state are used to calculate feedforward outputs. The ``until`` decorator is used after the ``startRun`` command factory to end the command when ``isFinished(0)`` returns true, which means that the profile has reached the goal state.
0 commit comments