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