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 java.util.stream.DoubleStream;
008
009@SuppressWarnings("MemberName")
010public class MecanumDriveWheelSpeeds {
011  /** Speed of the front left wheel. */
012  public double frontLeftMetersPerSecond;
013
014  /** Speed of the front right wheel. */
015  public double frontRightMetersPerSecond;
016
017  /** Speed of the rear left wheel. */
018  public double rearLeftMetersPerSecond;
019
020  /** Speed of the rear right wheel. */
021  public double rearRightMetersPerSecond;
022
023  /** Constructs a MecanumDriveWheelSpeeds with zeros for all member fields. */
024  public MecanumDriveWheelSpeeds() {}
025
026  /**
027   * Constructs a MecanumDriveWheelSpeeds.
028   *
029   * @param frontLeftMetersPerSecond Speed of the front left wheel.
030   * @param frontRightMetersPerSecond Speed of the front right wheel.
031   * @param rearLeftMetersPerSecond Speed of the rear left wheel.
032   * @param rearRightMetersPerSecond Speed of the rear right wheel.
033   */
034  public MecanumDriveWheelSpeeds(
035      double frontLeftMetersPerSecond,
036      double frontRightMetersPerSecond,
037      double rearLeftMetersPerSecond,
038      double rearRightMetersPerSecond) {
039    this.frontLeftMetersPerSecond = frontLeftMetersPerSecond;
040    this.frontRightMetersPerSecond = frontRightMetersPerSecond;
041    this.rearLeftMetersPerSecond = rearLeftMetersPerSecond;
042    this.rearRightMetersPerSecond = rearRightMetersPerSecond;
043  }
044
045  /**
046   * Renormalizes the wheel speeds if any individual speed is above the specified maximum.
047   *
048   * <p>Sometimes, after inverse kinematics, the requested speed from one or more wheels may be
049   * above the max attainable speed for the driving motor on that wheel. To fix this issue, one can
050   * reduce all the wheel speeds to make sure that all requested module speeds are at-or-below the
051   * absolute threshold, while maintaining the ratio of speeds between wheels.
052   *
053   * @param attainableMaxSpeedMetersPerSecond The absolute max speed that a wheel can reach.
054   */
055  public void desaturate(double attainableMaxSpeedMetersPerSecond) {
056    double realMaxSpeed =
057        DoubleStream.of(
058                frontLeftMetersPerSecond,
059                frontRightMetersPerSecond,
060                rearLeftMetersPerSecond,
061                rearRightMetersPerSecond)
062            .max()
063            .getAsDouble();
064
065    if (realMaxSpeed > attainableMaxSpeedMetersPerSecond) {
066      frontLeftMetersPerSecond =
067          frontLeftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
068      frontRightMetersPerSecond =
069          frontRightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
070      rearLeftMetersPerSecond =
071          rearLeftMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
072      rearRightMetersPerSecond =
073          rearRightMetersPerSecond / realMaxSpeed * attainableMaxSpeedMetersPerSecond;
074    }
075  }
076
077  @Override
078  public String toString() {
079    return String.format(
080        "MecanumDriveWheelSpeeds(Front Left: %.2f m/s, Front Right: %.2f m/s, "
081            + "Rear Left: %.2f m/s, Rear Right: %.2f m/s)",
082        frontLeftMetersPerSecond,
083        frontRightMetersPerSecond,
084        rearLeftMetersPerSecond,
085        rearRightMetersPerSecond);
086  }
087}