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.VecBuilder;
008import edu.wpi.first.math.numbers.N2;
009import edu.wpi.first.math.system.LinearSystem;
010import edu.wpi.first.math.system.plant.LinearSystemId;
011
012/** A helper class which computes the feedforward outputs for a differential drive drivetrain. */
013public class DifferentialDriveFeedforward {
014  private final LinearSystem<N2, N2, N2> m_plant;
015
016  /**
017   * Creates a new DifferentialDriveFeedforward with the specified parameters.
018   *
019   * @param kVLinear The linear velocity gain in volts per (meters per second).
020   * @param kALinear The linear acceleration gain in volts per (meters per second squared).
021   * @param kVAngular The angular velocity gain in volts per (radians per second).
022   * @param kAAngular The angular acceleration gain in volts per (radians per second squared).
023   * @param trackwidth The distance between the differential drive's left and right wheels, in
024   *     meters.
025   */
026  public DifferentialDriveFeedforward(
027      double kVLinear, double kALinear, double kVAngular, double kAAngular, double trackwidth) {
028    m_plant =
029        LinearSystemId.identifyDrivetrainSystem(
030            kVLinear, kALinear, kVAngular, kAAngular, trackwidth);
031  }
032
033  /**
034   * Creates a new DifferentialDriveFeedforward with the specified parameters.
035   *
036   * @param kVLinear The linear velocity gain in volts per (meters per second).
037   * @param kALinear The linear acceleration gain in volts per (meters per second squared).
038   * @param kVAngular The angular velocity gain in volts per (meters per second).
039   * @param kAAngular The angular acceleration gain in volts per (meters per second squared).
040   */
041  public DifferentialDriveFeedforward(
042      double kVLinear, double kALinear, double kVAngular, double kAAngular) {
043    m_plant = LinearSystemId.identifyDrivetrainSystem(kVLinear, kALinear, kVAngular, kAAngular);
044  }
045
046  /**
047   * Calculates the differential drive feedforward inputs given velocity setpoints.
048   *
049   * @param currentLeftVelocity The current left velocity of the differential drive in
050   *     meters/second.
051   * @param nextLeftVelocity The next left velocity of the differential drive in meters/second.
052   * @param currentRightVelocity The current right velocity of the differential drive in
053   *     meters/second.
054   * @param nextRightVelocity The next right velocity of the differential drive in meters/second.
055   * @param dtSeconds Discretization timestep.
056   * @return A DifferentialDriveWheelVoltages object containing the computed feedforward voltages.
057   */
058  public DifferentialDriveWheelVoltages calculate(
059      double currentLeftVelocity,
060      double nextLeftVelocity,
061      double currentRightVelocity,
062      double nextRightVelocity,
063      double dtSeconds) {
064    var feedforward = new LinearPlantInversionFeedforward<>(m_plant, dtSeconds);
065    var r = VecBuilder.fill(currentLeftVelocity, currentRightVelocity);
066    var nextR = VecBuilder.fill(nextLeftVelocity, nextRightVelocity);
067    var u = feedforward.calculate(r, nextR);
068    return new DifferentialDriveWheelVoltages(u.get(0, 0), u.get(1, 0));
069  }
070}