Merge pull request #69 from Shenzhen-Robotics-Alliance/dev

Better battery simulation & WPILib Beta 3

docs/vendordep/maple-sim.json

@@ -1,7 +1,7 @@
 {
     "fileName": "maple-sim.json",
     "name": "maplesim",
-    "version": "0.2.5",
+    "version": "0.2.6prev",
     "frcYear": "2025",
     "uuid": "c39481e8-4a63-4a4c-9df6-48d91e4da37b",
     "mavenUrls": [
@@ -13,7 +13,7 @@
         {
             "groupId": "org.ironmaple",
             "artifactId": "maplesim-java",
-            "version": "0.2.5"
+            "version": "0.2.6prev"
         },
         {
             "groupId": "org.dyn4j",

project/build.gradle

@@ -7,10 +7,10 @@ plugins {
     id 'edu.wpi.first.NativeUtils' version '2024.7.2'
     id 'edu.wpi.first.GradleJni' version '1.1.0'
     id 'edu.wpi.first.GradleVsCode' version '2.1.0'
-    id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-2"
+    id "edu.wpi.first.GradleRIO" version "2025.1.1-beta-3"
 }
 
-ext.wpilibVersion = "2025.1.1-beta-2"
+ext.wpilibVersion = "2025.1.1-beta-3"
 
 repositories {
     mavenCentral()

project/publish.gradle

@@ -2,7 +2,7 @@ apply plugin: 'maven-publish'
 
 ext.licenseFile = files("$rootDir/LICENSE.txt")
 
-def pubVersion = '0.2.5'
+def pubVersion = '0.2.6prev'
 
 def outputsFolder = file("$buildDir/outputs")
 

project/src/main/java/org/ironmaple/simulation/SimulatedArena.java

@@ -307,7 +307,6 @@ public synchronized void simulationPeriodic() {
         synchronized (SimulatedArena.class) {
             final long t0 = System.nanoTime();
             competitionPeriodic();
-            SimulatedBattery.getInstance().flush();
             // move through a few sub-periods in each update
             for (int i = 0; i < SIMULATION_SUB_TICKS_IN_1_PERIOD; i++) simulationSubTick(i);
 
@@ -332,6 +331,7 @@ public synchronized void simulationPeriodic() {
      * </ul>
      */
     private void simulationSubTick(int subTickNum) {
+        SimulatedBattery.simulationSubTick();
         for (AbstractDriveTrainSimulation driveTrainSimulation : driveTrainSimulations)
             driveTrainSimulation.simulationSubTick();
 

project/src/main/java/org/ironmaple/simulation/drivesims/AbstractDriveTrainSimulation.java

@@ -60,8 +60,8 @@ protected AbstractDriveTrainSimulation(DriveTrainSimulationConfig config, Pose2d
                 BUMPER_COEFFICIENT_OF_RESTITUTION);
 
         super.setMass(MassType.NORMAL);
-        super.setLinearDamping(1.0);
-        super.setAngularDamping(1.0);
+        super.setLinearDamping(0.1);
+        super.setAngularDamping(0.1);
         setSimulationWorldPose(initialPoseOnField);
     }
 
@@ -145,7 +145,8 @@ public Pose2d getSimulatedDriveTrainPose() {
      */
     public ChassisSpeeds getDriveTrainSimulatedChassisSpeedsRobotRelative() {
         ChassisSpeeds speeds = getDriveTrainSimulatedChassisSpeedsFieldRelative();
-        speeds.toRobotRelativeSpeeds(getSimulatedDriveTrainPose().getRotation());
+        speeds = ChassisSpeeds.fromFieldRelativeSpeeds(
+                speeds, getSimulatedDriveTrainPose().getRotation());
         return speeds;
     }
 

project/src/main/java/org/ironmaple/simulation/drivesims/SelfControlledSwerveDriveSimulation.java

@@ -266,18 +266,23 @@ public void addVisionEstimation(
      *     corner of the robot and provide a chassis speed that has only a dtheta component, the robot will rotate
      *     around that corner.
      * @param fieldCentricDrive Whether to execute field-centric drive with the provided speed.
-     * @param discretizeSpeeds Whether to apply {@link ChassisSpeeds#discretize(double)} to the provided speed.
+     * @param discretizeSpeeds Whether to apply {@link ChassisSpeeds#discretize(ChassisSpeeds, double)} to the provided
+     *     speed.
      */
     public void runChassisSpeeds(
             ChassisSpeeds chassisSpeeds,
             Translation2d centerOfRotationMeters,
             boolean fieldCentricDrive,
             boolean discretizeSpeeds) {
-        if (fieldCentricDrive)
-            chassisSpeeds.toRobotRelativeSpeeds(getOdometryEstimatedPose().getRotation());
-        if (discretizeSpeeds)
-            chassisSpeeds.discretize(
+        if (fieldCentricDrive) {
+            chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(
+                    chassisSpeeds, getOdometryEstimatedPose().getRotation());
+        }
+        if (discretizeSpeeds) {
+            chassisSpeeds = ChassisSpeeds.discretize(
+                    chassisSpeeds,
                     SimulatedArena.getSimulationDt().in(Seconds) * SimulatedArena.getSimulationSubTicksIn1Period());
+        }
         final SwerveModuleState[] setPoints = kinematics.toSwerveModuleStates(chassisSpeeds, centerOfRotationMeters);
         runSwerveStates(setPoints);
     }
@@ -341,7 +346,8 @@ public SwerveModuleState[] getSetPointsOptimized() {
      */
     public ChassisSpeeds getMeasuredSpeedsFieldRelative(boolean useGyroForAngularVelocity) {
         ChassisSpeeds speeds = getMeasuredSpeedsRobotRelative(useGyroForAngularVelocity);
-        speeds.toFieldRelativeSpeeds(getOdometryEstimatedPose().getRotation());
+        speeds = ChassisSpeeds.fromRobotRelativeSpeeds(
+                speeds, getOdometryEstimatedPose().getRotation());
         return speeds;
     }
 

project/src/main/java/org/ironmaple/simulation/drivesims/SwerveModuleSimulation.java

@@ -125,7 +125,7 @@ public SwerveModuleSimulation(
         this.driveMotorConfigs = new SimMotorConfigs(
                 driveMotorModel, DRIVE_GEAR_RATIO, KilogramSquareMeters.zero(), driveFrictionVoltage);
 
-        SimulatedBattery.getInstance().addElectricalAppliances(this::getDriveMotorSupplyCurrent);
+        SimulatedBattery.addElectricalAppliances(this::getDriveMotorSupplyCurrent);
         this.steerMotorSim = new MapleMotorSim(
                 new SimMotorConfigs(steerMotorModel, steerGearRatio, steerRotationalInertia, steerFrictionVoltage));
 
@@ -214,9 +214,7 @@ public Voltage getSteerMotorAppliedVoltage() {
      * @return the current supplied to the drive motor
      */
     public Current getDriveMotorSupplyCurrent() {
-        return getDriveMotorStatorCurrent()
-                .times(driveMotorAppliedVoltage.div(
-                        SimulatedBattery.getInstance().getBatteryVoltage()));
+        return getDriveMotorStatorCurrent().times(driveMotorAppliedVoltage.div(SimulatedBattery.getBatteryVoltage()));
     }
 
     /**
@@ -513,16 +511,20 @@ private double getDriveWheelTorque() {
                 getDriveEncoderUnGearedPosition(),
                 getDriveEncoderUnGearedSpeed());
 
-        driveMotorAppliedVoltage = Volts.of(MathUtil.clamp(driveMotorAppliedVoltage.in(Volts), -12, 12));
-
-        driveMotorAppliedVoltage = Volts.of(
-                MathUtil.applyDeadband(driveMotorAppliedVoltage.in(Volts), DRIVE_FRICTION_VOLTAGE.in(Volts), 12));
+        driveMotorAppliedVoltage = SimulatedBattery.clamp(driveMotorAppliedVoltage);
 
         /* calculate the stator current */
         driveMotorStatorCurrent = driveMotorConfigs.calculateCurrent(driveWheelFinalSpeed, driveMotorAppliedVoltage);
 
         /* calculate the torque generated */
-        return driveMotorConfigs.calculateTorque(driveMotorStatorCurrent).in(NewtonMeters);
+        Torque driveWheelTorque = driveMotorConfigs.calculateTorque(driveMotorStatorCurrent);
+
+        /* calculates the torque if you included losses from friction */
+        Torque driveWheelTorqueWithFriction = NewtonMeters.of(MathUtil.applyDeadband(
+                driveWheelTorque.in(NewtonMeters),
+                driveMotorConfigs.friction.in(NewtonMeters),
+                Double.POSITIVE_INFINITY));
+        return driveWheelTorqueWithFriction.in(NewtonMeters);
     }
 
     /** @return the current module state of this simulation module */

project/src/main/java/org/ironmaple/simulation/motorsims/MapleMotorSim.java

@@ -41,7 +41,7 @@ public MapleMotorSim(SimMotorConfigs configs) {
         this.appliedVoltage = Volts.zero();
         this.statorCurrent = Amps.zero();
 
-        SimulatedBattery.getInstance().addMotor(this);
+        SimulatedBattery.addMotor(this);
     }
 
     /**
@@ -57,6 +57,7 @@ public void update(Time dt) {
                 state.mechanismAngularVelocity,
                 state.mechanismAngularPosition.times(configs.gearing),
                 state.mechanismAngularVelocity.times(configs.gearing));
+        this.appliedVoltage = SimulatedBattery.clamp(appliedVoltage);
         this.statorCurrent = configs.calculateCurrent(state.mechanismAngularVelocity, appliedVoltage);
         this.state.step(configs.calculateTorque(statorCurrent), configs.friction, configs.loadMOI, dt);
 
@@ -169,7 +170,7 @@ public Current getSupplyCurrent() {
         // Hence,
         // Battery Voltage x Supply Current = Applied Voltage x Stator Current
         // Supply Current = Stator Current * Applied Voltage / Battery Voltage
-        return getStatorCurrent().times(appliedVoltage.div(Volts.of(12)));
+        return getStatorCurrent().times(appliedVoltage.div(SimulatedBattery.getBatteryVoltage()));
     }
 
     /**

project/src/main/java/org/ironmaple/simulation/motorsims/SimulatedBattery.java

@@ -4,48 +4,135 @@
 import static edu.wpi.first.units.Units.Volts;
 
 import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.filter.LinearFilter;
 import edu.wpi.first.units.measure.Current;
 import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.simulation.BatterySim;
 import edu.wpi.first.wpilibj.simulation.RoboRioSim;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import java.util.ArrayList;
 import java.util.List;
 import java.util.function.Supplier;
 
+/**
+ *
+ *
+ * <h1>Simulates the main battery of the robot.</h1>
+ *
+ * <p>This class simulates the behavior of a robot's battery. Electrical appliances can be added to the battery to draw
+ * current. The battery voltage is affected by the current drawn from various appliances.
+ */
 public class SimulatedBattery {
-    private static SimulatedBattery instance = null;
+    // Nominal voltage for a fully charged battery (13.5 volts).
+    private static final double BATTERY_NOMINAL_VOLTAGE = 13.5;
 
-    public static SimulatedBattery getInstance() {
-        if (instance == null) instance = new SimulatedBattery();
-        return instance;
-    }
+    // Filter to smooth the current readings.
+    private static final LinearFilter currentFilter = LinearFilter.movingAverage(50);
+
+    private static final List<Supplier<Current>> electricalAppliances = new ArrayList<>();
 
-    private final List<Supplier<Current>> electricalAppliances = new ArrayList<>();
-    private double batteryVoltageVolts = 12.0;
+    // The current battery voltage in volts.
+    private static double batteryVoltageVolts = BATTERY_NOMINAL_VOLTAGE;
 
-    public void addElectricalAppliances(Supplier<Current> customElectricalAppliances) {
-        this.electricalAppliances.add(customElectricalAppliances);
+    /**
+     *
+     *
+     * <h2>Adds a custom electrical appliance.</h2>
+     *
+     * <p>Connects the electrical appliance to the battery, allowing it to draw current from the battery.
+     *
+     * @param customElectricalAppliances The supplier for the current drawn by the appliance.
+     */
+    public static void addElectricalAppliances(Supplier<Current> customElectricalAppliances) {
+        electricalAppliances.add(customElectricalAppliances);
     }
 
-    public void addMotor(MapleMotorSim mapleMotorSim) {
-        this.electricalAppliances.add(mapleMotorSim::getSupplyCurrent);
+    /**
+     *
+     *
+     * <h2>Adds a motor to the list of electrical appliances.</h2>
+     *
+     * <p>The motor will draw current from the battery.
+     *
+     * @param mapleMotorSim The motor simulation object.
+     */
+    public static void addMotor(MapleMotorSim mapleMotorSim) {
+        electricalAppliances.add(mapleMotorSim::getSupplyCurrent);
     }
 
-    public void flush() {
-        final double totalCurrentAmps = electricalAppliances.stream()
-                .mapToDouble(currentSupplier -> currentSupplier.get().in(Amps))
-                .sum();
+    /**
+     *
+     *
+     * <h2>Updates the battery simulation.</h2>
+     *
+     * <p>Calculates the battery voltage based on the current drawn by all appliances.
+     *
+     * <p>The battery voltage is clamped to avoid going below the brownout voltage.
+     */
+    public static void simulationSubTick() {
+        double totalCurrentAmps = getTotalCurrentDrawn().in(Amps);
+        totalCurrentAmps = currentFilter.calculate(totalCurrentAmps);
 
-        SmartDashboard.putNumber("BatterySim/TotalCurrentAmps", totalCurrentAmps);
-        batteryVoltageVolts = BatterySim.calculateDefaultBatteryLoadedVoltage(totalCurrentAmps);
+        batteryVoltageVolts = BatterySim.calculateLoadedBatteryVoltage(BATTERY_NOMINAL_VOLTAGE, 0.02, totalCurrentAmps);
 
-        batteryVoltageVolts = MathUtil.clamp(batteryVoltageVolts, 0, 12);
+        if (Double.isNaN(batteryVoltageVolts)) {
+            batteryVoltageVolts = 12.0;
+            DriverStation.reportError(
+                    "[MapleSim] Internal Library Error: Calculated battery voltage is invalid"
+                            + ", reverting to normal operation voltage...",
+                    false);
+        }
+        if (batteryVoltageVolts < RoboRioSim.getBrownoutVoltage()) {
+            batteryVoltageVolts = RoboRioSim.getBrownoutVoltage();
+            DriverStation.reportError("[MapleSim] BrownOut Detected, protecting battery voltage...", false);
+        }
 
         RoboRioSim.setVInVoltage(batteryVoltageVolts);
+
+        SmartDashboard.putNumber("BatterySim/TotalCurrent (Amps)", totalCurrentAmps);
+        SmartDashboard.putNumber("BatterySim/BatteryVoltage (Volts)", batteryVoltageVolts);
     }
 
-    public Voltage getBatteryVoltage() {
+    /**
+     *
+     *
+     * <h2>Obtains the voltage of the battery.</h2>
+     *
+     * @return The battery voltage as a {@link Voltage} object.
+     */
+    public static Voltage getBatteryVoltage() {
         return Volts.of(batteryVoltageVolts);
     }
+
+    /**
+     *
+     *
+     * <h2>Obtains the total current drawn from the battery.</h2>
+     *
+     * <p>Iterates through all the appliances to obtain the total current used.
+     *
+     * @return The total current as a {@link Current} object.
+     */
+    public static Current getTotalCurrentDrawn() {
+        double totalCurrentAmps = electricalAppliances.stream()
+                .mapToDouble(currentSupplier -> currentSupplier.get().in(Amps))
+                .sum();
+        return Amps.of(totalCurrentAmps);
+    }
+
+    /**
+     *
+     *
+     * <h2>Clamps the voltage according to the supplied voltage and the battery's capabilities.</h2>
+     *
+     * <p>If the supplied voltage exceeds the battery's maximum voltage, it will be reduced to match the battery's
+     * voltage.
+     *
+     * @param voltage The voltage to be clamped.
+     * @return The clamped voltage as a {@link Voltage} object.
+     */
+    public static Voltage clamp(Voltage voltage) {
+        return Volts.of(MathUtil.clamp(voltage.in(Volts), -batteryVoltageVolts, batteryVoltageVolts));
+    }
 }

templates/AdvantageKit_SparkSwerveTemplate-maple-sim/src/main/java/frc/robot/commands/DriveCommands.java

@@ -86,7 +86,8 @@ public static Command joystickDrive(
                             omega * drive.getMaxAngularSpeedRadPerSec());
                     boolean isFlipped = DriverStation.getAlliance().isPresent()
                             && DriverStation.getAlliance().get() == Alliance.Red;
-                    speeds.toRobotRelativeSpeeds(
+                    speeds = ChassisSpeeds.fromFieldRelativeSpeeds(
+                            speeds,
                             isFlipped ? drive.getRotation().plus(new Rotation2d(Math.PI)) : drive.getRotation());
                     drive.runVelocity(speeds);
                 },
@@ -124,7 +125,8 @@ public static Command joystickDriveAtAngle(
                                     omega);
                             boolean isFlipped = DriverStation.getAlliance().isPresent()
                                     && DriverStation.getAlliance().get() == Alliance.Red;
-                            speeds.toRobotRelativeSpeeds(
+                            speeds = ChassisSpeeds.fromFieldRelativeSpeeds(
+                                    speeds,
                                     isFlipped
                                             ? drive.getRotation().plus(new Rotation2d(Math.PI))
                                             : drive.getRotation());

templates/AdvantageKit_SparkSwerveTemplate-maple-sim/src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -173,7 +173,7 @@ public void periodic() {
      */
     public void runVelocity(ChassisSpeeds speeds) {
         // Calculate module setpoints
-        speeds.discretize(0.02);
+        speeds = ChassisSpeeds.discretize(speeds, 0.02);
         SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(speeds);
         SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, maxSpeedMetersPerSec);
 

templates/AdvantageKit_SparkSwerveTemplate-maple-sim/src/main/java/frc/robot/subsystems/drive/DriveConstants.java

@@ -58,7 +58,7 @@ public class DriveConstants {
     public static final int backRightTurnCanId = 8;
 
     // Drive motor configuration
-    public static final int driveMotorCurrentLimit = 50;
+    public static final int driveMotorCurrentLimit = 60;
     public static final double wheelRadiusMeters = Units.inchesToMeters(1.5);
     public static final double driveMotorReduction =
             (45.0 * 22.0) / (14.0 * 15.0); // MAXSwerve with 14 pinion teeth and 22 spur teeth
@@ -100,7 +100,7 @@ public class DriveConstants {
     public static final double turnPIDMaxInput = 2 * Math.PI; // Radians
 
     // PathPlanner configuration
-    public static final double robotMassKg = 74.088;
+    public static final double robotMassKg = 45;
     public static final double robotMOI = 6.883;
     public static final double wheelCOF = 1.2;
     public static final RobotConfig ppConfig = new RobotConfig(