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 SwerveModulePosition implements Comparable<SwerveModulePosition> {
012  /** Distance measured by the wheel of the module. */
013  public double distanceMeters;
014
015  /** Angle of the module. */
016  public Rotation2d angle = Rotation2d.fromDegrees(0);
017
018  /** Constructs a SwerveModulePosition with zeros for distance and angle. */
019  public SwerveModulePosition() {}
020
021  /**
022   * Constructs a SwerveModulePosition.
023   *
024   * @param distanceMeters The distance measured by the wheel of the module.
025   * @param angle The angle of the module.
026   */
027  public SwerveModulePosition(double distanceMeters, Rotation2d angle) {
028    this.distanceMeters = distanceMeters;
029    this.angle = angle;
030  }
031
032  @Override
033  public boolean equals(Object obj) {
034    if (obj instanceof SwerveModulePosition) {
035      SwerveModulePosition other = (SwerveModulePosition) obj;
036      return Math.abs(other.distanceMeters - distanceMeters) < 1E-9 && angle.equals(other.angle);
037    }
038    return false;
039  }
040
041  @Override
042  public int hashCode() {
043    return Objects.hash(distanceMeters, angle);
044  }
045
046  /**
047   * Compares two swerve module positions. One swerve module is "greater" than the other if its
048   * distance is higher than the other.
049   *
050   * @param other The other swerve module.
051   * @return 1 if this is greater, 0 if both are equal, -1 if other is greater.
052   */
053  @Override
054  public int compareTo(SwerveModulePosition other) {
055    return Double.compare(this.distanceMeters, other.distanceMeters);
056  }
057
058  @Override
059  public String toString() {
060    return String.format(
061        "SwerveModulePosition(Distance: %.2f m, Angle: %s)", distanceMeters, angle);
062  }
063}