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. */
011public class SwerveModuleState implements Comparable<SwerveModuleState> {
012  /** Speed of the wheel of the module. */
013  public double speedMetersPerSecond;
014
015  /** Angle of the module. */
016  public Rotation2d angle = Rotation2d.fromDegrees(0);
017
018  /** Constructs a SwerveModuleState with zeros for speed and angle. */
019  public SwerveModuleState() {}
020
021  /**
022   * Constructs a SwerveModuleState.
023   *
024   * @param speedMetersPerSecond The speed of the wheel of the module.
025   * @param angle The angle of the module.
026   */
027  public SwerveModuleState(double speedMetersPerSecond, Rotation2d angle) {
028    this.speedMetersPerSecond = speedMetersPerSecond;
029    this.angle = angle;
030  }
031
032  @Override
033  public boolean equals(Object obj) {
034    if (obj instanceof SwerveModuleState) {
035      SwerveModuleState other = (SwerveModuleState) obj;
036      return Math.abs(other.speedMetersPerSecond - speedMetersPerSecond) < 1E-9
037          && angle.equals(other.angle);
038    }
039    return false;
040  }
041
042  @Override
043  public int hashCode() {
044    return Objects.hash(speedMetersPerSecond, angle);
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}