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.MetersPerSecond;
008
009import org.wpilib.annotation.NoDiscard;
010import org.wpilib.math.interpolation.Interpolatable;
011import org.wpilib.math.kinematics.proto.DifferentialDriveWheelVelocitiesProto;
012import org.wpilib.math.kinematics.struct.DifferentialDriveWheelVelocitiesStruct;
013import org.wpilib.units.measure.LinearVelocity;
014import org.wpilib.util.protobuf.ProtobufSerializable;
015import org.wpilib.util.struct.StructSerializable;
016
017/** Represents the wheel velocities for a differential drive drivetrain. */
018@NoDiscard
019public class DifferentialDriveWheelVelocities
020    implements Interpolatable<DifferentialDriveWheelVelocities>,
021        ProtobufSerializable,
022        StructSerializable {
023  /** Velocity of the left side of the robot in meters per second. */
024  public double left;
025
026  /** Velocity of the right side of the robot in meters per second. */
027  public double right;
028
029  /** DifferentialDriveWheelVelocities protobuf for serialization. */
030  public static final DifferentialDriveWheelVelocitiesProto proto =
031      new DifferentialDriveWheelVelocitiesProto();
032
033  /** DifferentialDriveWheelVelocities struct for serialization. */
034  public static final DifferentialDriveWheelVelocitiesStruct struct =
035      new DifferentialDriveWheelVelocitiesStruct();
036
037  /** Constructs a DifferentialDriveWheelVelocities with zeros for left and right velocities. */
038  public DifferentialDriveWheelVelocities() {}
039
040  /**
041   * Constructs a DifferentialDriveWheelVelocities.
042   *
043   * @param left The left velocity in meters per second.
044   * @param right The right velocity in meters per second.
045   */
046  public DifferentialDriveWheelVelocities(double left, double right) {
047    this.left = left;
048    this.right = right;
049  }
050
051  /**
052   * Constructs a DifferentialDriveWheelVelocities.
053   *
054   * @param left The left velocity in meters per second.
055   * @param right The right velocity in meters per second.
056   */
057  public DifferentialDriveWheelVelocities(LinearVelocity left, LinearVelocity right) {
058    this(left.in(MetersPerSecond), right.in(MetersPerSecond));
059  }
060
061  /**
062   * Renormalizes the wheel velocities if any either side is above the specified maximum.
063   *
064   * <p>Sometimes, after inverse kinematics, the requested velocity from one or more wheels may be
065   * above the max attainable velocity for the driving motor on that wheel. To fix this issue, one
066   * can reduce all the wheel velocities to make sure that all requested module velocities are
067   * at-or-below the absolute threshold, while maintaining the ratio of velocities between wheels.
068   *
069   * @param attainableMaxVelocity The absolute max velocity in meters per second that a wheel can
070   *     reach.
071   * @return The desaturated DifferentialDriveWheelVelocities.
072   */
073  public DifferentialDriveWheelVelocities desaturate(double attainableMaxVelocity) {
074    double realMaxVelocity = Math.max(Math.abs(left), Math.abs(right));
075
076    if (realMaxVelocity > attainableMaxVelocity) {
077      return new DifferentialDriveWheelVelocities(
078          left / realMaxVelocity * attainableMaxVelocity,
079          right / realMaxVelocity * attainableMaxVelocity);
080    }
081    return new DifferentialDriveWheelVelocities(left, right);
082  }
083
084  /**
085   * Renormalizes the wheel velocities if any either side is above the specified maximum.
086   *
087   * <p>Sometimes, after inverse kinematics, the requested velocity from one or more wheels may be
088   * above the max attainable velocity for the driving motor on that wheel. To fix this issue, one
089   * can reduce all the wheel velocities to make sure that all requested module velocities are
090   * at-or-below the absolute threshold, while maintaining the ratio of velocities between wheels.
091   *
092   * @param attainableMaxVelocity The absolute max velocity in meters per second that a wheel can
093   *     reach.
094   * @return The desaturated DifferentialDriveWheelVelocities.
095   */
096  public DifferentialDriveWheelVelocities desaturate(LinearVelocity attainableMaxVelocity) {
097    return desaturate(attainableMaxVelocity.in(MetersPerSecond));
098  }
099
100  /**
101   * Adds two DifferentialDriveWheelVelocities and returns the sum.
102   *
103   * <p>For example, DifferentialDriveWheelVelocities{1.0, 0.5} +
104   * DifferentialDriveWheelVelocities{2.0, 1.5} = DifferentialDriveWheelVelocities{3.0, 2.0}
105   *
106   * @param other The DifferentialDriveWheelVelocities to add.
107   * @return The sum of the DifferentialDriveWheelVelocities.
108   */
109  public DifferentialDriveWheelVelocities plus(DifferentialDriveWheelVelocities other) {
110    return new DifferentialDriveWheelVelocities(left + other.left, right + other.right);
111  }
112
113  /**
114   * Subtracts the other DifferentialDriveWheelVelocities from the current
115   * DifferentialDriveWheelVelocities and returns the difference.
116   *
117   * <p>For example, DifferentialDriveWheelVelocities{5.0, 4.0} -
118   * DifferentialDriveWheelVelocities{1.0, 2.0} = DifferentialDriveWheelVelocities{4.0, 2.0}
119   *
120   * @param other The DifferentialDriveWheelVelocities to subtract.
121   * @return The difference between the two DifferentialDriveWheelVelocities.
122   */
123  public DifferentialDriveWheelVelocities minus(DifferentialDriveWheelVelocities other) {
124    return new DifferentialDriveWheelVelocities(left - other.left, right - other.right);
125  }
126
127  /**
128   * Returns the inverse of the current DifferentialDriveWheelVelocities. This is equivalent to
129   * negating all components of the DifferentialDriveWheelVelocities.
130   *
131   * @return The inverse of the current DifferentialDriveWheelVelocities.
132   */
133  public DifferentialDriveWheelVelocities unaryMinus() {
134    return new DifferentialDriveWheelVelocities(-left, -right);
135  }
136
137  /**
138   * Multiplies the DifferentialDriveWheelVelocities by a scalar and returns the new
139   * DifferentialDriveWheelVelocities.
140   *
141   * <p>For example, DifferentialDriveWheelVelocities{2.0, 2.5} * 2 =
142   * DifferentialDriveWheelVelocities{4.0, 5.0}
143   *
144   * @param scalar The scalar to multiply by.
145   * @return The scaled DifferentialDriveWheelVelocities.
146   */
147  public DifferentialDriveWheelVelocities times(double scalar) {
148    return new DifferentialDriveWheelVelocities(left * scalar, right * scalar);
149  }
150
151  /**
152   * Divides the DifferentialDriveWheelVelocities by a scalar and returns the new
153   * DifferentialDriveWheelVelocities.
154   *
155   * <p>For example, DifferentialDriveWheelVelocities{2.0, 2.5} / 2 =
156   * DifferentialDriveWheelVelocities{1.0, 1.25}
157   *
158   * @param scalar The scalar to divide by.
159   * @return The scaled DifferentialDriveWheelVelocities.
160   */
161  public DifferentialDriveWheelVelocities div(double scalar) {
162    return new DifferentialDriveWheelVelocities(left / scalar, right / scalar);
163  }
164
165  /**
166   * Returns the linear interpolation of this DifferentialDriveWheelVelocities and another.
167   *
168   * @param endValue The end value for the interpolation.
169   * @param t How far between the two values to interpolate. This is clamped to [0, 1].
170   * @return The interpolated value.
171   */
172  @Override
173  public DifferentialDriveWheelVelocities interpolate(
174      DifferentialDriveWheelVelocities endValue, double t) {
175    // Clamp t to [0, 1]
176    t = Math.max(0.0, Math.min(1.0, t));
177
178    return new DifferentialDriveWheelVelocities(
179        left + t * (endValue.left - left), right + t * (endValue.right - right));
180  }
181
182  @Override
183  public String toString() {
184    return String.format(
185        "DifferentialDriveWheelVelocities(Left: %.2f m/s, Right: %.2f m/s)", left, right);
186  }
187}