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