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}