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