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