001// Copyright (c) FIRST and other WPILib contributors.
002// Open Source Software; you can modify and/or share it under the terms of
003// the WPILib BSD license file in the root directory of this project.
004
005package edu.wpi.first.math.controller;
006
007import edu.wpi.first.math.MatBuilder;
008import edu.wpi.first.math.Nat;
009import edu.wpi.first.math.numbers.N2;
010import edu.wpi.first.math.system.LinearSystem;
011
012/**
013 * Filters the provided voltages to limit a differential drive's linear and angular acceleration.
014 *
015 * <p>The differential drive model can be created via the functions in {@link
016 * edu.wpi.first.math.system.plant.LinearSystemId}.
017 */
018public class DifferentialDriveAccelerationLimiter {
019  private final LinearSystem<N2, N2, N2> m_system;
020  private final double m_trackwidth;
021  private final double m_minLinearAccel;
022  private final double m_maxLinearAccel;
023  private final double m_maxAngularAccel;
024
025  /**
026   * Constructs a DifferentialDriveAccelerationLimiter.
027   *
028   * @param system The differential drive dynamics.
029   * @param trackwidth The distance between the differential drive's left and right wheels in
030   *     meters.
031   * @param maxLinearAccel The maximum linear acceleration in meters per second squared.
032   * @param maxAngularAccel The maximum angular acceleration in radians per second squared.
033   */
034  public DifferentialDriveAccelerationLimiter(
035      LinearSystem<N2, N2, N2> system,
036      double trackwidth,
037      double maxLinearAccel,
038      double maxAngularAccel) {
039    this(system, trackwidth, -maxLinearAccel, maxLinearAccel, maxAngularAccel);
040  }
041
042  /**
043   * Constructs a DifferentialDriveAccelerationLimiter.
044   *
045   * @param system The differential drive dynamics.
046   * @param trackwidth The distance between the differential drive's left and right wheels in
047   *     meters.
048   * @param minLinearAccel The minimum (most negative) linear acceleration in meters per second
049   *     squared.
050   * @param maxLinearAccel The maximum (most positive) linear acceleration in meters per second
051   *     squared.
052   * @param maxAngularAccel The maximum angular acceleration in radians per second squared.
053   * @throws IllegalArgumentException if minimum linear acceleration is greater than maximum linear
054   *     acceleration
055   */
056  public DifferentialDriveAccelerationLimiter(
057      LinearSystem<N2, N2, N2> system,
058      double trackwidth,
059      double minLinearAccel,
060      double maxLinearAccel,
061      double maxAngularAccel) {
062    if (minLinearAccel > maxLinearAccel) {
063      throw new IllegalArgumentException("maxLinearAccel must be greater than minLinearAccel");
064    }
065    m_system = system;
066    m_trackwidth = trackwidth;
067    m_minLinearAccel = minLinearAccel;
068    m_maxLinearAccel = maxLinearAccel;
069    m_maxAngularAccel = maxAngularAccel;
070  }
071
072  /**
073   * Returns the next voltage pair subject to acceleration constraints.
074   *
075   * @param leftVelocity The left wheel velocity in meters per second.
076   * @param rightVelocity The right wheel velocity in meters per second.
077   * @param leftVoltage The unconstrained left motor voltage.
078   * @param rightVoltage The unconstrained right motor voltage.
079   * @return The constrained wheel voltages.
080   */
081  public DifferentialDriveWheelVoltages calculate(
082      double leftVelocity, double rightVelocity, double leftVoltage, double rightVoltage) {
083    var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(leftVoltage, rightVoltage);
084
085    // Find unconstrained wheel accelerations
086    var x = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(leftVelocity, rightVelocity);
087    var dxdt = m_system.getA().times(x).plus(m_system.getB().times(u));
088
089    // Convert from wheel accelerations to linear and angular accelerations
090    //
091    // a = (dxdt(0) + dx/dt(1)) / 2
092    //   = 0.5 dxdt(0) + 0.5 dxdt(1)
093    //
094    // α = (dxdt(1) - dxdt(0)) / trackwidth
095    //   = -1/trackwidth dxdt(0) + 1/trackwidth dxdt(1)
096    //
097    // [a] = [          0.5           0.5][dxdt(0)]
098    // [α]   [-1/trackwidth  1/trackwidth][dxdt(1)]
099    //
100    // accels = M dxdt where M = [0.5, 0.5; -1/trackwidth, 1/trackwidth]
101    var M =
102        new MatBuilder<>(Nat.N2(), Nat.N2())
103            .fill(0.5, 0.5, -1.0 / m_trackwidth, 1.0 / m_trackwidth);
104    var accels = M.times(dxdt);
105
106    // Constrain the linear and angular accelerations
107    if (accels.get(0, 0) > m_maxLinearAccel) {
108      accels.set(0, 0, m_maxLinearAccel);
109    } else if (accels.get(0, 0) < m_minLinearAccel) {
110      accels.set(0, 0, m_minLinearAccel);
111    }
112    if (accels.get(1, 0) > m_maxAngularAccel) {
113      accels.set(1, 0, m_maxAngularAccel);
114    } else if (accels.get(1, 0) < -m_maxAngularAccel) {
115      accels.set(1, 0, -m_maxAngularAccel);
116    }
117
118    // Convert the constrained linear and angular accelerations back to wheel
119    // accelerations
120    dxdt = M.solve(accels);
121
122    // Find voltages for the given wheel accelerations
123    //
124    // dx/dt = Ax + Bu
125    // u = B⁻¹(dx/dt - Ax)
126    u = m_system.getB().solve(dxdt.minus(m_system.getA().times(x)));
127
128    return new DifferentialDriveWheelVoltages(u.get(0, 0), u.get(1, 0));
129  }
130}