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