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