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 in meters. */
026  public double distance;
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 distance The distance measured by the wheel of the module in meters.
044   * @param angle The angle of the module.
045   */
046  public SwerveModulePosition(double distance, Rotation2d angle) {
047    this.distance = distance;
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.distance - distance) < 1E-9
065        && angle.equals(other.angle);
066  }
067
068  @Override
069  public int hashCode() {
070    return Objects.hash(distance, 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.distance, other.distance);
083  }
084
085  @Override
086  public String toString() {
087    return String.format("SwerveModulePosition(Distance: %.2f m, Angle: %s)", distance, angle);
088  }
089
090  /**
091   * Returns a copy of this swerve module position.
092   *
093   * @return A copy.
094   */
095  public SwerveModulePosition copy() {
096    return new SwerveModulePosition(distance, angle);
097  }
098
099  @Override
100  public SwerveModulePosition interpolate(SwerveModulePosition endValue, double t) {
101    return new SwerveModulePosition(
102        MathUtil.interpolate(this.distance, endValue.distance, t),
103        this.angle.interpolate(endValue.angle, t));
104  }
105}