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