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.MetersPerSecondPerSecond;
008
009import java.util.Objects;
010import org.wpilib.math.geometry.Rotation2d;
011import org.wpilib.math.interpolation.Interpolatable;
012import org.wpilib.math.kinematics.proto.SwerveModuleAccelerationProto;
013import org.wpilib.math.kinematics.struct.SwerveModuleAccelerationStruct;
014import org.wpilib.units.measure.LinearAcceleration;
015import org.wpilib.util.protobuf.ProtobufSerializable;
016import org.wpilib.util.struct.StructSerializable;
017
018/** Represents the accelerations of one swerve module. */
019public class SwerveModuleAcceleration
020    implements Interpolatable<SwerveModuleAcceleration>,
021        Comparable<SwerveModuleAcceleration>,
022        ProtobufSerializable,
023        StructSerializable {
024  /** Acceleration of the wheel of the module in meters per second squared. */
025  public double acceleration;
026
027  /** Angle of the acceleration vector. */
028  public Rotation2d angle = new Rotation2d();
029
030  /** SwerveModuleAccelerations protobuf for serialization. */
031  public static final SwerveModuleAccelerationProto proto = new SwerveModuleAccelerationProto();
032
033  /** SwerveModuleAccelerations struct for serialization. */
034  public static final SwerveModuleAccelerationStruct struct = new SwerveModuleAccelerationStruct();
035
036  /** Constructs a SwerveModuleAccelerations with zeros for acceleration and angle. */
037  public SwerveModuleAcceleration() {}
038
039  /**
040   * Constructs a SwerveModuleAccelerations.
041   *
042   * @param acceleration The acceleration of the wheel of the module in meters per second squared.
043   * @param angle The angle of the acceleration vector.
044   */
045  public SwerveModuleAcceleration(double acceleration, Rotation2d angle) {
046    this.acceleration = acceleration;
047    this.angle = angle;
048  }
049
050  /**
051   * Constructs a SwerveModuleAccelerations.
052   *
053   * @param acceleration The acceleration of the wheel of the module.
054   * @param angle The angle of the acceleration vector.
055   */
056  public SwerveModuleAcceleration(LinearAcceleration acceleration, Rotation2d angle) {
057    this(acceleration.in(MetersPerSecondPerSecond), angle);
058  }
059
060  /**
061   * Returns the linear interpolation of this SwerveModuleAccelerations and another.
062   *
063   * @param endValue The end value for the interpolation.
064   * @param t How far between the two values to interpolate. This is clamped to [0, 1].
065   * @return The interpolated value.
066   */
067  @Override
068  public SwerveModuleAcceleration interpolate(SwerveModuleAcceleration endValue, double t) {
069    // Clamp t to [0, 1]
070    t = Math.max(0.0, Math.min(1.0, t));
071
072    return new SwerveModuleAcceleration(
073        acceleration + t * (endValue.acceleration - acceleration),
074        angle.interpolate(endValue.angle, t));
075  }
076
077  @Override
078  public boolean equals(Object obj) {
079    return obj instanceof SwerveModuleAcceleration other
080        && Math.abs(other.acceleration - acceleration) < 1E-9
081        && angle.equals(other.angle);
082  }
083
084  @Override
085  public int hashCode() {
086    return Objects.hash(acceleration, angle);
087  }
088
089  /**
090   * Compares two swerve module accelerations. One swerve module is "greater" than the other if its
091   * acceleration magnitude is higher than the other.
092   *
093   * @param other The other swerve module.
094   * @return 1 if this is greater, 0 if both are equal, -1 if other is greater.
095   */
096  @Override
097  public int compareTo(SwerveModuleAcceleration other) {
098    return Double.compare(this.acceleration, other.acceleration);
099  }
100
101  @Override
102  public String toString() {
103    return String.format(
104        "SwerveModuleAccelerations(Acceleration: %.2f m/s², Angle: %s)", acceleration, angle);
105  }
106}