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