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