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