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 org.wpilib.math.kinematics;
006
007import static org.wpilib.units.Units.MetersPerSecond;
008
009import java.util.Objects;
010import org.wpilib.annotation.NoDiscard;
011import org.wpilib.math.geometry.Rotation2d;
012import org.wpilib.math.interpolation.Interpolatable;
013import org.wpilib.math.kinematics.proto.SwerveModuleVelocityProto;
014import org.wpilib.math.kinematics.struct.SwerveModuleVelocityStruct;
015import org.wpilib.units.measure.LinearVelocity;
016import org.wpilib.util.protobuf.ProtobufSerializable;
017import org.wpilib.util.struct.StructSerializable;
018
019/** Represents the velocity of one swerve module. */
020@NoDiscard
021public class SwerveModuleVelocity
022    implements Interpolatable<SwerveModuleVelocity>,
023        Comparable<SwerveModuleVelocity>,
024        ProtobufSerializable,
025        StructSerializable {
026  /** Velocity of the wheel of the module in meters per second. */
027  public double velocity;
028
029  /** Angle of the module. */
030  public Rotation2d angle = Rotation2d.kZero;
031
032  /** SwerveModuleVelocity protobuf for serialization. */
033  public static final SwerveModuleVelocityProto proto = new SwerveModuleVelocityProto();
034
035  /** SwerveModuleVelocity struct for serialization. */
036  public static final SwerveModuleVelocityStruct struct = new SwerveModuleVelocityStruct();
037
038  /** Constructs a SwerveModuleVelocity with zeros for velocity and angle. */
039  public SwerveModuleVelocity() {}
040
041  /**
042   * Constructs a SwerveModuleVelocity.
043   *
044   * @param velocity The velocity of the wheel of the module in meters per second.
045   * @param angle The angle of the module.
046   */
047  public SwerveModuleVelocity(double velocity, Rotation2d angle) {
048    this.velocity = velocity;
049    this.angle = angle;
050  }
051
052  /**
053   * Constructs a SwerveModuleVelocity.
054   *
055   * @param velocity The velocity of the wheel of the module.
056   * @param angle The angle of the module.
057   */
058  public SwerveModuleVelocity(LinearVelocity velocity, Rotation2d angle) {
059    this(velocity.in(MetersPerSecond), angle);
060  }
061
062  @Override
063  public boolean equals(Object obj) {
064    return obj instanceof SwerveModuleVelocity other
065        && Math.abs(other.velocity - velocity) < 1E-9
066        && angle.equals(other.angle);
067  }
068
069  @Override
070  public int hashCode() {
071    return Objects.hash(velocity, angle);
072  }
073
074  /**
075   * Compares two swerve module velocities. One swerve module is "greater" than the other if its
076   * velocity is higher than the other.
077   *
078   * @param other The other swerve module.
079   * @return 1 if this is greater, 0 if both are equal, -1 if other is greater.
080   */
081  @Override
082  public int compareTo(SwerveModuleVelocity other) {
083    return Double.compare(this.velocity, other.velocity);
084  }
085
086  @Override
087  public String toString() {
088    return String.format("SwerveModuleVelocity(Velocity: %.2f m/s, Angle: %s)", velocity, angle);
089  }
090
091  /**
092   * Minimize the change in heading this swerve module velocity 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 currentAngle The current module angle.
097   * @return The optimized SwerveModuleVelocity.
098   */
099  public SwerveModuleVelocity optimize(Rotation2d currentAngle) {
100    var delta = angle.minus(currentAngle);
101    if (Math.abs(delta.getDegrees()) > 90.0) {
102      return new SwerveModuleVelocity(-velocity, angle.rotateBy(Rotation2d.kPi));
103    } else {
104      return new SwerveModuleVelocity(velocity, angle);
105    }
106  }
107
108  /**
109   * Returns the linear interpolation of this SwerveModuleVelocity and another. The angle is
110   * interpolated using the shortest path between the two angles.
111   *
112   * @param endValue The end value for the interpolation.
113   * @param t How far between the two values to interpolate. This is clamped to [0, 1].
114   * @return The interpolated value.
115   */
116  @Override
117  public SwerveModuleVelocity interpolate(SwerveModuleVelocity endValue, double t) {
118    // Clamp t to [0, 1]
119    t = Math.max(0.0, Math.min(1.0, t));
120
121    // Interpolate velocity linearly
122    double interpolatedVelocity = velocity + t * (endValue.velocity - velocity);
123
124    // Interpolate angle using the shortest path
125    Rotation2d interpolatedAngle = angle.interpolate(endValue.angle, t);
126
127    return new SwerveModuleVelocity(interpolatedVelocity, interpolatedAngle);
128  }
129
130  /**
131   * Scales velocity by cosine of angle error. This scales down movement perpendicular to the
132   * desired direction of travel that can occur when modules change directions. This results in
133   * smoother driving.
134   *
135   * @param currentAngle The current module angle.
136   * @return The scaled SwerveModuleVelocity.
137   */
138  public SwerveModuleVelocity cosineScale(Rotation2d currentAngle) {
139    return new SwerveModuleVelocity(velocity * angle.minus(currentAngle).getCos(), angle);
140  }
141}