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