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