Rewrite TrapezoidProfile internals to use phase space formulation

[calcmogul]

This should fix several corner cases and hard-to-reproduce bugs in the current implementation, and make the internals easier to understand and maintain.

The gist of the implementation is:

    def calculate(self, state: State, goal: State, dt: float):
        """Returns the next acceleration of the double integrator.

        Keyword arguments:
        state -- the current position and velocity
        goal -- the desired position and velocity
        dt -- timestep duration in seconds
        """
        q = state.position
        q_ref = goal.position

        qdot = state.velocity
        qdot_ref = goal.velocity

        x = [q, qdot]

        u_max = self.constraints.max_acceleration

        # TODO: To avoid phase space chatter caused by discretization error,
        # find crossover point analytically, then use that to determine phase
        # space position instead of rolling dynamics forward

        # If the position and velocity are within the distance and velocity the
        # controller can travel in one timestep respectively, turn the
        # controller off
        if (
            abs(q_ref - q) <= 0.5 * u_max * dt**2
            and abs(qdot_ref - qdot) <= u_max * dt
        ):
            return TrapezoidProfileFilter.State(q_ref, qdot_ref)

        # c₊ = qᵣ − 0.5 / u_max q̇ᵣ²
        c_plus = q_ref - 0.5 / u_max * qdot_ref**2

        # c₋ = qᵣ + 0.5 / u_max q̇ᵣ²
        c_minus = q_ref + 0.5 / u_max * qdot_ref**2

        # if (q̇ < q̇ᵣ and q ≤ 0.5 / u_max q̇² + c₊) or
        #    (q̇ ≥ q̇ᵣ and q < −0.5 / u_max q̇² + c₋)
        if (qdot < qdot_ref and q <= 0.5 / u_max * qdot**2 + c_plus) or (
            qdot >= qdot_ref and q < -0.5 / u_max * qdot**2 + c_minus
        ):
            # Enforce qdot maximum
            if qdot < self.constraints.max_velocity:
                u = u_max
            else:
                u = 0.0
        else:
            # Enforce qdot minimum
            if qdot > -self.constraints.max_velocity:
                u = -u_max
            else:
                u = 0.0

        # qₖ₊₁ = qₖ + Tq̇ₖ + 1/2 T²q̈ₖ
        # q̇ₖ₊₁ =       q̇ₖ +      Tq̈ₖ
        return TrapezoidProfileFilter.State(
            x[0] + dt * x[1] + 0.5 * dt * dt * u, x[1] + dt * u
        )

but it should use an exact time domain intercept instead of doing forward rollout with the optimal continuous input, like it does now.

[calcmogul]

Relevant implementation by FRC team 100 based on https://arxiv.org/pdf/2210.01744:

https://github.com/Team100/all25/blob/1d97a30da424a297200504498ff8eb23ce8bb248/lib/src/main/java/org/team100/lib/profile/incremental/TrapezoidProfile100.java