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}