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}