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