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