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.MathUtil;
008import edu.wpi.first.math.geometry.Rotation2d;
009import edu.wpi.first.math.interpolation.Interpolatable;
010import java.util.Objects;
011
012/** Represents the state of one swerve module. */
013public class SwerveModulePosition
014    implements Comparable<SwerveModulePosition>, Interpolatable<SwerveModulePosition> {
015  /** Distance measured by the wheel of the module. */
016  public double distanceMeters;
017
018  /** Angle of the module. */
019  public Rotation2d angle = Rotation2d.fromDegrees(0);
020
021  /** Constructs a SwerveModulePosition with zeros for distance and angle. */
022  public SwerveModulePosition() {}
023
024  /**
025   * Constructs a SwerveModulePosition.
026   *
027   * @param distanceMeters The distance measured by the wheel of the module.
028   * @param angle The angle of the module.
029   */
030  public SwerveModulePosition(double distanceMeters, Rotation2d angle) {
031    this.distanceMeters = distanceMeters;
032    this.angle = angle;
033  }
034
035  @Override
036  public boolean equals(Object obj) {
037    if (obj instanceof SwerveModulePosition) {
038      SwerveModulePosition other = (SwerveModulePosition) obj;
039      return Math.abs(other.distanceMeters - distanceMeters) < 1E-9 && angle.equals(other.angle);
040    }
041    return false;
042  }
043
044  @Override
045  public int hashCode() {
046    return Objects.hash(distanceMeters, angle);
047  }
048
049  /**
050   * Compares two swerve module positions. One swerve module is "greater" than the other if its
051   * distance is higher than the other.
052   *
053   * @param other The other swerve module.
054   * @return 1 if this is greater, 0 if both are equal, -1 if other is greater.
055   */
056  @Override
057  public int compareTo(SwerveModulePosition other) {
058    return Double.compare(this.distanceMeters, other.distanceMeters);
059  }
060
061  @Override
062  public String toString() {
063    return String.format(
064        "SwerveModulePosition(Distance: %.2f m, Angle: %s)", distanceMeters, angle);
065  }
066
067  /**
068   * Returns a copy of this swerve module position.
069   *
070   * @return A copy.
071   */
072  public SwerveModulePosition copy() {
073    return new SwerveModulePosition(distanceMeters, angle);
074  }
075
076  /**
077   * Calculates the difference between two swerve module positions. The difference has a length
078   * equal to the difference in lengths and an angle equal to the ending angle (this module
079   * position's angle).
080   *
081   * @param other The swerve module position to subtract.
082   * @return The difference.
083   */
084  public SwerveModulePosition minus(SwerveModulePosition other) {
085    return new SwerveModulePosition(this.distanceMeters - other.distanceMeters, this.angle);
086  }
087
088  @Override
089  public SwerveModulePosition interpolate(SwerveModulePosition endValue, double t) {
090    return new SwerveModulePosition(
091        MathUtil.interpolate(this.distanceMeters, endValue.distanceMeters, t),
092        this.angle.interpolate(endValue.angle, t));
093  }
094}