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 in meters per second. */
021  public double speed;
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 speed The speed of the wheel of the module in meters per second.
039   * @param angle The angle of the module.
040   */
041  public SwerveModuleState(double speed, Rotation2d angle) {
042    this.speed = speed;
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.speed - speed) < 1E-9
060        && angle.equals(other.angle);
061  }
062
063  @Override
064  public int hashCode() {
065    return Objects.hash(speed, 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.speed, other.speed);
078  }
079
080  @Override
081  public String toString() {
082    return String.format("SwerveModuleState(Speed: %.2f m/s, Angle: %s)", speed, angle);
083  }
084
085  /**
086   * Minimize the change in heading this swerve module state would require by potentially reversing
087   * the direction the wheel spins. If this is used with the PIDController class's continuous input
088   * functionality, the furthest a wheel will ever rotate is 90 degrees.
089   *
090   * @param currentAngle The current module angle.
091   */
092  public void optimize(Rotation2d currentAngle) {
093    var delta = angle.minus(currentAngle);
094    if (Math.abs(delta.getDegrees()) > 90.0) {
095      speed *= -1;
096      angle = angle.rotateBy(Rotation2d.kPi);
097    }
098  }
099
100  /**
101   * Minimize the change in heading the desired swerve module state would require by potentially
102   * reversing the direction the wheel spins. If this is used with the PIDController class's
103   * continuous input functionality, the furthest a wheel will ever rotate is 90 degrees.
104   *
105   * @param desiredState The desired state.
106   * @param currentAngle The current module angle.
107   * @return Optimized swerve module state.
108   * @deprecated Use the instance method instead.
109   */
110  @Deprecated
111  public static SwerveModuleState optimize(
112      SwerveModuleState desiredState, Rotation2d currentAngle) {
113    var delta = desiredState.angle.minus(currentAngle);
114    if (Math.abs(delta.getDegrees()) > 90.0) {
115      return new SwerveModuleState(
116          -desiredState.speed, desiredState.angle.rotateBy(Rotation2d.kPi));
117    } else {
118      return new SwerveModuleState(desiredState.speed, desiredState.angle);
119    }
120  }
121
122  /**
123   * Scales speed by cosine of angle error. This scales down movement perpendicular to the desired
124   * direction of travel that can occur when modules change directions. This results in smoother
125   * driving.
126   *
127   * @param currentAngle The current module angle.
128   */
129  public void cosineScale(Rotation2d currentAngle) {
130    speed *= angle.minus(currentAngle).getCos();
131  }
132}