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.measure.LinearVelocity;
013import edu.wpi.first.util.protobuf.ProtobufSerializable;
014import edu.wpi.first.util.struct.StructSerializable;
015import java.util.Objects;
016
017/** Represents the state of one swerve module. */
018public class SwerveModuleState
019    implements Comparable<SwerveModuleState>, ProtobufSerializable, StructSerializable {
020  /** Speed of the wheel of the module. */
021  public double speedMetersPerSecond;
022
023  /** Angle of the module. */
024  public Rotation2d angle = Rotation2d.kZero;
025
026  /** SwerveModuleState protobuf for serialization. */
027  public static final SwerveModuleStateProto proto = new SwerveModuleStateProto();
028
029  /** SwerveModuleState struct for serialization. */
030  public static final SwerveModuleStateStruct struct = new SwerveModuleStateStruct();
031
032  /** Constructs a SwerveModuleState with zeros for speed and angle. */
033  public SwerveModuleState() {}
034
035  /**
036   * Constructs a SwerveModuleState.
037   *
038   * @param speedMetersPerSecond The speed of the wheel of the module.
039   * @param angle The angle of the module.
040   */
041  public SwerveModuleState(double speedMetersPerSecond, Rotation2d angle) {
042    this.speedMetersPerSecond = speedMetersPerSecond;
043    this.angle = angle;
044  }
045
046  /**
047   * Constructs a SwerveModuleState.
048   *
049   * @param speed The speed of the wheel of the module.
050   * @param angle The angle of the module.
051   */
052  public SwerveModuleState(LinearVelocity speed, Rotation2d angle) {
053    this(speed.in(MetersPerSecond), angle);
054  }
055
056  @Override
057  public boolean equals(Object obj) {
058    return obj instanceof SwerveModuleState other
059        && Math.abs(other.speedMetersPerSecond - speedMetersPerSecond) < 1E-9
060        && angle.equals(other.angle);
061  }
062
063  @Override
064  public int hashCode() {
065    return Objects.hash(speedMetersPerSecond, angle);
066  }
067
068  /**
069   * Compares two swerve module states. One swerve module is "greater" than the other if its speed
070   * is higher than the other.
071   *
072   * @param other The other swerve module.
073   * @return 1 if this is greater, 0 if both are equal, -1 if other is greater.
074   */
075  @Override
076  public int compareTo(SwerveModuleState other) {
077    return Double.compare(this.speedMetersPerSecond, other.speedMetersPerSecond);
078  }
079
080  @Override
081  public String toString() {
082    return String.format(
083        "SwerveModuleState(Speed: %.2f m/s, Angle: %s)", speedMetersPerSecond, angle);
084  }
085
086  /**
087   * Minimize the change in heading this swerve module state would require by potentially reversing
088   * the direction the wheel spins. If this is used with the PIDController class's continuous input
089   * functionality, the furthest a wheel will ever rotate is 90 degrees.
090   *
091   * @param currentAngle The current module angle.
092   */
093  public void optimize(Rotation2d currentAngle) {
094    var delta = angle.minus(currentAngle);
095    if (Math.abs(delta.getDegrees()) > 90.0) {
096      speedMetersPerSecond *= -1;
097      angle = angle.rotateBy(Rotation2d.kPi);
098    }
099  }
100
101  /**
102   * Minimize the change in heading the desired swerve module state would require by potentially
103   * reversing the direction the wheel spins. If this is used with the PIDController class's
104   * continuous input functionality, the furthest a wheel will ever rotate is 90 degrees.
105   *
106   * @param desiredState The desired state.
107   * @param currentAngle The current module angle.
108   * @return Optimized swerve module state.
109   * @deprecated Use the instance method instead.
110   */
111  @Deprecated
112  public static SwerveModuleState optimize(
113      SwerveModuleState desiredState, Rotation2d currentAngle) {
114    var delta = desiredState.angle.minus(currentAngle);
115    if (Math.abs(delta.getDegrees()) > 90.0) {
116      return new SwerveModuleState(
117          -desiredState.speedMetersPerSecond, desiredState.angle.rotateBy(Rotation2d.kPi));
118    } else {
119      return new SwerveModuleState(desiredState.speedMetersPerSecond, desiredState.angle);
120    }
121  }
122
123  /**
124   * Scales speed by cosine of angle error. This scales down movement perpendicular to the desired
125   * direction of travel that can occur when modules change directions. This results in smoother
126   * driving.
127   *
128   * @param currentAngle The current module angle.
129   */
130  public void cosineScale(Rotation2d currentAngle) {
131    speedMetersPerSecond *= angle.minus(currentAngle).getCos();
132  }
133}