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.Meters;
008
009import edu.wpi.first.math.MathUtil;
010import edu.wpi.first.math.interpolation.Interpolatable;
011import edu.wpi.first.math.kinematics.proto.DifferentialDriveWheelPositionsProto;
012import edu.wpi.first.math.kinematics.struct.DifferentialDriveWheelPositionsStruct;
013import edu.wpi.first.units.measure.Distance;
014import edu.wpi.first.util.protobuf.ProtobufSerializable;
015import edu.wpi.first.util.struct.StructSerializable;
016import java.util.Objects;
017
018/** Represents the wheel positions for a differential drive drivetrain. */
019public class DifferentialDriveWheelPositions
020    implements StructSerializable,
021        ProtobufSerializable,
022        Interpolatable<DifferentialDriveWheelPositions> {
023  /** Distance measured by the left side in meters. */
024  public double left;
025
026  /** Distance measured by the right side in meters. */
027  public double right;
028
029  /** DifferentialDriveWheelPositions struct for serialization. */
030  public static final DifferentialDriveWheelPositionsStruct struct =
031      new DifferentialDriveWheelPositionsStruct();
032
033  /** DifferentialDriveWheelPositions struct for serialization. */
034  public static final DifferentialDriveWheelPositionsProto proto =
035      new DifferentialDriveWheelPositionsProto();
036
037  /**
038   * Constructs a DifferentialDriveWheelPositions.
039   *
040   * @param left Distance measured by the left side in meters.
041   * @param right Distance measured by the right side in meters.
042   */
043  public DifferentialDriveWheelPositions(double left, double right) {
044    this.left = left;
045    this.right = right;
046  }
047
048  /**
049   * Constructs a DifferentialDriveWheelPositions.
050   *
051   * @param left Distance measured by the left side in meters.
052   * @param right Distance measured by the right side in meters.
053   */
054  public DifferentialDriveWheelPositions(Distance left, Distance right) {
055    this(left.in(Meters), right.in(Meters));
056  }
057
058  @Override
059  public boolean equals(Object obj) {
060    return obj instanceof DifferentialDriveWheelPositions other
061        && Math.abs(other.left - left) < 1E-9
062        && Math.abs(other.right - right) < 1E-9;
063  }
064
065  @Override
066  public int hashCode() {
067    return Objects.hash(left, right);
068  }
069
070  @Override
071  public String toString() {
072    return String.format(
073        "DifferentialDriveWheelPositions(Left: %.2f m, Right: %.2f m", left, right);
074  }
075
076  @Override
077  public DifferentialDriveWheelPositions interpolate(
078      DifferentialDriveWheelPositions endValue, double t) {
079    return new DifferentialDriveWheelPositions(
080        MathUtil.interpolate(this.left, endValue.left, t),
081        MathUtil.interpolate(this.right, endValue.right, t));
082  }
083}