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 org.wpilib.math.interpolation.Interpolatable;
010import org.wpilib.math.kinematics.proto.MecanumDriveWheelAccelerationsProto;
011import org.wpilib.math.kinematics.struct.MecanumDriveWheelAccelerationsStruct;
012import org.wpilib.units.measure.LinearAcceleration;
013import org.wpilib.util.protobuf.ProtobufSerializable;
014import org.wpilib.util.struct.StructSerializable;
015
016/** Represents the wheel accelerations for a mecanum drive drivetrain. */
017public class MecanumDriveWheelAccelerations
018    implements Interpolatable<MecanumDriveWheelAccelerations>,
019        ProtobufSerializable,
020        StructSerializable {
021  /** Acceleration of the front left wheel in meters per second squared. */
022  public double frontLeft;
023
024  /** Acceleration of the front right wheel in meters per second squared. */
025  public double frontRight;
026
027  /** Acceleration of the rear left wheel in meters per second squared. */
028  public double rearLeft;
029
030  /** Acceleration of the rear right wheel in meters per second squared. */
031  public double rearRight;
032
033  /** MecanumDriveWheelAccelerations protobuf for serialization. */
034  public static final MecanumDriveWheelAccelerationsProto proto =
035      new MecanumDriveWheelAccelerationsProto();
036
037  /** MecanumDriveWheelAccelerations struct for serialization. */
038  public static final MecanumDriveWheelAccelerationsStruct struct =
039      new MecanumDriveWheelAccelerationsStruct();
040
041  /** Constructs a MecanumDriveWheelAccelerations with zeros for all member fields. */
042  public MecanumDriveWheelAccelerations() {}
043
044  /**
045   * Constructs a MecanumDriveWheelAccelerations.
046   *
047   * @param frontLeft Acceleration of the front left wheel in meters per second squared.
048   * @param frontRight Acceleration of the front right wheel in meters per second squared.
049   * @param rearLeft Acceleration of the rear left wheel in meters per second squared.
050   * @param rearRight Acceleration of the rear right wheel in meters per second squared.
051   */
052  public MecanumDriveWheelAccelerations(
053      double frontLeft, double frontRight, double rearLeft, double rearRight) {
054    this.frontLeft = frontLeft;
055    this.frontRight = frontRight;
056    this.rearLeft = rearLeft;
057    this.rearRight = rearRight;
058  }
059
060  /**
061   * Constructs a MecanumDriveWheelAccelerations.
062   *
063   * @param frontLeft Acceleration of the front left wheel in meters per second squared.
064   * @param frontRight Acceleration of the front right wheel in meters per second squared.
065   * @param rearLeft Acceleration of the rear left wheel in meters per second squared.
066   * @param rearRight Acceleration of the rear right wheel in meters per second squared.
067   */
068  public MecanumDriveWheelAccelerations(
069      LinearAcceleration frontLeft,
070      LinearAcceleration frontRight,
071      LinearAcceleration rearLeft,
072      LinearAcceleration rearRight) {
073    this(
074        frontLeft.in(MetersPerSecondPerSecond),
075        frontRight.in(MetersPerSecondPerSecond),
076        rearLeft.in(MetersPerSecondPerSecond),
077        rearRight.in(MetersPerSecondPerSecond));
078  }
079
080  /**
081   * Adds two MecanumDriveWheelAccelerations and returns the sum.
082   *
083   * <p>For example, MecanumDriveWheelAccelerations{1.0, 0.5, 2.0, 1.5} +
084   * MecanumDriveWheelAccelerations{2.0, 1.5, 0.5, 1.0} = MecanumDriveWheelAccelerations{3.0, 2.0,
085   * 2.5, 2.5}
086   *
087   * @param other The MecanumDriveWheelAccelerations to add.
088   * @return The sum of the MecanumDriveWheelAccelerations.
089   */
090  public MecanumDriveWheelAccelerations plus(MecanumDriveWheelAccelerations other) {
091    return new MecanumDriveWheelAccelerations(
092        frontLeft + other.frontLeft,
093        frontRight + other.frontRight,
094        rearLeft + other.rearLeft,
095        rearRight + other.rearRight);
096  }
097
098  /**
099   * Subtracts the other MecanumDriveWheelAccelerations from the current
100   * MecanumDriveWheelAccelerations and returns the difference.
101   *
102   * <p>For example, MecanumDriveWheelAccelerations{5.0, 4.0, 6.0, 2.5} -
103   * MecanumDriveWheelAccelerations{1.0, 2.0, 3.0, 0.5} = MecanumDriveWheelAccelerations{4.0, 2.0,
104   * 3.0, 2.0}
105   *
106   * @param other The MecanumDriveWheelAccelerations to subtract.
107   * @return The difference between the two MecanumDriveWheelAccelerations.
108   */
109  public MecanumDriveWheelAccelerations minus(MecanumDriveWheelAccelerations other) {
110    return new MecanumDriveWheelAccelerations(
111        frontLeft - other.frontLeft,
112        frontRight - other.frontRight,
113        rearLeft - other.rearLeft,
114        rearRight - other.rearRight);
115  }
116
117  /**
118   * Returns the inverse of the current MecanumDriveWheelAccelerations. This is equivalent to
119   * negating all components of the MecanumDriveWheelAccelerations.
120   *
121   * @return The inverse of the current MecanumDriveWheelAccelerations.
122   */
123  public MecanumDriveWheelAccelerations unaryMinus() {
124    return new MecanumDriveWheelAccelerations(-frontLeft, -frontRight, -rearLeft, -rearRight);
125  }
126
127  /**
128   * Multiplies the MecanumDriveWheelAccelerations by a scalar and returns the new
129   * MecanumDriveWheelAccelerations.
130   *
131   * <p>For example, MecanumDriveWheelAccelerations{2.0, 2.5, 3.0, 3.5} * 2 =
132   * MecanumDriveWheelAccelerations{4.0, 5.0, 6.0, 7.0}
133   *
134   * @param scalar The scalar to multiply by.
135   * @return The scaled MecanumDriveWheelAccelerations.
136   */
137  public MecanumDriveWheelAccelerations times(double scalar) {
138    return new MecanumDriveWheelAccelerations(
139        frontLeft * scalar, frontRight * scalar, rearLeft * scalar, rearRight * scalar);
140  }
141
142  /**
143   * Divides the MecanumDriveWheelAccelerations by a scalar and returns the new
144   * MecanumDriveWheelAccelerations.
145   *
146   * <p>For example, MecanumDriveWheelAccelerations{2.0, 2.5, 1.5, 1.0} / 2 =
147   * MecanumDriveWheelAccelerations{1.0, 1.25, 0.75, 0.5}
148   *
149   * @param scalar The scalar to divide by.
150   * @return The scaled MecanumDriveWheelAccelerations.
151   */
152  public MecanumDriveWheelAccelerations div(double scalar) {
153    return new MecanumDriveWheelAccelerations(
154        frontLeft / scalar, frontRight / scalar, rearLeft / scalar, rearRight / scalar);
155  }
156
157  /**
158   * Returns the linear interpolation of this MecanumDriveWheelAccelerations and another.
159   *
160   * @param endValue The end value for the interpolation.
161   * @param t How far between the two values to interpolate. This is clamped to [0, 1].
162   * @return The interpolated value.
163   */
164  @Override
165  public MecanumDriveWheelAccelerations interpolate(
166      MecanumDriveWheelAccelerations endValue, double t) {
167    // Clamp t to [0, 1]
168    t = Math.max(0.0, Math.min(1.0, t));
169
170    return new MecanumDriveWheelAccelerations(
171        frontLeft + t * (endValue.frontLeft - frontLeft),
172        frontRight + t * (endValue.frontRight - frontRight),
173        rearLeft + t * (endValue.rearLeft - rearLeft),
174        rearRight + t * (endValue.rearRight - rearRight));
175  }
176
177  @Override
178  public String toString() {
179    return String.format(
180        "MecanumDriveWheelAccelerations(Front Left: %.2f m/s², Front Right: %.2f m/s², "
181            + "Rear Left: %.2f m/s², Rear Right: %.2f m/s²)",
182        frontLeft, frontRight, rearLeft, rearRight);
183  }
184}