diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp index 902c26b30e..6059db4ece 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/MecanumControllerCommand.cpp @@ -6,7 +6,12 @@ #include +#include +#include + using namespace frc2; +using kv_unit = units::compound_unit>; MecanumControllerCommand::MecanumControllerCommand( frc::Trajectory trajectory, std::function pose, @@ -95,6 +100,7 @@ MecanumControllerCommand::MecanumControllerCommand( Requirements requirements) : m_trajectory(std::move(trajectory)), m_pose(std::move(pose)), + m_feedforward(0_V, units::unit_t{0}), m_kinematics(kinematics), m_controller(xController, yController, thetaController), m_desiredRotation(std::move(desiredRotation)), @@ -116,6 +122,7 @@ MecanumControllerCommand::MecanumControllerCommand( Requirements requirements) : m_trajectory(std::move(trajectory)), m_pose(std::move(pose)), + m_feedforward(0_V, units::unit_t{0}), m_kinematics(kinematics), m_controller(xController, yController, thetaController), m_maxWheelVelocity(maxWheelVelocity), diff --git a/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp b/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp index 91459858dd..b78b6ad4c1 100644 --- a/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp +++ b/wpilibNewCommands/src/main/native/cpp/frc2/command/RamseteCommand.cpp @@ -6,9 +6,13 @@ #include +#include +#include #include using namespace frc2; +using kv_unit = units::compound_unit>; RamseteCommand::RamseteCommand( frc::Trajectory trajectory, std::function pose, @@ -42,6 +46,7 @@ RamseteCommand::RamseteCommand( : m_trajectory(std::move(trajectory)), m_pose(std::move(pose)), m_controller(controller), + m_feedforward(0_V, units::unit_t{0}), m_kinematics(std::move(kinematics)), m_outputVel(std::move(output)), m_usePID(false) { diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java index 8c53f61958..e7551a762b 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ArmFeedforward.java @@ -28,12 +28,20 @@ public class ArmFeedforward { * @param kg The gravity gain. * @param kv The velocity gain. * @param ka The acceleration gain. + * @throws IllegalArgumentException for kv < zero. + * @throws IllegalArgumentException for ka < zero. */ public ArmFeedforward(double ks, double kg, double kv, double ka) { this.ks = ks; this.kg = kg; this.kv = kv; this.ka = ka; + if (kv < 0.0) { + throw new IllegalArgumentException("kv must be a non-negative number, got " + kv + "!"); + } + if (ka < 0.0) { + throw new IllegalArgumentException("ka must be a non-negative number, got " + kv + "!"); + } } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java index b7986caedf..9ae8135858 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/ElevatorFeedforward.java @@ -31,12 +31,20 @@ public class ElevatorFeedforward { * @param kg The gravity gain. * @param kv The velocity gain. * @param ka The acceleration gain. + * @throws IllegalArgumentException for kv < zero. + * @throws IllegalArgumentException for ka < zero. */ public ElevatorFeedforward(double ks, double kg, double kv, double ka) { this.ks = ks; this.kg = kg; this.kv = kv; this.ka = ka; + if (kv < 0.0) { + throw new IllegalArgumentException("kv must be a non-negative number, got " + kv + "!"); + } + if (ka < 0.0) { + throw new IllegalArgumentException("ka must be a non-negative number, got " + kv + "!"); + } } /** diff --git a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java index 63ceb8fa8f..f19e836617 100644 --- a/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java +++ b/wpimath/src/main/java/edu/wpi/first/math/controller/SimpleMotorFeedforward.java @@ -21,11 +21,19 @@ public class SimpleMotorFeedforward { * @param ks The static gain. * @param kv The velocity gain. * @param ka The acceleration gain. + * @throws IllegalArgumentException for kv < zero. + * @throws IllegalArgumentException for ka < zero. */ public SimpleMotorFeedforward(double ks, double kv, double ka) { this.ks = ks; this.kv = kv; this.ka = ka; + if (kv < 0.0) { + throw new IllegalArgumentException("kv must be a non-negative number, got " + kv + "!"); + } + if (ka < 0.0) { + throw new IllegalArgumentException("ka must be a non-negative number, got " + kv + "!"); + } } /** diff --git a/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h b/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h index 39f4f90e59..933a4051d6 100644 --- a/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/ArmFeedforward.h @@ -11,6 +11,7 @@ #include "units/angular_velocity.h" #include "units/math.h" #include "units/voltage.h" +#include "wpimath/MathShared.h" namespace frc { /** @@ -29,8 +30,6 @@ class WPILIB_DLLEXPORT ArmFeedforward { using ka_unit = units::compound_unit>; - constexpr ArmFeedforward() = default; - /** * Creates a new ArmFeedforward with the specified gains. * @@ -42,7 +41,20 @@ class WPILIB_DLLEXPORT ArmFeedforward { constexpr ArmFeedforward( units::volt_t kS, units::volt_t kG, units::unit_t kV, units::unit_t kA = units::unit_t(0)) - : kS(kS), kG(kG), kV(kV), kA(kA) {} + : kS(kS), kG(kG), kV(kV), kA(kA) { + if (kV.value() < 0) { + wpi::math::MathSharedStore::ReportError( + "kV must be a non-negative number, got {}!", kV.value()); + kV = units::unit_t{0}; + wpi::math::MathSharedStore::ReportWarning("kV defaulted to 0."); + } + if (kA.value() < 0) { + wpi::math::MathSharedStore::ReportError( + "kA must be a non-negative number, got {}!", kA.value()); + kA = units::unit_t{0}; + wpi::math::MathSharedStore::ReportWarning("kA defaulted to 0;"); + } + } /** * Calculates the feedforward from the gains and setpoints. @@ -163,10 +175,10 @@ class WPILIB_DLLEXPORT ArmFeedforward { return MaxAchievableAcceleration(-maxVoltage, angle, velocity); } - units::volt_t kS{0}; - units::volt_t kG{0}; - units::unit_t kV{0}; - units::unit_t kA{0}; + const units::volt_t kS; + const units::volt_t kG; + const units::unit_t kV; + const units::unit_t kA; }; } // namespace frc diff --git a/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h b/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h index 4d308f4ec7..2d81a0ae33 100644 --- a/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/ElevatorFeedforward.h @@ -12,6 +12,7 @@ #include "units/length.h" #include "units/time.h" #include "units/voltage.h" +#include "wpimath/MathShared.h" namespace frc { /** @@ -29,8 +30,6 @@ class ElevatorFeedforward { using ka_unit = units::compound_unit>; - ElevatorFeedforward() = default; - /** * Creates a new ElevatorFeedforward with the specified gains. * @@ -42,7 +41,20 @@ class ElevatorFeedforward { constexpr ElevatorFeedforward( units::volt_t kS, units::volt_t kG, units::unit_t kV, units::unit_t kA = units::unit_t(0)) - : kS(kS), kG(kG), kV(kV), kA(kA) {} + : kS(kS), kG(kG), kV(kV), kA(kA) { + if (kV.value() < 0) { + wpi::math::MathSharedStore::ReportError( + "kV must be a non-negative number, got {}!", kV.value()); + kV = units::unit_t{0}; + wpi::math::MathSharedStore::ReportWarning("kV defaulted to 0."); + } + if (kA.value() < 0) { + wpi::math::MathSharedStore::ReportError( + "kA must be a non-negative number, got {}!", kA.value()); + kA = units::unit_t{0}; + wpi::math::MathSharedStore::ReportWarning("kA defaulted to 0;"); + } + } /** * Calculates the feedforward from the gains and setpoints. @@ -170,10 +182,10 @@ class ElevatorFeedforward { return MaxAchievableAcceleration(-maxVoltage, velocity); } - units::volt_t kS{0}; - units::volt_t kG{0}; - units::unit_t kV{0}; - units::unit_t kA{0}; + const units::volt_t kS; + const units::volt_t kG; + const units::unit_t kV; + const units::unit_t kA; }; } // namespace frc diff --git a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h index 86b40eab06..767b05d4d6 100644 --- a/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h +++ b/wpimath/src/main/native/include/frc/controller/SimpleMotorFeedforward.h @@ -11,6 +11,7 @@ #include "frc/system/plant/LinearSystemId.h" #include "units/time.h" #include "units/voltage.h" +#include "wpimath/MathShared.h" namespace frc { /** @@ -28,8 +29,6 @@ class SimpleMotorFeedforward { using ka_unit = units::compound_unit>; - constexpr SimpleMotorFeedforward() = default; - /** * Creates a new SimpleMotorFeedforward with the specified gains. * @@ -40,7 +39,20 @@ class SimpleMotorFeedforward { constexpr SimpleMotorFeedforward( units::volt_t kS, units::unit_t kV, units::unit_t kA = units::unit_t(0)) - : kS(kS), kV(kV), kA(kA) {} + : kS(kS), kV(kV), kA(kA) { + if (kV.value() < 0) { + wpi::math::MathSharedStore::ReportError( + "kV must be a non-negative number, got {}!", kV.value()); + kV = units::unit_t{0}; + wpi::math::MathSharedStore::ReportWarning("kV defaulted to 0."); + } + if (kA.value() < 0) { + wpi::math::MathSharedStore::ReportError( + "kA must be a non-negative number, got {}!", kA.value()); + kA = units::unit_t{0}; + wpi::math::MathSharedStore::ReportWarning("kA defaulted to 0;"); + } + } /** * Calculates the feedforward from the gains and setpoints. @@ -148,8 +160,8 @@ class SimpleMotorFeedforward { return MaxAchievableAcceleration(-maxVoltage, velocity); } - units::volt_t kS{0}; - units::unit_t kV{0}; - units::unit_t kA{0}; + const units::volt_t kS; + const units::unit_t kV; + const units::unit_t kA; }; } // namespace frc