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.MetersPerSecond; 008 009import edu.wpi.first.math.geometry.Rotation2d; 010import edu.wpi.first.math.kinematics.proto.SwerveModuleStateProto; 011import edu.wpi.first.math.kinematics.struct.SwerveModuleStateStruct; 012import edu.wpi.first.units.Distance; 013import edu.wpi.first.units.Measure; 014import edu.wpi.first.units.Velocity; 015import java.util.Objects; 016 017/** Represents the state of one swerve module. */ 018public class SwerveModuleState implements Comparable<SwerveModuleState> { 019 /** Speed of the wheel of the module. */ 020 public double speedMetersPerSecond; 021 022 /** Angle of the module. */ 023 public Rotation2d angle = Rotation2d.fromDegrees(0); 024 025 public static final SwerveModuleStateStruct struct = new SwerveModuleStateStruct(); 026 public static final SwerveModuleStateProto proto = new SwerveModuleStateProto(); 027 028 /** Constructs a SwerveModuleState with zeros for speed and angle. */ 029 public SwerveModuleState() {} 030 031 /** 032 * Constructs a SwerveModuleState. 033 * 034 * @param speedMetersPerSecond The speed of the wheel of the module. 035 * @param angle The angle of the module. 036 */ 037 public SwerveModuleState(double speedMetersPerSecond, Rotation2d angle) { 038 this.speedMetersPerSecond = speedMetersPerSecond; 039 this.angle = angle; 040 } 041 042 /** 043 * Constructs a SwerveModuleState. 044 * 045 * @param speed The speed of the wheel of the module. 046 * @param angle The angle of the module. 047 */ 048 public SwerveModuleState(Measure<Velocity<Distance>> speed, Rotation2d angle) { 049 this(speed.in(MetersPerSecond), angle); 050 } 051 052 @Override 053 public boolean equals(Object obj) { 054 if (obj instanceof SwerveModuleState) { 055 SwerveModuleState other = (SwerveModuleState) obj; 056 return Math.abs(other.speedMetersPerSecond - speedMetersPerSecond) < 1E-9 057 && angle.equals(other.angle); 058 } 059 return false; 060 } 061 062 @Override 063 public int hashCode() { 064 return Objects.hash(speedMetersPerSecond, angle); 065 } 066 067 /** 068 * Compares two swerve module states. One swerve module is "greater" than the other if its speed 069 * is higher than the other. 070 * 071 * @param other The other swerve module. 072 * @return 1 if this is greater, 0 if both are equal, -1 if other is greater. 073 */ 074 @Override 075 public int compareTo(SwerveModuleState other) { 076 return Double.compare(this.speedMetersPerSecond, other.speedMetersPerSecond); 077 } 078 079 @Override 080 public String toString() { 081 return String.format( 082 "SwerveModuleState(Speed: %.2f m/s, Angle: %s)", speedMetersPerSecond, angle); 083 } 084 085 /** 086 * Minimize the change in heading the desired swerve module state would require by potentially 087 * reversing the direction the wheel spins. If this is used with the PIDController class's 088 * continuous input functionality, the furthest a wheel will ever rotate is 90 degrees. 089 * 090 * @param desiredState The desired state. 091 * @param currentAngle The current module angle. 092 * @return Optimized swerve module state. 093 */ 094 public static SwerveModuleState optimize( 095 SwerveModuleState desiredState, Rotation2d currentAngle) { 096 var delta = desiredState.angle.minus(currentAngle); 097 if (Math.abs(delta.getDegrees()) > 90.0) { 098 return new SwerveModuleState( 099 -desiredState.speedMetersPerSecond, 100 desiredState.angle.rotateBy(Rotation2d.fromDegrees(180.0))); 101 } else { 102 return new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle); 103 } 104 } 105}