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 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 WheelPositions<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      Measure<Distance> frontLeft,
074      Measure<Distance> frontRight,
075      Measure<Distance> rearLeft,
076      Measure<Distance> rearRight) {
077    this(frontLeft.in(Meters), frontRight.in(Meters), rearLeft.in(Meters), rearRight.in(Meters));
078  }
079
080  @Override
081  public boolean equals(Object obj) {
082    if (obj instanceof MecanumDriveWheelPositions) {
083      MecanumDriveWheelPositions other = (MecanumDriveWheelPositions) obj;
084      return Math.abs(other.frontLeftMeters - frontLeftMeters) < 1E-9
085          && Math.abs(other.frontRightMeters - frontRightMeters) < 1E-9
086          && Math.abs(other.rearLeftMeters - rearLeftMeters) < 1E-9
087          && Math.abs(other.rearRightMeters - rearRightMeters) < 1E-9;
088    }
089    return false;
090  }
091
092  @Override
093  public int hashCode() {
094    return Objects.hash(frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters);
095  }
096
097  @Override
098  public String toString() {
099    return String.format(
100        "MecanumDriveWheelPositions(Front Left: %.2f m, Front Right: %.2f m, "
101            + "Rear Left: %.2f m, Rear Right: %.2f m)",
102        frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters);
103  }
104
105  @Override
106  public MecanumDriveWheelPositions copy() {
107    return new MecanumDriveWheelPositions(
108        frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters);
109  }
110
111  @Override
112  public MecanumDriveWheelPositions interpolate(MecanumDriveWheelPositions endValue, double t) {
113    return new MecanumDriveWheelPositions(
114        MathUtil.interpolate(this.frontLeftMeters, endValue.frontLeftMeters, t),
115        MathUtil.interpolate(this.frontRightMeters, endValue.frontRightMeters, t),
116        MathUtil.interpolate(this.rearLeftMeters, endValue.rearLeftMeters, t),
117        MathUtil.interpolate(this.rearRightMeters, endValue.rearRightMeters, t));
118  }
119}