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.kinematics.proto.DifferentialDriveWheelPositionsProto;
011import edu.wpi.first.math.kinematics.struct.DifferentialDriveWheelPositionsStruct;
012import edu.wpi.first.units.Distance;
013import edu.wpi.first.units.Measure;
014import java.util.Objects;
015
016/** Represents the wheel positions for a differential drive drivetrain. */
017public class DifferentialDriveWheelPositions
018    implements WheelPositions<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  /** DifferentialDriveWheelPostions struct for serialization. */
026  public static final DifferentialDriveWheelPositionsStruct struct =
027      new DifferentialDriveWheelPositionsStruct();
028
029  /** DifferentialDriveWheelPostions 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(Measure<Distance> left, Measure<Distance> right) {
051    this(left.in(Meters), right.in(Meters));
052  }
053
054  @Override
055  public boolean equals(Object obj) {
056    if (obj instanceof DifferentialDriveWheelPositions) {
057      DifferentialDriveWheelPositions other = (DifferentialDriveWheelPositions) obj;
058      return Math.abs(other.leftMeters - leftMeters) < 1E-9
059          && Math.abs(other.rightMeters - rightMeters) < 1E-9;
060    }
061    return false;
062  }
063
064  @Override
065  public int hashCode() {
066    return Objects.hash(leftMeters, rightMeters);
067  }
068
069  @Override
070  public String toString() {
071    return String.format(
072        "DifferentialDriveWheelPositions(Left: %.2f m, Right: %.2f m", leftMeters, rightMeters);
073  }
074
075  @Override
076  public DifferentialDriveWheelPositions copy() {
077    return new DifferentialDriveWheelPositions(leftMeters, rightMeters);
078  }
079
080  @Override
081  public DifferentialDriveWheelPositions interpolate(
082      DifferentialDriveWheelPositions endValue, double t) {
083    return new DifferentialDriveWheelPositions(
084        MathUtil.interpolate(this.leftMeters, endValue.leftMeters, t),
085        MathUtil.interpolate(this.rightMeters, endValue.rightMeters, t));
086  }
087}