From ce1a7d698a8cdf0a36c991749e88cffe35265fff Mon Sep 17 00:00:00 2001 From: Tyler Veness Date: Sun, 1 May 2022 11:01:20 -0700 Subject: [PATCH] [wpimath] Refactor WheelVoltages inner class to a separate file (#4203) --- .../DifferentialDriveAccelerationLimiter.java | 18 ++---------- .../DifferentialDriveWheelVoltages.java | 19 +++++++++++++ .../LTVDifferentialDriveController.java | 20 ++----------- .../DifferentialDriveAccelerationLimiter.cpp | 3 +- .../LTVDifferentialDriveController.cpp | 9 +++--- .../DifferentialDriveAccelerationLimiter.h | 17 ++++------- .../DifferentialDriveWheelVoltages.h | 19 +++++++++++++ .../LTVDifferentialDriveController.h | 28 +++++++------------ 8 files changed, 63 insertions(+), 70 deletions(-) create mode 100644 wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveWheelVoltages.java create mode 100644 wpimath/src/main/native/include/frc/controller/DifferentialDriveWheelVoltages.h diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiter.java b/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiter.java index 8e3a35b618..94d2c8fd33 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiter.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveAccelerationLimiter.java @@ -21,20 +21,6 @@ public class DifferentialDriveAccelerationLimiter { private final double m_maxLinearAccel; private final double m_maxAngularAccel; - /** Motor voltages for a differential drive. */ - @SuppressWarnings("MemberName") - public static class WheelVoltages { - public double left; - public double right; - - private WheelVoltages() {} - - public WheelVoltages(double left, double right) { - this.left = left; - this.right = right; - } - } - /** * Constructs a DifferentialDriveAccelerationLimiter. * @@ -64,7 +50,7 @@ public class DifferentialDriveAccelerationLimiter { * @return The constrained wheel voltages. */ @SuppressWarnings("LocalVariableName") - public WheelVoltages calculate( + public DifferentialDriveWheelVoltages calculate( double leftVelocity, double rightVelocity, double leftVoltage, double rightVoltage) { var u = new MatBuilder<>(Nat.N2(), Nat.N1()).fill(leftVoltage, rightVoltage); @@ -99,6 +85,6 @@ public class DifferentialDriveAccelerationLimiter { // u = B⁻¹(dx/dt - Ax) u = m_system.getB().solve(dxdt.minus(m_system.getA().times(x))); - return new WheelVoltages(u.get(0, 0), u.get(1, 0)); + return new DifferentialDriveWheelVoltages(u.get(0, 0), u.get(1, 0)); } } diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveWheelVoltages.java b/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveWheelVoltages.java new file mode 100644 index 0000000000..55f12b9f6d --- /dev/null +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/DifferentialDriveWheelVoltages.java @@ -0,0 +1,19 @@ +// 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.controller; + +/** Motor voltages for a differential drive. */ +@SuppressWarnings("MemberName") +public class DifferentialDriveWheelVoltages { + public double left; + public double right; + + public DifferentialDriveWheelVoltages() {} + + public DifferentialDriveWheelVoltages(double left, double right) { + this.left = left; + this.right = right; + } +} diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java index 5b87aa6cd1..b809790536 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/LTVDifferentialDriveController.java @@ -37,20 +37,6 @@ public class LTVDifferentialDriveController { private Matrix m_error = new Matrix<>(Nat.N5(), Nat.N1()); private Matrix m_tolerance = new Matrix<>(Nat.N5(), Nat.N1()); - /** Motor voltages for a differential drive. */ - @SuppressWarnings("MemberName") - public static class WheelVoltages { - public double left; - public double right; - - public WheelVoltages() {} - - public WheelVoltages(double left, double right) { - this.left = left; - this.right = right; - } - } - /** States of the drivetrain system. */ enum State { kX(0), @@ -204,7 +190,7 @@ public class LTVDifferentialDriveController { * @return Left and right output voltages of the LTV controller. */ @SuppressWarnings("LocalVariableName") - public WheelVoltages calculate( + public DifferentialDriveWheelVoltages calculate( Pose2d currentPose, double leftVelocity, double rightVelocity, @@ -245,7 +231,7 @@ public class LTVDifferentialDriveController { var u = K.times(inRobotFrame).times(m_error); - return new WheelVoltages(u.get(0, 0), u.get(1, 0)); + return new DifferentialDriveWheelVoltages(u.get(0, 0), u.get(1, 0)); } /** @@ -260,7 +246,7 @@ public class LTVDifferentialDriveController { * @param desiredState The desired pose, linear velocity, and angular velocity from a trajectory. * @return The left and right output voltages of the LTV controller. */ - public WheelVoltages calculate( + public DifferentialDriveWheelVoltages calculate( Pose2d currentPose, double leftVelocity, double rightVelocity, diff --git a/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp b/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp index 5727d87f90..b0eb440b34 100644 --- a/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp +++ b/wpimath/src/main/native/cpp/controller/DifferentialDriveAccelerationLimiter.cpp @@ -19,8 +19,7 @@ DifferentialDriveAccelerationLimiter::DifferentialDriveAccelerationLimiter( m_maxLinearAccel{maxLinearAccel}, m_maxAngularAccel{maxAngularAccel} {} -DifferentialDriveAccelerationLimiter::WheelVoltages -DifferentialDriveAccelerationLimiter::Calculate( +DifferentialDriveWheelVoltages DifferentialDriveAccelerationLimiter::Calculate( units::meters_per_second_t leftVelocity, units::meters_per_second_t rightVelocity, units::volt_t leftVoltage, units::volt_t rightVoltage) { diff --git a/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp b/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp index b596523bcf..cd29dedf1c 100644 --- a/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp +++ b/wpimath/src/main/native/cpp/controller/LTVDifferentialDriveController.cpp @@ -94,8 +94,7 @@ void LTVDifferentialDriveController::SetTolerance( leftVelocityTolerance.value(), rightVelocityTolerance.value()}; } -LTVDifferentialDriveController::WheelVoltages -LTVDifferentialDriveController::Calculate( +DifferentialDriveWheelVoltages LTVDifferentialDriveController::Calculate( const Pose2d& currentPose, units::meters_per_second_t leftVelocity, units::meters_per_second_t rightVelocity, const Pose2d& poseRef, units::meters_per_second_t leftVelocityRef, @@ -124,11 +123,11 @@ LTVDifferentialDriveController::Calculate( Vectord<2> u = K * inRobotFrame * m_error; - return WheelVoltages{units::volt_t{u(0)}, units::volt_t{u(1)}}; + return DifferentialDriveWheelVoltages{units::volt_t{u(0)}, + units::volt_t{u(1)}}; } -LTVDifferentialDriveController::WheelVoltages -LTVDifferentialDriveController::Calculate( +DifferentialDriveWheelVoltages LTVDifferentialDriveController::Calculate( const Pose2d& currentPose, units::meters_per_second_t leftVelocity, units::meters_per_second_t rightVelocity, const Trajectory::State& desiredState) { diff --git a/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h b/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h index d2173c59ba..34370f4b15 100644 --- a/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h +++ b/wpimath/src/main/native/include/frc/controller/DifferentialDriveAccelerationLimiter.h @@ -7,6 +7,7 @@ #include #include "Eigen/Core" +#include "frc/controller/DifferentialDriveWheelVoltages.h" #include "frc/system/LinearSystem.h" #include "units/acceleration.h" #include "units/angular_acceleration.h" @@ -25,14 +26,6 @@ namespace frc { */ class WPILIB_DLLEXPORT DifferentialDriveAccelerationLimiter { public: - /** - * Motor voltages for a differential drive. - */ - struct WheelVoltages { - units::volt_t left = 0_V; - units::volt_t right = 0_V; - }; - /** * Constructs a DifferentialDriveAccelerationLimiter. * @@ -55,10 +48,10 @@ class WPILIB_DLLEXPORT DifferentialDriveAccelerationLimiter { * @param rightVoltage The unconstrained right motor voltage. * @return The constrained wheel voltages. */ - WheelVoltages Calculate(units::meters_per_second_t leftVelocity, - units::meters_per_second_t rightVelocity, - units::volt_t leftVoltage, - units::volt_t rightVoltage); + DifferentialDriveWheelVoltages Calculate( + units::meters_per_second_t leftVelocity, + units::meters_per_second_t rightVelocity, units::volt_t leftVoltage, + units::volt_t rightVoltage); private: LinearSystem<2, 2, 2> m_system; diff --git a/wpimath/src/main/native/include/frc/controller/DifferentialDriveWheelVoltages.h b/wpimath/src/main/native/include/frc/controller/DifferentialDriveWheelVoltages.h new file mode 100644 index 0000000000..48f341e6d6 --- /dev/null +++ b/wpimath/src/main/native/include/frc/controller/DifferentialDriveWheelVoltages.h @@ -0,0 +1,19 @@ +// 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 "units/voltage.h" + +namespace frc { + +/** + * Motor voltages for a differential drive. + */ +struct DifferentialDriveWheelVoltages { + units::volt_t left = 0_V; + units::volt_t right = 0_V; +}; + +} // namespace frc diff --git a/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h b/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h index c97c66ae8d..391a909167 100644 --- a/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h +++ b/wpimath/src/main/native/include/frc/controller/LTVDifferentialDriveController.h @@ -9,6 +9,7 @@ #include #include "frc/EigenCore.h" +#include "frc/controller/DifferentialDriveWheelVoltages.h" #include "frc/geometry/Pose2d.h" #include "frc/system/LinearSystem.h" #include "frc/trajectory/Trajectory.h" @@ -31,14 +32,6 @@ namespace frc { */ class WPILIB_DLLEXPORT LTVDifferentialDriveController { public: - /** - * Motor voltages for a differential drive. - */ - struct WheelVoltages { - units::volt_t left = 0_V; - units::volt_t right = 0_V; - }; - /** * Constructs a linear time-varying differential drive controller. * @@ -95,12 +88,11 @@ class WPILIB_DLLEXPORT LTVDifferentialDriveController { * @param leftVelocityRef The desired left velocity. * @param rightVelocityRef The desired right velocity. */ - WheelVoltages Calculate(const Pose2d& currentPose, - units::meters_per_second_t leftVelocity, - units::meters_per_second_t rightVelocity, - const Pose2d& poseRef, - units::meters_per_second_t leftVelocityRef, - units::meters_per_second_t rightVelocityRef); + DifferentialDriveWheelVoltages Calculate( + const Pose2d& currentPose, units::meters_per_second_t leftVelocity, + units::meters_per_second_t rightVelocity, const Pose2d& poseRef, + units::meters_per_second_t leftVelocityRef, + units::meters_per_second_t rightVelocityRef); /** * Returns the left and right output voltages of the LTV controller. @@ -114,10 +106,10 @@ class WPILIB_DLLEXPORT LTVDifferentialDriveController { * @param desiredState The desired pose, linear velocity, and angular velocity * from a trajectory. */ - WheelVoltages Calculate(const Pose2d& currentPose, - units::meters_per_second_t leftVelocity, - units::meters_per_second_t rightVelocity, - const Trajectory::State& desiredState); + DifferentialDriveWheelVoltages Calculate( + const Pose2d& currentPose, units::meters_per_second_t leftVelocity, + units::meters_per_second_t rightVelocity, + const Trajectory::State& desiredState); private: units::meter_t m_trackwidth;