diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java index d67a781647..c8a443904f 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveKinematics.java @@ -8,7 +8,6 @@ import static edu.wpi.first.units.Units.Meters; import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.MathUsageId; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.proto.DifferentialDriveKinematicsProto; import edu.wpi.first.math.kinematics.struct.DifferentialDriveKinematicsStruct; @@ -132,8 +131,6 @@ public class DifferentialDriveKinematics DifferentialDriveWheelPositions startValue, DifferentialDriveWheelPositions endValue, double t) { - return new DifferentialDriveWheelPositions( - MathUtil.interpolate(startValue.leftMeters, endValue.leftMeters, t), - MathUtil.interpolate(startValue.rightMeters, endValue.rightMeters, t)); + return startValue.interpolate(endValue, t); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java index ef4e5b0e92..d97285f646 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/DifferentialDriveWheelPositions.java @@ -7,6 +7,7 @@ package edu.wpi.first.math.kinematics; import static edu.wpi.first.units.Units.Meters; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.kinematics.proto.DifferentialDriveWheelPositionsProto; import edu.wpi.first.math.kinematics.struct.DifferentialDriveWheelPositionsStruct; import edu.wpi.first.units.Distance; @@ -15,7 +16,7 @@ import java.util.Objects; /** Represents the wheel positions for a differential drive drivetrain. */ public class DifferentialDriveWheelPositions - implements WheelPositions { + implements Interpolatable { /** Distance measured by the left side. */ public double leftMeters; @@ -69,11 +70,6 @@ public class DifferentialDriveWheelPositions "DifferentialDriveWheelPositions(Left: %.2f m, Right: %.2f m", leftMeters, rightMeters); } - @Override - public DifferentialDriveWheelPositions copy() { - return new DifferentialDriveWheelPositions(leftMeters, rightMeters); - } - @Override public DifferentialDriveWheelPositions interpolate( DifferentialDriveWheelPositions endValue, double t) { diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java index 26108088c6..0135af24d6 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveKinematics.java @@ -6,7 +6,6 @@ package edu.wpi.first.math.kinematics; import edu.wpi.first.math.MathSharedStore; import edu.wpi.first.math.MathUsageId; -import edu.wpi.first.math.MathUtil; import edu.wpi.first.math.geometry.Translation2d; import edu.wpi.first.math.geometry.Twist2d; import edu.wpi.first.math.kinematics.proto.MecanumDriveKinematicsProto; @@ -277,10 +276,6 @@ public class MecanumDriveKinematics @Override public MecanumDriveWheelPositions interpolate( MecanumDriveWheelPositions startValue, MecanumDriveWheelPositions endValue, double t) { - return new MecanumDriveWheelPositions( - MathUtil.interpolate(startValue.frontLeftMeters, endValue.frontLeftMeters, t), - MathUtil.interpolate(startValue.frontRightMeters, endValue.frontRightMeters, t), - MathUtil.interpolate(startValue.rearLeftMeters, endValue.rearLeftMeters, t), - MathUtil.interpolate(startValue.rearRightMeters, endValue.rearRightMeters, t)); + return startValue.interpolate(endValue, t); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java index e193dbdc4b..1dc14a8292 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java +++ b/wpimath/src/main/java/edu/wpi/first/math/kinematics/MecanumDriveWheelPositions.java @@ -7,6 +7,7 @@ package edu.wpi.first.math.kinematics; import static edu.wpi.first.units.Units.Meters; import edu.wpi.first.math.MathUtil; +import edu.wpi.first.math.interpolation.Interpolatable; import edu.wpi.first.math.kinematics.proto.MecanumDriveWheelPositionsProto; import edu.wpi.first.math.kinematics.struct.MecanumDriveWheelPositionsStruct; import edu.wpi.first.units.Distance; @@ -17,7 +18,7 @@ import java.util.Objects; /** Represents the wheel positions for a mecanum drive drivetrain. */ public class MecanumDriveWheelPositions - implements WheelPositions, + implements Interpolatable, ProtobufSerializable, StructSerializable { /** Distance measured by the front left wheel. */ @@ -99,12 +100,6 @@ public class MecanumDriveWheelPositions frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters); } - @Override - public MecanumDriveWheelPositions copy() { - return new MecanumDriveWheelPositions( - frontLeftMeters, frontRightMeters, rearLeftMeters, rearRightMeters); - } - @Override public MecanumDriveWheelPositions interpolate(MecanumDriveWheelPositions endValue, double t) { return new MecanumDriveWheelPositions( diff --git a/wpimath/src/main/java/edu/wpi/first/math/kinematics/WheelPositions.java b/wpimath/src/main/java/edu/wpi/first/math/kinematics/WheelPositions.java deleted file mode 100644 index c0167e3e86..0000000000 --- a/wpimath/src/main/java/edu/wpi/first/math/kinematics/WheelPositions.java +++ /dev/null @@ -1,21 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -package edu.wpi.first.math.kinematics; - -import edu.wpi.first.math.interpolation.Interpolatable; - -/** - * Interface for wheel positions. - * - * @param Wheel positions type. - */ -public interface WheelPositions> extends Interpolatable { - /** - * Returns a copy of this instance. - * - * @return A copy. - */ - T copy(); -} diff --git a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h index da9f4f3ee6..f75a5afec7 100644 --- a/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h +++ b/wpimath/src/main/native/include/frc/estimator/PoseEstimator.h @@ -19,7 +19,6 @@ #include "frc/interpolation/TimeInterpolatableBuffer.h" #include "frc/kinematics/Kinematics.h" #include "frc/kinematics/Odometry.h" -#include "frc/kinematics/WheelPositions.h" #include "units/time.h" #include "wpimath/MathShared.h" diff --git a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h index a1be448c7b..fbbb519182 100644 --- a/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/DifferentialDriveKinematics.h @@ -91,8 +91,7 @@ class WPILIB_DLLEXPORT DifferentialDriveKinematics DifferentialDriveWheelPositions Interpolate( const DifferentialDriveWheelPositions& start, const DifferentialDriveWheelPositions& end, double t) const override { - return {wpi::Lerp(start.left, end.left, t), - wpi::Lerp(start.right, end.right, t)}; + return start.Interpolate(end, t); } /// Differential drive trackwidth. diff --git a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h index 6c3d664496..218908fd31 100644 --- a/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h +++ b/wpimath/src/main/native/include/frc/kinematics/MecanumDriveKinematics.h @@ -169,10 +169,7 @@ class WPILIB_DLLEXPORT MecanumDriveKinematics MecanumDriveWheelPositions Interpolate( const MecanumDriveWheelPositions& start, const MecanumDriveWheelPositions& end, double t) const override { - return {wpi::Lerp(start.frontLeft, end.frontLeft, t), - wpi::Lerp(start.frontRight, end.frontRight, t), - wpi::Lerp(start.rearLeft, end.rearLeft, t), - wpi::Lerp(start.rearRight, end.rearRight, t)}; + return start.Interpolate(end, t); } private: diff --git a/wpimath/src/main/native/include/frc/kinematics/Odometry.h b/wpimath/src/main/native/include/frc/kinematics/Odometry.h index c0daea0e3e..e5a8e0f618 100644 --- a/wpimath/src/main/native/include/frc/kinematics/Odometry.h +++ b/wpimath/src/main/native/include/frc/kinematics/Odometry.h @@ -10,7 +10,6 @@ #include "frc/geometry/Rotation2d.h" #include "frc/geometry/Translation2d.h" #include "frc/kinematics/Kinematics.h" -#include "frc/kinematics/WheelPositions.h" namespace frc { /** diff --git a/wpimath/src/main/native/include/frc/kinematics/WheelPositions.h b/wpimath/src/main/native/include/frc/kinematics/WheelPositions.h deleted file mode 100644 index 8867f6662a..0000000000 --- a/wpimath/src/main/native/include/frc/kinematics/WheelPositions.h +++ /dev/null @@ -1,15 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#pragma once - -#include - -namespace frc { -template -concept WheelPositions = - std::copy_constructible && requires(T a, T b, double t) { - { a.Interpolate(b, t) } -> std::convertible_to; - }; -} // namespace frc