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