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 edu.wpi.first.math.geometry.Rotation2d;
008import java.util.Objects;
009
010/** Represents the state of one swerve module. */
011@SuppressWarnings("MemberName")
012public class SwerveModuleState implements Comparable<SwerveModuleState> {
013  /** Speed of the wheel of the module. */
014  public double speedMetersPerSecond;
015
016  /** Angle of the module. */
017  public Rotation2d angle = Rotation2d.fromDegrees(0);
018
019  /** Constructs a SwerveModuleState with zeros for speed and angle. */
020  public SwerveModuleState() {}
021
022  /**
023   * Constructs a SwerveModuleState.
024   *
025   * @param speedMetersPerSecond The speed of the wheel of the module.
026   * @param angle The angle of the module.
027   */
028  public SwerveModuleState(double speedMetersPerSecond, Rotation2d angle) {
029    this.speedMetersPerSecond = speedMetersPerSecond;
030    this.angle = angle;
031  }
032
033  @Override
034  public boolean equals(Object obj) {
035    if (obj instanceof SwerveModuleState) {
036      return Double.compare(speedMetersPerSecond, ((SwerveModuleState) obj).speedMetersPerSecond)
037          == 0;
038    }
039    return false;
040  }
041
042  @Override
043  public int hashCode() {
044    return Objects.hash(speedMetersPerSecond);
045  }
046
047  /**
048   * Compares two swerve module states. One swerve module is "greater" than the other if its speed
049   * is higher than the other.
050   *
051   * @param other The other swerve module.
052   * @return 1 if this is greater, 0 if both are equal, -1 if other is greater.
053   */
054  @Override
055  public int compareTo(SwerveModuleState other) {
056    return Double.compare(this.speedMetersPerSecond, other.speedMetersPerSecond);
057  }
058
059  @Override
060  public String toString() {
061    return String.format(
062        "SwerveModuleState(Speed: %.2f m/s, Angle: %s)", speedMetersPerSecond, angle);
063  }
064
065  /**
066   * Minimize the change in heading the desired swerve module state would require by potentially
067   * reversing the direction the wheel spins. If this is used with the PIDController class's
068   * continuous input functionality, the furthest a wheel will ever rotate is 90 degrees.
069   *
070   * @param desiredState The desired state.
071   * @param currentAngle The current module angle.
072   * @return Optimized swerve module state.
073   */
074  public static SwerveModuleState optimize(
075      SwerveModuleState desiredState, Rotation2d currentAngle) {
076    var delta = desiredState.angle.minus(currentAngle);
077    if (Math.abs(delta.getDegrees()) > 90.0) {
078      return new SwerveModuleState(
079          -desiredState.speedMetersPerSecond,
080          desiredState.angle.rotateBy(Rotation2d.fromDegrees(180.0)));
081    } else {
082      return new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle);
083    }
084  }
085}