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