From 44f78cb03e8430d2467dc469a8b433a978394d9e Mon Sep 17 00:00:00 2001 From: Jordan McMichael Date: Thu, 21 Nov 2024 19:21:27 -0500 Subject: [PATCH] [Examples] Limit minimum battery voltage in sim to 0.1V (#1600) Occasionally, the sim projects are capable of simulating current draw of over 600A, which triggers a condition in `BatterySim::calculateDefaultBatteryLoadedVoltage` that limits the minimum measured battery voltage to 0V (to prevent it from going negative). When battery voltage measures 0, this causes NaN values to propagate through the drivetrain model, making sim inoperable. Specifically, [this is the line](https://github.com/PhotonVision/photonvision/blob/master/photonlib-java-examples/aimandrange/src/main/java/frc/robot/subsystems/drivetrain/SwerveDriveSim.java#L452) that causes the initial NaN values in simulation. This PR is posed as a patch to ensure that simulation doesn't break. --- photonlib-cpp-examples/aimandrange/src/main/cpp/Robot.cpp | 4 +++- photonlib-cpp-examples/aimattarget/src/main/cpp/Robot.cpp | 4 +++- photonlib-cpp-examples/poseest/src/main/cpp/Robot.cpp | 5 ++++- .../aimandrange/src/main/java/frc/robot/Robot.java | 8 ++++++-- .../aimattarget/src/main/java/frc/robot/Robot.java | 8 ++++++-- .../poseest/src/main/java/frc/robot/Robot.java | 8 ++++++-- 6 files changed, 28 insertions(+), 9 deletions(-) diff --git a/photonlib-cpp-examples/aimandrange/src/main/cpp/Robot.cpp b/photonlib-cpp-examples/aimandrange/src/main/cpp/Robot.cpp index d46d5b2ed..c50d003a1 100644 --- a/photonlib-cpp-examples/aimandrange/src/main/cpp/Robot.cpp +++ b/photonlib-cpp-examples/aimandrange/src/main/cpp/Robot.cpp @@ -125,7 +125,9 @@ void Robot::SimulationPeriodic() { units::ampere_t totalCurrent = drivetrain.GetCurrentDraw(); units::volt_t loadedBattVolts = frc::sim::BatterySim::Calculate({totalCurrent}); - frc::sim::RoboRioSim::SetVInVoltage(loadedBattVolts); + // Using max(0.1, voltage) here isn't a *physically correct* solution, + // but it avoids problems with battery voltage measuring 0. + frc::sim::RoboRioSim::SetVInVoltage(units::math::max(0.1_V, loadedBattVolts)); } #ifndef RUNNING_FRC_TESTS diff --git a/photonlib-cpp-examples/aimattarget/src/main/cpp/Robot.cpp b/photonlib-cpp-examples/aimattarget/src/main/cpp/Robot.cpp index 140af4563..5431f3cfb 100644 --- a/photonlib-cpp-examples/aimattarget/src/main/cpp/Robot.cpp +++ b/photonlib-cpp-examples/aimattarget/src/main/cpp/Robot.cpp @@ -115,7 +115,9 @@ void Robot::SimulationPeriodic() { units::ampere_t totalCurrent = drivetrain.GetCurrentDraw(); units::volt_t loadedBattVolts = frc::sim::BatterySim::Calculate({totalCurrent}); - frc::sim::RoboRioSim::SetVInVoltage(loadedBattVolts); + // Using max(0.1, voltage) here isn't a *physically correct* solution, + // but it avoids problems with battery voltage measuring 0. + frc::sim::RoboRioSim::SetVInVoltage(units::math::max(0.1_V, loadedBattVolts)); } #ifndef RUNNING_FRC_TESTS diff --git a/photonlib-cpp-examples/poseest/src/main/cpp/Robot.cpp b/photonlib-cpp-examples/poseest/src/main/cpp/Robot.cpp index 75a4ff22c..75c72d216 100644 --- a/photonlib-cpp-examples/poseest/src/main/cpp/Robot.cpp +++ b/photonlib-cpp-examples/poseest/src/main/cpp/Robot.cpp @@ -105,7 +105,10 @@ void Robot::SimulationPeriodic() { units::ampere_t totalCurrent = drivetrain.GetCurrentDraw(); units::volt_t loadedBattVolts = frc::sim::BatterySim::Calculate({totalCurrent}); - frc::sim::RoboRioSim::SetVInVoltage(loadedBattVolts); + + // Using max(0.1, voltage) here isn't a *physically correct* solution, + // but it avoids problems with battery voltage measuring 0. + frc::sim::RoboRioSim::SetVInVoltage(units::math::max(0.1_V, loadedBattVolts)); } #ifndef RUNNING_FRC_TESTS diff --git a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Robot.java b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Robot.java index 0d1429c6c..2d58a2747 100644 --- a/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Robot.java +++ b/photonlib-java-examples/aimandrange/src/main/java/frc/robot/Robot.java @@ -147,8 +147,12 @@ public class Robot extends TimedRobot { debugField.getObject("EstimatedRobotModules").setPoses(drivetrain.getModulePoses()); // Calculate battery voltage sag due to current draw - RoboRioSim.setVInVoltage( - BatterySim.calculateDefaultBatteryLoadedVoltage(drivetrain.getCurrentDraw())); + var batteryVoltage = + BatterySim.calculateDefaultBatteryLoadedVoltage(drivetrain.getCurrentDraw()); + + // Using max(0.1, voltage) here isn't a *physically correct* solution, + // but it avoids problems with battery voltage measuring 0. + RoboRioSim.setVInVoltage(Math.max(0.1, batteryVoltage)); } public void resetPose() { diff --git a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Robot.java b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Robot.java index 4a79bba13..f3c5e24f2 100644 --- a/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Robot.java +++ b/photonlib-java-examples/aimattarget/src/main/java/frc/robot/Robot.java @@ -129,8 +129,12 @@ public class Robot extends TimedRobot { debugField.getObject("EstimatedRobotModules").setPoses(drivetrain.getModulePoses()); // Calculate battery voltage sag due to current draw - RoboRioSim.setVInVoltage( - BatterySim.calculateDefaultBatteryLoadedVoltage(drivetrain.getCurrentDraw())); + var batteryVoltage = + BatterySim.calculateDefaultBatteryLoadedVoltage(drivetrain.getCurrentDraw()); + + // Using max(0.1, voltage) here isn't a *physically correct* solution, + // but it avoids problems with battery voltage measuring 0. + RoboRioSim.setVInVoltage(Math.max(0.1, batteryVoltage)); } public void resetPose() { diff --git a/photonlib-java-examples/poseest/src/main/java/frc/robot/Robot.java b/photonlib-java-examples/poseest/src/main/java/frc/robot/Robot.java index 0425f8185..d47e3056e 100644 --- a/photonlib-java-examples/poseest/src/main/java/frc/robot/Robot.java +++ b/photonlib-java-examples/poseest/src/main/java/frc/robot/Robot.java @@ -127,8 +127,12 @@ public class Robot extends TimedRobot { gpLauncher.simulationPeriodic(); // Calculate battery voltage sag due to current draw - RoboRioSim.setVInVoltage( - BatterySim.calculateDefaultBatteryLoadedVoltage(drivetrain.getCurrentDraw())); + var batteryVoltage = + BatterySim.calculateDefaultBatteryLoadedVoltage(drivetrain.getCurrentDraw()); + + // Using max(0.1, voltage) here isn't a *physically correct* solution, + // but it avoids problems with battery voltage measuring 0. + RoboRioSim.setVInVoltage(Math.max(0.1, batteryVoltage)); } public void resetPose() {