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.units.Distance; 011import edu.wpi.first.units.Measure; 012import java.util.Objects; 013 014public class DifferentialDriveWheelPositions 015 implements WheelPositions<DifferentialDriveWheelPositions> { 016 /** Distance measured by the left side. */ 017 public double leftMeters; 018 019 /** Distance measured by the right side. */ 020 public double rightMeters; 021 022 /** 023 * Constructs a DifferentialDriveWheelPositions. 024 * 025 * @param leftMeters Distance measured by the left side. 026 * @param rightMeters Distance measured by the right side. 027 */ 028 public DifferentialDriveWheelPositions(double leftMeters, double rightMeters) { 029 this.leftMeters = leftMeters; 030 this.rightMeters = rightMeters; 031 } 032 033 /** 034 * Constructs a DifferentialDriveWheelPositions. 035 * 036 * @param left Distance measured by the left side. 037 * @param right Distance measured by the right side. 038 */ 039 public DifferentialDriveWheelPositions(Measure<Distance> left, Measure<Distance> right) { 040 this(left.in(Meters), right.in(Meters)); 041 } 042 043 @Override 044 public boolean equals(Object obj) { 045 if (obj instanceof DifferentialDriveWheelPositions) { 046 DifferentialDriveWheelPositions other = (DifferentialDriveWheelPositions) obj; 047 return Math.abs(other.leftMeters - leftMeters) < 1E-9 048 && Math.abs(other.rightMeters - rightMeters) < 1E-9; 049 } 050 return false; 051 } 052 053 @Override 054 public int hashCode() { 055 return Objects.hash(leftMeters, rightMeters); 056 } 057 058 @Override 059 public String toString() { 060 return String.format( 061 "DifferentialDriveWheelPositions(Left: %.2f m, Right: %.2f m", leftMeters, rightMeters); 062 } 063 064 @Override 065 public DifferentialDriveWheelPositions copy() { 066 return new DifferentialDriveWheelPositions(leftMeters, rightMeters); 067 } 068 069 @Override 070 public DifferentialDriveWheelPositions interpolate( 071 DifferentialDriveWheelPositions endValue, double t) { 072 return new DifferentialDriveWheelPositions( 073 MathUtil.interpolate(this.leftMeters, endValue.leftMeters, t), 074 MathUtil.interpolate(this.rightMeters, endValue.rightMeters, t)); 075 } 076}