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 MecanumDriveWheelPositions implements WheelPositions<MecanumDriveWheelPositions> {
011  /** Distance measured by the front left wheel. */
012  public double frontLeftMeters;
013
014  /** Distance measured by the front right wheel. */
015  public double frontRightMeters;
016
017  /** Distance measured by the rear left wheel. */
018  public double rearLeftMeters;
019
020  /** Distance measured by the rear right wheel. */
021  public double rearRightMeters;
022
023  /** Constructs a MecanumDriveWheelPositions with zeros for all member fields. */
024  public MecanumDriveWheelPositions() {}
025
026  /**
027   * Constructs a MecanumDriveWheelPositions.
028   *
029   * @param frontLeftMeters Distance measured by the front left wheel.
030   * @param frontRightMeters Distance measured by the front right wheel.
031   * @param rearLeftMeters Distance measured by the rear left wheel.
032   * @param rearRightMeters Distance measured by the rear right wheel.
033   */
034  public MecanumDriveWheelPositions(
035      double frontLeftMeters,
036      double frontRightMeters,
037      double rearLeftMeters,
038      double rearRightMeters) {
039    this.frontLeftMeters = frontLeftMeters;
040    this.frontRightMeters = frontRightMeters;
041    this.rearLeftMeters = rearLeftMeters;
042    this.rearRightMeters = rearRightMeters;
043  }
044
045  @Override
046  public boolean equals(Object obj) {
047    if (obj instanceof MecanumDriveWheelPositions) {
048      MecanumDriveWheelPositions other = (MecanumDriveWheelPositions) obj;
049      return Math.abs(other.frontLeftMeters - frontLeftMeters) < 1E-9
050          && Math.abs(other.frontRightMeters - frontRightMeters) < 1E-9
051          && Math.abs(other.rearLeftMeters - rearLeftMeters) < 1E-9
052          && Math.abs(other.rearRightMeters - rearRightMeters) < 1E-9;
053    }
054    return false;
055  }
056
057  @Override
058  public int hashCode() {
059    return Objects.hash(frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters);
060  }
061
062  @Override
063  public String toString() {
064    return String.format(
065        "MecanumDriveWheelPositions(Front Left: %.2f m, Front Right: %.2f m, "
066            + "Rear Left: %.2f m, Rear Right: %.2f m)",
067        frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters);
068  }
069
070  @Override
071  public MecanumDriveWheelPositions copy() {
072    return new MecanumDriveWheelPositions(
073        frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters);
074  }
075
076  @Override
077  public MecanumDriveWheelPositions minus(MecanumDriveWheelPositions other) {
078    return new MecanumDriveWheelPositions(
079        this.frontLeftMeters - other.frontLeftMeters,
080        this.frontRightMeters - other.frontRightMeters,
081        this.rearLeftMeters - other.rearLeftMeters,
082        this.rearRightMeters - other.rearRightMeters);
083  }
084
085  @Override
086  public MecanumDriveWheelPositions interpolate(MecanumDriveWheelPositions endValue, double t) {
087    return new MecanumDriveWheelPositions(
088        MathUtil.interpolate(this.frontLeftMeters, endValue.frontLeftMeters, t),
089        MathUtil.interpolate(this.frontRightMeters, endValue.frontRightMeters, t),
090        MathUtil.interpolate(this.rearLeftMeters, endValue.rearLeftMeters, t),
091        MathUtil.interpolate(this.rearRightMeters, endValue.rearRightMeters, t));
092  }
093}