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