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 org.wpilib.math.kinematics;
006
007import static org.wpilib.units.Units.MetersPerSecondPerSecond;
008
009import org.wpilib.math.interpolation.Interpolatable;
010import org.wpilib.math.kinematics.proto.DifferentialDriveWheelAccelerationsProto;
011import org.wpilib.math.kinematics.struct.DifferentialDriveWheelAccelerationsStruct;
012import org.wpilib.units.measure.LinearAcceleration;
013import org.wpilib.util.protobuf.ProtobufSerializable;
014import org.wpilib.util.struct.StructSerializable;
015
016/** Represents the wheel accelerations for a differential drive drivetrain. */
017public class DifferentialDriveWheelAccelerations
018    implements Interpolatable<DifferentialDriveWheelAccelerations>,
019        ProtobufSerializable,
020        StructSerializable {
021  /** Acceleration of the left side of the robot in meters per second squared. */
022  public double left;
023
024  /** Acceleration of the right side of the robot in meters per second squared. */
025  public double right;
026
027  /** DifferentialDriveWheelAccelerations protobuf for serialization. */
028  public static final DifferentialDriveWheelAccelerationsProto proto =
029      new DifferentialDriveWheelAccelerationsProto();
030
031  /** DifferentialDriveWheelAccelerations struct for serialization. */
032  public static final DifferentialDriveWheelAccelerationsStruct struct =
033      new DifferentialDriveWheelAccelerationsStruct();
034
035  /**
036   * Constructs a DifferentialDriveWheelAccelerations with zeros for left and right accelerations.
037   */
038  public DifferentialDriveWheelAccelerations() {}
039
040  /**
041   * Constructs a DifferentialDriveWheelAccelerations.
042   *
043   * @param left The left acceleration in meters per second squared.
044   * @param right The right acceleration in meters per second squared.
045   */
046  public DifferentialDriveWheelAccelerations(double left, double right) {
047    this.left = left;
048    this.right = right;
049  }
050
051  /**
052   * Constructs a DifferentialDriveWheelAccelerations.
053   *
054   * @param left The left acceleration in meters per second squared.
055   * @param right The right acceleration in meters per second squared.
056   */
057  public DifferentialDriveWheelAccelerations(LinearAcceleration left, LinearAcceleration right) {
058    this(left.in(MetersPerSecondPerSecond), right.in(MetersPerSecondPerSecond));
059  }
060
061  /**
062   * Adds two DifferentialDriveWheelAccelerations and returns the sum.
063   *
064   * <p>For example, DifferentialDriveWheelAccelerations{1.0, 0.5} +
065   * DifferentialDriveWheelAccelerations{2.0, 1.5} = DifferentialDriveWheelAccelerations{3.0, 2.0}
066   *
067   * @param other The DifferentialDriveWheelAccelerations to add.
068   * @return The sum of the DifferentialDriveWheelAccelerations.
069   */
070  public DifferentialDriveWheelAccelerations plus(DifferentialDriveWheelAccelerations other) {
071    return new DifferentialDriveWheelAccelerations(left + other.left, right + other.right);
072  }
073
074  /**
075   * Subtracts the other DifferentialDriveWheelAccelerations from the current
076   * DifferentialDriveWheelAccelerations and returns the difference.
077   *
078   * <p>For example, DifferentialDriveWheelAccelerations{5.0, 4.0} -
079   * DifferentialDriveWheelAccelerations{1.0, 2.0} = DifferentialDriveWheelAccelerations{4.0, 2.0}
080   *
081   * @param other The DifferentialDriveWheelAccelerations to subtract.
082   * @return The difference between the two DifferentialDriveWheelAccelerations.
083   */
084  public DifferentialDriveWheelAccelerations minus(DifferentialDriveWheelAccelerations other) {
085    return new DifferentialDriveWheelAccelerations(left - other.left, right - other.right);
086  }
087
088  /**
089   * Returns the inverse of the current DifferentialDriveWheelAccelerations. This is equivalent to
090   * negating all components of the DifferentialDriveWheelAccelerations.
091   *
092   * @return The inverse of the current DifferentialDriveWheelAccelerations.
093   */
094  public DifferentialDriveWheelAccelerations unaryMinus() {
095    return new DifferentialDriveWheelAccelerations(-left, -right);
096  }
097
098  /**
099   * Multiplies the DifferentialDriveWheelAccelerations by a scalar and returns the new
100   * DifferentialDriveWheelAccelerations.
101   *
102   * <p>For example, DifferentialDriveWheelAccelerations{2.0, 2.5} * 2 =
103   * DifferentialDriveWheelAccelerations{4.0, 5.0}
104   *
105   * @param scalar The scalar to multiply by.
106   * @return The scaled DifferentialDriveWheelAccelerations.
107   */
108  public DifferentialDriveWheelAccelerations times(double scalar) {
109    return new DifferentialDriveWheelAccelerations(left * scalar, right * scalar);
110  }
111
112  /**
113   * Divides the DifferentialDriveWheelAccelerations by a scalar and returns the new
114   * DifferentialDriveWheelAccelerations.
115   *
116   * <p>For example, DifferentialDriveWheelAccelerations{2.0, 2.5} / 2 =
117   * DifferentialDriveWheelAccelerations{1.0, 1.25}
118   *
119   * @param scalar The scalar to divide by.
120   * @return The scaled DifferentialDriveWheelAccelerations.
121   */
122  public DifferentialDriveWheelAccelerations div(double scalar) {
123    return new DifferentialDriveWheelAccelerations(left / scalar, right / scalar);
124  }
125
126  /**
127   * Returns the linear interpolation of this DifferentialDriveWheelAccelerations and another.
128   *
129   * @param endValue The end value for the interpolation.
130   * @param t How far between the two values to interpolate. This is clamped to [0, 1].
131   * @return The interpolated value.
132   */
133  @Override
134  public DifferentialDriveWheelAccelerations interpolate(
135      DifferentialDriveWheelAccelerations endValue, double t) {
136    // Clamp t to [0, 1]
137    t = Math.max(0.0, Math.min(1.0, t));
138
139    return new DifferentialDriveWheelAccelerations(
140        left + t * (endValue.left - left), right + t * (endValue.right - right));
141  }
142
143  @Override
144  public String toString() {
145    return String.format(
146        "DifferentialDriveWheelAccelerations(Left: %.2f m/s², Right: %.2f m/s²)", left, right);
147  }
148}