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 java.util.Arrays; 008import java.util.Objects; 009 010public class SwerveDriveWheelPositions implements WheelPositions<SwerveDriveWheelPositions> { 011 public SwerveModulePosition[] positions; 012 013 /** 014 * Creates a new SwerveDriveWheelPositions instance. 015 * 016 * @param positions The swerve module positions. This will be deeply copied. 017 */ 018 public SwerveDriveWheelPositions(SwerveModulePosition[] positions) { 019 this.positions = new SwerveModulePosition[positions.length]; 020 for (int i = 0; i < positions.length; i++) { 021 this.positions[i] = positions[i].copy(); 022 } 023 } 024 025 @Override 026 public boolean equals(Object obj) { 027 if (obj instanceof SwerveDriveWheelPositions) { 028 SwerveDriveWheelPositions other = (SwerveDriveWheelPositions) obj; 029 return Arrays.equals(this.positions, other.positions); 030 } 031 return false; 032 } 033 034 @Override 035 public int hashCode() { 036 // Cast to interpret positions as single argument, not array of the arguments 037 return Objects.hash((Object) positions); 038 } 039 040 @Override 041 public String toString() { 042 return String.format("SwerveDriveWheelPositions(%s)", Arrays.toString(positions)); 043 } 044 045 @Override 046 public SwerveDriveWheelPositions copy() { 047 return new SwerveDriveWheelPositions(positions); 048 } 049 050 @Override 051 public SwerveDriveWheelPositions interpolate(SwerveDriveWheelPositions endValue, double t) { 052 if (endValue.positions.length != positions.length) { 053 throw new IllegalArgumentException("Inconsistent number of modules!"); 054 } 055 var newPositions = new SwerveModulePosition[positions.length]; 056 for (int i = 0; i < positions.length; i++) { 057 newPositions[i] = positions[i].interpolate(endValue.positions[i], t); 058 } 059 return new SwerveDriveWheelPositions(newPositions); 060 } 061}