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 in meters. */
024  public double frontLeft;
025
026  /** Distance measured by the front right wheel in meters. */
027  public double frontRight;
028
029  /** Distance measured by the rear left wheel in meters. */
030  public double rearLeft;
031
032  /** Distance measured by the rear right wheel in meters. */
033  public double rearRight;
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 frontLeft Distance measured by the front left wheel in meters.
049   * @param frontRight Distance measured by the front right wheel in meters.
050   * @param rearLeft Distance measured by the rear left wheel in meters.
051   * @param rearRight Distance measured by the rear right wheel in meters.
052   */
053  public MecanumDriveWheelPositions(
054      double frontLeft, double frontRight, double rearLeft, double rearRight) {
055    this.frontLeft = frontLeft;
056    this.frontRight = frontRight;
057    this.rearLeft = rearLeft;
058    this.rearRight = rearRight;
059  }
060
061  /**
062   * Constructs a MecanumDriveWheelPositions.
063   *
064   * @param frontLeft Distance measured by the front left wheel in meters.
065   * @param frontRight Distance measured by the front right wheel in meters.
066   * @param rearLeft Distance measured by the rear left wheel in meters.
067   * @param rearRight Distance measured by the rear right wheel in meters.
068   */
069  public MecanumDriveWheelPositions(
070      Distance frontLeft, Distance frontRight, Distance rearLeft, Distance rearRight) {
071    this(frontLeft.in(Meters), frontRight.in(Meters), rearLeft.in(Meters), rearRight.in(Meters));
072  }
073
074  @Override
075  public boolean equals(Object obj) {
076    return obj instanceof MecanumDriveWheelPositions other
077        && Math.abs(other.frontLeft - frontLeft) < 1E-9
078        && Math.abs(other.frontRight - frontRight) < 1E-9
079        && Math.abs(other.rearLeft - rearLeft) < 1E-9
080        && Math.abs(other.rearRight - rearRight) < 1E-9;
081  }
082
083  @Override
084  public int hashCode() {
085    return Objects.hash(frontLeft, frontRight, rearLeft, rearRight);
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        frontLeft, frontRight, rearLeft, rearRight);
094  }
095
096  @Override
097  public MecanumDriveWheelPositions interpolate(MecanumDriveWheelPositions endValue, double t) {
098    return new MecanumDriveWheelPositions(
099        MathUtil.interpolate(this.frontLeft, endValue.frontLeft, t),
100        MathUtil.interpolate(this.frontRight, endValue.frontRight, t),
101        MathUtil.interpolate(this.rearLeft, endValue.rearLeft, t),
102        MathUtil.interpolate(this.rearRight, endValue.rearRight, t));
103  }
104}