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.controller.proto.DifferentialDriveFeedforwardProto;
009import edu.wpi.first.math.controller.struct.DifferentialDriveFeedforwardStruct;
010import edu.wpi.first.math.numbers.N2;
011import edu.wpi.first.math.system.LinearSystem;
012import edu.wpi.first.math.system.plant.LinearSystemId;
013import edu.wpi.first.util.protobuf.ProtobufSerializable;
014import edu.wpi.first.util.struct.StructSerializable;
015
016/** A helper class which computes the feedforward outputs for a differential drive drivetrain. */
017public class DifferentialDriveFeedforward implements ProtobufSerializable, StructSerializable {
018  private final LinearSystem<N2, N2, N2> m_plant;
019
020  /** The linear velocity gain in volts per (meters per second). */
021  public final double m_kVLinear;
022
023  /** The linear acceleration gain in volts per (meters per second squared). */
024  public final double m_kALinear;
025
026  /** The angular velocity gain in volts per (radians per second). */
027  public final double m_kVAngular;
028
029  /** The angular acceleration gain in volts per (radians per second squared). */
030  public final double m_kAAngular;
031
032  /**
033   * Creates a new DifferentialDriveFeedforward with the specified parameters.
034   *
035   * @param kVLinear The linear velocity gain in volts per (meters per second).
036   * @param kALinear The linear acceleration gain in volts per (meters per second squared).
037   * @param kVAngular The angular velocity gain in volts per (radians per second).
038   * @param kAAngular The angular acceleration gain in volts per (radians per second squared).
039   * @param trackwidth The distance between the differential drive's left and right wheels, in
040   *     meters.
041   */
042  public DifferentialDriveFeedforward(
043      double kVLinear, double kALinear, double kVAngular, double kAAngular, double trackwidth) {
044    // See LinearSystemId.identifyDrivetrainSystem(double, double, double, double, double)
045    this(kVLinear, kALinear, kVAngular * 2.0 / trackwidth, kAAngular * 2.0 / trackwidth);
046  }
047
048  /**
049   * Creates a new DifferentialDriveFeedforward with the specified parameters.
050   *
051   * @param kVLinear The linear velocity gain in volts per (meters per second).
052   * @param kALinear The linear acceleration gain in volts per (meters per second squared).
053   * @param kVAngular The angular velocity gain in volts per (meters per second).
054   * @param kAAngular The angular acceleration gain in volts per (meters per second squared).
055   */
056  public DifferentialDriveFeedforward(
057      double kVLinear, double kALinear, double kVAngular, double kAAngular) {
058    m_plant = LinearSystemId.identifyDrivetrainSystem(kVLinear, kALinear, kVAngular, kAAngular);
059    m_kVLinear = kVLinear;
060    m_kALinear = kALinear;
061    m_kVAngular = kVAngular;
062    m_kAAngular = kAAngular;
063  }
064
065  /**
066   * Calculates the differential drive feedforward inputs given velocity setpoints.
067   *
068   * @param currentLeftVelocity The current left velocity of the differential drive in
069   *     meters/second.
070   * @param nextLeftVelocity The next left velocity of the differential drive in meters/second.
071   * @param currentRightVelocity The current right velocity of the differential drive in
072   *     meters/second.
073   * @param nextRightVelocity The next right velocity of the differential drive in meters/second.
074   * @param dtSeconds Discretization timestep.
075   * @return A DifferentialDriveWheelVoltages object containing the computed feedforward voltages.
076   */
077  public DifferentialDriveWheelVoltages calculate(
078      double currentLeftVelocity,
079      double nextLeftVelocity,
080      double currentRightVelocity,
081      double nextRightVelocity,
082      double dtSeconds) {
083    var feedforward = new LinearPlantInversionFeedforward<>(m_plant, dtSeconds);
084    var r = VecBuilder.fill(currentLeftVelocity, currentRightVelocity);
085    var nextR = VecBuilder.fill(nextLeftVelocity, nextRightVelocity);
086    var u = feedforward.calculate(r, nextR);
087    return new DifferentialDriveWheelVoltages(u.get(0, 0), u.get(1, 0));
088  }
089
090  /** DifferentialDriveFeedforward struct for serialization. */
091  public static final DifferentialDriveFeedforwardStruct struct =
092      new DifferentialDriveFeedforwardStruct();
093
094  /** DifferentialDriveFeedforward protobuf for serialization. */
095  public static final DifferentialDriveFeedforwardProto proto =
096      new DifferentialDriveFeedforwardProto();
097}