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}