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;
008import static org.wpilib.units.Units.RadiansPerSecondPerSecond;
009
010import java.util.Objects;
011import org.wpilib.math.geometry.Rotation2d;
012import org.wpilib.math.geometry.Translation2d;
013import org.wpilib.math.interpolation.Interpolatable;
014import org.wpilib.math.kinematics.proto.ChassisAccelerationsProto;
015import org.wpilib.math.kinematics.struct.ChassisAccelerationsStruct;
016import org.wpilib.math.util.MathUtil;
017import org.wpilib.units.measure.AngularAcceleration;
018import org.wpilib.units.measure.LinearAcceleration;
019import org.wpilib.util.protobuf.ProtobufSerializable;
020import org.wpilib.util.struct.StructSerializable;
021
022/**
023 * Represents robot chassis accelerations.
024 *
025 * <p>A strictly non-holonomic drivetrain, such as a differential drive, should never have an ay
026 * component because it can never move sideways. Holonomic drivetrains such as swerve and mecanum
027 * will often have all three components.
028 */
029public class ChassisAccelerations
030    implements ProtobufSerializable, StructSerializable, Interpolatable<ChassisAccelerations> {
031  /** Acceleration along the x-axis in meters per second squared. (Fwd is +) */
032  public double ax;
033
034  /** Acceleration along the y-axis in meters per second squared. (Left is +) */
035  public double ay;
036
037  /** Angular acceleration of the robot frame in radians per second squared. (CCW is +) */
038  public double alpha;
039
040  /** ChassisAccelerations struct for serialization. */
041  public static final ChassisAccelerationsStruct struct = new ChassisAccelerationsStruct();
042
043  /** ChassisAccelerations proto for serialization. */
044  public static final ChassisAccelerationsProto proto = new ChassisAccelerationsProto();
045
046  /** Constructs a ChassisAccelerations with zeros for ax, ay, and omega. */
047  public ChassisAccelerations() {}
048
049  /**
050   * Constructs a ChassisAccelerations object.
051   *
052   * @param ax Forward acceleration in meters per second squared.
053   * @param ay Sideways acceleration in meters per second squared.
054   * @param alpha Angular acceleration in radians per second squared.
055   */
056  public ChassisAccelerations(double ax, double ay, double alpha) {
057    this.ax = ax;
058    this.ay = ay;
059    this.alpha = alpha;
060  }
061
062  /**
063   * Constructs a ChassisAccelerations object.
064   *
065   * @param ax Forward acceleration.
066   * @param ay Sideways acceleration.
067   * @param alpha Angular acceleration.
068   */
069  public ChassisAccelerations(
070      LinearAcceleration ax, LinearAcceleration ay, AngularAcceleration alpha) {
071    this(
072        ax.in(MetersPerSecondPerSecond),
073        ay.in(MetersPerSecondPerSecond),
074        alpha.in(RadiansPerSecondPerSecond));
075  }
076
077  /**
078   * Converts this field-relative set of accelerations into a robot-relative ChassisAccelerations
079   * object.
080   *
081   * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is
082   *     considered to be zero when it is facing directly away from your alliance station wall.
083   *     Remember that this should be CCW positive.
084   * @return ChassisAccelerations object representing the accelerations in the robot's frame of
085   *     reference.
086   */
087  public ChassisAccelerations toRobotRelative(Rotation2d robotAngle) {
088    // CW rotation into chassis frame
089    var rotated = new Translation2d(ax, ay).rotateBy(robotAngle.unaryMinus());
090    return new ChassisAccelerations(rotated.getX(), rotated.getY(), alpha);
091  }
092
093  /**
094   * Converts this robot-relative set of accelerations into a field-relative ChassisAccelerations
095   * object.
096   *
097   * @param robotAngle The angle of the robot as measured by a gyroscope. The robot's angle is
098   *     considered to be zero when it is facing directly away from your alliance station wall.
099   *     Remember that this should be CCW positive.
100   * @return ChassisAccelerations object representing the accelerations in the field's frame of
101   *     reference.
102   */
103  public ChassisAccelerations toFieldRelative(Rotation2d robotAngle) {
104    // CCW rotation out of chassis frame
105    var rotated = new Translation2d(ax, ay).rotateBy(robotAngle);
106    return new ChassisAccelerations(rotated.getX(), rotated.getY(), alpha);
107  }
108
109  /**
110   * Adds two ChassisAccelerations and returns the sum.
111   *
112   * <p>For example, ChassisAccelerations{1.0, 0.5, 0.75} + ChassisAccelerations{2.0, 1.5, 0.25} =
113   * ChassisAccelerations{3.0, 2.0, 1.0}
114   *
115   * @param other The ChassisAccelerations to add.
116   * @return The sum of the ChassisAccelerations.
117   */
118  public ChassisAccelerations plus(ChassisAccelerations other) {
119    return new ChassisAccelerations(ax + other.ax, ay + other.ay, alpha + other.alpha);
120  }
121
122  /**
123   * Subtracts the other ChassisAccelerations from the current ChassisAccelerations and returns the
124   * difference.
125   *
126   * <p>For example, ChassisAccelerations{5.0, 4.0, 2.0} - ChassisAccelerations{1.0, 2.0, 1.0} =
127   * ChassisAccelerations{4.0, 2.0, 1.0}
128   *
129   * @param other The ChassisAccelerations to subtract.
130   * @return The difference between the two ChassisAccelerations.
131   */
132  public ChassisAccelerations minus(ChassisAccelerations other) {
133    return new ChassisAccelerations(ax - other.ax, ay - other.ay, alpha - other.alpha);
134  }
135
136  /**
137   * Returns the inverse of the current ChassisAccelerations. This is equivalent to negating all
138   * components of the ChassisAccelerations.
139   *
140   * @return The inverse of the current ChassisAccelerations.
141   */
142  public ChassisAccelerations unaryMinus() {
143    return new ChassisAccelerations(-ax, -ay, -alpha);
144  }
145
146  /**
147   * Multiplies the ChassisAccelerations by a scalar and returns the new ChassisAccelerations.
148   *
149   * <p>For example, ChassisAccelerations{2.0, 2.5, 1.0} * 2 = ChassisAccelerations{4.0, 5.0, 1.0}
150   *
151   * @param scalar The scalar to multiply by.
152   * @return The scaled ChassisAccelerations.
153   */
154  public ChassisAccelerations times(double scalar) {
155    return new ChassisAccelerations(ax * scalar, ay * scalar, alpha * scalar);
156  }
157
158  /**
159   * Divides the ChassisAccelerations by a scalar and returns the new ChassisAccelerations.
160   *
161   * <p>For example, ChassisAccelerations{2.0, 2.5, 1.0} / 2 = ChassisAccelerations{1.0, 1.25, 0.5}
162   *
163   * @param scalar The scalar to divide by.
164   * @return The scaled ChassisAccelerations.
165   */
166  public ChassisAccelerations div(double scalar) {
167    return new ChassisAccelerations(ax / scalar, ay / scalar, alpha / scalar);
168  }
169
170  @Override
171  public ChassisAccelerations interpolate(ChassisAccelerations endValue, double t) {
172    if (t <= 0) {
173      return this;
174    } else if (t >= 1) {
175      return endValue;
176    } else {
177      return new ChassisAccelerations(
178          MathUtil.lerp(this.ax, endValue.ax, t),
179          MathUtil.lerp(this.ay, endValue.ay, t),
180          MathUtil.lerp(this.alpha, endValue.alpha, t));
181    }
182  }
183
184  @Override
185  public final int hashCode() {
186    return Objects.hash(ax, ay, alpha);
187  }
188
189  @Override
190  public boolean equals(Object o) {
191    return o == this
192        || o instanceof ChassisAccelerations c && ax == c.ax && ay == c.ay && alpha == c.alpha;
193  }
194
195  @Override
196  public String toString() {
197    return String.format(
198        "ChassisAccelerations(Ax: %.2f m/s², Ay: %.2f m/s², Alpha: %.2f rad/s²)", ax, ay, alpha);
199  }
200}