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