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

CTRE Generated Swerve simulation

LICENSE-IronMaple.md

@@ -6,7 +6,7 @@
 #### This project uses [maple-sim](https://github.com/Shenzhen-Robotics-Alliance/maple-sim) for simulation.
 If you encounter any bugs related to drivetrain physics simulation, please [submit an issue to maple-sim](https://github.com/Shenzhen-Robotics-Alliance/maple-sim/issues/new/choose).
 
-## MIT License, Copyright (c) 2024 Team5516 "Iron Maple"
+## MIT License, Copyright (c) 2024-2025 Team5516 "Iron Maple"
 
 **Permission is hereby granted, free of charge, to any person obtaining a copyof this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:**
 

LICENSE-WPILib.md

@@ -1,4 +1,4 @@
-Copyright (c) 2009-2024 FIRST and other WPILib contributors
+Copyright (c) 2009-2025 FIRST and other WPILib contributors
 All rights reserved.
 
 Redistribution and use in source and binary forms, with or without

docs/vendordep/maple-sim.json

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

project/publish.gradle

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

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

@@ -63,11 +63,10 @@ public class SwerveModuleSimulation {
     private Voltage driveMotorAppliedVoltage = Volts.zero();
     private Current driveMotorStatorCurrent = Amps.zero();
     private Angle driveWheelFinalPosition = Radians.zero();
-    private AngularVelocity steerVelocity = RadiansPerSecond.zero(), driveWheelFinalSpeed = RadiansPerSecond.zero();
+    private AngularVelocity driveWheelFinalSpeed = RadiansPerSecond.zero();
 
     private SimulatedMotorController driveMotorController;
 
-    private Rotation2d steerAbsoluteFacing = Rotation2d.fromRotations(Math.random());
     private final Angle steerRelativeEncoderOffSet = Radians.of((Math.random() - 0.5) * 30);
     private final Queue<Angle> driveWheelFinalPositionCache;
     private final Queue<Rotation2d> steerAbsolutePositionCache;
@@ -93,7 +92,7 @@ public SwerveModuleSimulation(SwerveModuleSimulationConfig config) {
             driveWheelFinalPositionCache.offer(driveWheelFinalPosition);
         this.steerAbsolutePositionCache = new ConcurrentLinkedQueue<>();
         for (int i = 0; i < SimulatedArena.getSimulationSubTicksIn1Period(); i++)
-            steerAbsolutePositionCache.offer(steerAbsoluteFacing);
+            steerAbsolutePositionCache.offer(getSteerAbsoluteFacing());
 
         this.driveMotorController = new SimulatedMotorController.GenericMotorController(config.driveMotorConfigs.motor);
         this.steerMotorSim.useSimpleDCMotorController();
@@ -160,11 +159,11 @@ public Vector2 updateSimulationSubTickGetModuleForce(
             Rotation2d robotFacing,
             double gravityForceOnModuleNewtons) {
         /* Step1: Update the steer mechanism simulation */
-        updateSteerSimulation();
+        steerMotorSim.update(SimulatedArena.getSimulationDt());
 
         /* Step2: Simulate the amount of propelling force generated by the module. */
         final double grippingForceNewtons = config.getGrippingForceNewtons(gravityForceOnModuleNewtons);
-        final Rotation2d moduleWorldFacing = this.steerAbsoluteFacing.plus(robotFacing);
+        final Rotation2d moduleWorldFacing = this.getSteerAbsoluteFacing().plus(robotFacing);
         final Vector2 propellingForce =
                 getPropellingForce(grippingForceNewtons, moduleWorldFacing, moduleCurrentGroundVelocityWorldRelative);
 
@@ -174,18 +173,6 @@ public Vector2 updateSimulationSubTickGetModuleForce(
         return propellingForce;
     }
 
-    /**
-     *
-     *
-     * <h2>updates the simulation for the steer mechanism.</h2>
-     */
-    private void updateSteerSimulation() {
-        /* update the readings of the sensor */
-        steerMotorSim.update(SimulatedArena.getSimulationDt());
-        this.steerAbsoluteFacing = new Rotation2d(steerMotorSim.getAngularPosition());
-        this.steerVelocity = steerMotorSim.getVelocity();
-    }
-
     /**
      *
      *
@@ -263,7 +250,7 @@ private double getDriveWheelTorque() {
     public SwerveModuleState getCurrentState() {
         return new SwerveModuleState(
                 MetersPerSecond.of(getDriveWheelFinalSpeed().in(RadiansPerSecond) * config.WHEEL_RADIUS.in(Meters)),
-                steerAbsoluteFacing);
+                getSteerAbsoluteFacing());
     }
 
     /**
@@ -284,7 +271,7 @@ protected SwerveModuleState getFreeSpinState() {
                                         driveMotorAppliedVoltage)
                                 .in(RadiansPerSecond)
                         * config.WHEEL_RADIUS.in(Meters),
-                steerAbsoluteFacing);
+                getSteerAbsoluteFacing());
     }
 
     /**
@@ -301,7 +288,7 @@ private void updateEncoderCaches() {
 
         /* cache sensor readings to queue for high-frequency odometry */
         this.steerAbsolutePositionCache.poll();
-        this.steerAbsolutePositionCache.offer(steerAbsoluteFacing);
+        this.steerAbsolutePositionCache.offer(getSteerAbsoluteFacing());
 
         this.driveWheelFinalPositionCache.poll();
         this.driveWheelFinalPositionCache.offer(driveWheelFinalPosition);
@@ -460,7 +447,19 @@ public AngularVelocity getSteerRelativeEncoderVelocity() {
      * @return the absolute facing of the steer mechanism, as a {@link Rotation2d}
      */
     public Rotation2d getSteerAbsoluteFacing() {
-        return steerAbsoluteFacing;
+        return new Rotation2d(getSteerAbsoluteAngle());
+    }
+
+    /**
+     *
+     *
+     * <h2>Obtains the Absolute Angle of the Steer Mechanism.</h2>
+     *
+     * @return the (continuous) final angle of the steer mechanism, as a {@link Angle}
+     * @see MapleMotorSim#getAngularPosition()
+     */
+    public Angle getSteerAbsoluteAngle() {
+        return steerMotorSim.getAngularPosition();
     }
 
     /**
@@ -471,7 +470,7 @@ public Rotation2d getSteerAbsoluteFacing() {
      * @return the absolute angular velocity of the steer mechanism
      */
     public AngularVelocity getSteerAbsoluteEncoderSpeed() {
-        return steerVelocity;
+        return steerMotorSim.getVelocity();
     }
 
     /**

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

@@ -83,7 +83,7 @@ public SimulatedMotorController.GenericMotorController useSimpleDCMotorControlle
      *
      * <p>This is equivalent to {@link edu.wpi.first.wpilibj.simulation.DCMotorSim#getAngularPosition()}.
      *
-     * @return the angular position of the motor, continuous
+     * @return the angular position of the mechanism, continuous
      */
     public Angle getAngularPosition() {
         return state.mechanismAngularPosition;
@@ -107,7 +107,7 @@ public Angle getEncoderPosition() {
      *
      * <p>This is equivalent to {@link edu.wpi.first.wpilibj.simulation.DCMotorSim#getAngularVelocity()}.
      *
-     * @return the final angular velocity of the rotter
+     * @return the final angular velocity of the mechanism
      */
     public AngularVelocity getVelocity() {
         return state.mechanismAngularVelocity;

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

@@ -191,4 +191,14 @@ protected SimMotorConfigs clone() {
 
         return cfg;
     }
+
+    @Override
+    public String toString() {
+        return "SimMotorConfigs {"
+                + "\n  motor = " + motor // Relies on DCMotor.toString() for details
+                + "\n  gearing = " + gearing
+                + "\n  loadMOI (kg·m^2) = " + loadMOI.in(KilogramSquareMeters)
+                + "\n  friction (N·m) = " + friction.in(NewtonMeters)
+                + "\n}";
+    }
 }

project/src/main/java/org/ironmaple/utils/FieldMirroringUtils.java

@@ -12,7 +12,7 @@ public class FieldMirroringUtils {
     public static final double FIELD_HEIGHT = 8.052;
 
     public static Rotation2d toCurrentAllianceRotation(Rotation2d rotationAtBlueSide) {
-        return rotationAtBlueSide.times(-1);
+        return isSidePresentedAsRed() ? rotationAtBlueSide.plus(Rotation2d.k180deg) : rotationAtBlueSide;
     }
 
     public static Translation2d toCurrentAllianceTranslation(Translation2d translationAtBlueSide) {
@@ -45,9 +45,6 @@ public static boolean isSidePresentedAsRed() {
     }
 
     public static Rotation2d getCurrentAllianceDriverStationFacing() {
-        return switch (DriverStation.getAlliance().orElse(DriverStation.Alliance.Red)) {
-            case Red -> new Rotation2d(Math.PI);
-            case Blue -> new Rotation2d(0);
-        };
+        return toCurrentAllianceRotation(Rotation2d.kZero);
     }
 }

templates/AdvantageKit_SparkSwerveTemplate-maple-sim/LICENSE-IronMaple.md

@@ -6,7 +6,7 @@
 #### This project uses [maple-sim](https://github.com/Shenzhen-Robotics-Alliance/maple-sim) for simulation.
 If you encounter any bugs related to drivetrain physics simulation, please [submit an issue to maple-sim](https://github.com/Shenzhen-Robotics-Alliance/maple-sim/issues/new/choose).
 
-## MIT License, Copyright (c) 2024 Team5516 "Iron Maple"
+## MIT License, Copyright (c) 2024-2025 Team5516 "Iron Maple"
 
 **Permission is hereby granted, free of charge, to any person obtaining a copyof this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:**
 

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

@@ -1,4 +1,4 @@
-// Copyright 2021-2024 FRC 6328
+// Copyright 2021-2025 FRC 6328
 // http://github.com/Mechanical-Advantage
 //
 // This program is free software; you can redistribute it and/or

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

@@ -1,4 +1,4 @@
-// Copyright 2021-2024 FRC 6328
+// Copyright 2021-2025 FRC 6328
 // http://github.com/Mechanical-Advantage
 //
 // This program is free software; you can redistribute it and/or

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

@@ -1,4 +1,4 @@
-// Copyright 2021-2024 FRC 6328
+// Copyright 2021-2025 FRC 6328
 // http://github.com/Mechanical-Advantage
 //
 // This program is free software; you can redistribute it and/or

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

@@ -1,4 +1,4 @@
-// Copyright 2021-2024 FRC 6328
+// Copyright 2021-2025 FRC 6328
 // http://github.com/Mechanical-Advantage
 //
 // This program is free software; you can redistribute it and/or

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

@@ -1,4 +1,4 @@
-// Copyright 2021-2024 FRC 6328
+// Copyright 2021-2025 FRC 6328
 // http://github.com/Mechanical-Advantage
 //
 // This program is free software; you can redistribute it and/or

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

@@ -1,4 +1,4 @@
-// Copyright 2021-2024 FRC 6328
+// Copyright 2021-2025 FRC 6328
 // http://github.com/Mechanical-Advantage
 //
 // This program is free software; you can redistribute it and/or

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

@@ -1,4 +1,4 @@
-// Copyright 2021-2024 FRC 6328
+// Copyright 2021-2025 FRC 6328
 // http://github.com/Mechanical-Advantage
 //
 // This program is free software; you can redistribute it and/or

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

@@ -1,4 +1,4 @@
-// Copyright 2021-2024 FRC 6328
+// Copyright 2021-2025 FRC 6328
 // http://github.com/Mechanical-Advantage
 //
 // This program is free software; you can redistribute it and/or

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

@@ -1,4 +1,4 @@
-// Copyright 2021-2024 FRC 6328
+// Copyright 2021-2025 FRC 6328
 // http://github.com/Mechanical-Advantage
 //
 // This program is free software; you can redistribute it and/or

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

@@ -1,4 +1,4 @@
-// Copyright 2021-2024 FRC 6328
+// Copyright 2021-2025 FRC 6328
 // http://github.com/Mechanical-Advantage
 //
 // This program is free software; you can redistribute it and/or

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

@@ -1,4 +1,4 @@
-// Copyright 2021-2024 FRC 6328
+// Copyright 2021-2025 FRC 6328
 // http://github.com/Mechanical-Advantage
 //
 // This program is free software; you can redistribute it and/or

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

@@ -1,4 +1,4 @@
-// Copyright 2021-2024 FRC 6328
+// Copyright 2021-2025 FRC 6328
 // http://github.com/Mechanical-Advantage
 //
 // This program is free software; you can redistribute it and/or

templates/AdvantageKit_SparkSwerveTemplate-maple-sim/src/main/java/frc/robot/subsystems/vision/Vision.java

@@ -1,4 +1,4 @@
-// Copyright 2021-2024 FRC 6328
+// Copyright 2021-2025 FRC 6328
 // http://github.com/Mechanical-Advantage
 //
 // This program is free software; you can redistribute it and/or

templates/AdvantageKit_SparkSwerveTemplate-maple-sim/src/main/java/frc/robot/subsystems/vision/VisionConstants.java

@@ -1,4 +1,4 @@
-// Copyright 2021-2024 FRC 6328
+// Copyright 2021-2025 FRC 6328
 // http://github.com/Mechanical-Advantage
 //
 // This program is free software; you can redistribute it and/or

templates/AdvantageKit_SparkSwerveTemplate-maple-sim/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVisionSim.java

@@ -1,4 +1,4 @@
-// Copyright 2021-2024 FRC 6328
+// Copyright 2021-2025 FRC 6328
 // http://github.com/Mechanical-Advantage
 //
 // This program is free software; you can redistribute it and/or

templates/AdvantageKit_SparkSwerveTemplate-maple-sim/src/main/java/frc/robot/util/SparkUtil.java

@@ -1,4 +1,4 @@
-// Copyright 2021-2024 FRC 6328
+// Copyright 2021-2025 FRC 6328
 // http://github.com/Mechanical-Advantage
 //
 // This program is free software; you can redistribute it and/or

templates/AdvantageKit_TalonSwerveTemplate-maple-sim/LICENSE-IronMaple.md

@@ -6,7 +6,7 @@
 #### This project uses [maple-sim](https://github.com/Shenzhen-Robotics-Alliance/maple-sim) for simulation.
 If you encounter any bugs related to drivetrain physics simulation, please [submit an issue to maple-sim](https://github.com/Shenzhen-Robotics-Alliance/maple-sim/issues/new/choose).
 
-## MIT License, Copyright (c) 2024 Team5516 "Iron Maple"
+## MIT License, Copyright (c) 2024-2025 Team5516 "Iron Maple"
 
 **Permission is hereby granted, free of charge, to any person obtaining a copyof this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:**
 

templates/AdvantageKit_TalonSwerveTemplate-maple-sim/src/main/java/frc/robot/Robot.java

@@ -1,4 +1,4 @@
-// Copyright 2021-2024 FRC 6328
+// Copyright 2021-2025 FRC 6328
 // http://github.com/Mechanical-Advantage
 //
 // This program is free software; you can redistribute it and/or

templates/AdvantageKit_TalonSwerveTemplate-maple-sim/src/main/java/frc/robot/RobotContainer.java

@@ -1,4 +1,4 @@
-// Copyright 2021-2024 FRC 6328
+// Copyright 2021-2025 FRC 6328
 // http://github.com/Mechanical-Advantage
 //
 // This program is free software; you can redistribute it and/or

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

@@ -1,4 +1,4 @@
-// Copyright 2021-2024 FRC 6328
+// Copyright 2021-2025 FRC 6328
 // http://github.com/Mechanical-Advantage
 //
 // This program is free software; you can redistribute it and/or

templates/AdvantageKit_TalonSwerveTemplate-maple-sim/src/main/java/frc/robot/subsystems/drive/ModuleIO.java

@@ -1,4 +1,4 @@
-// Copyright 2021-2024 FRC 6328
+// Copyright 2021-2025 FRC 6328
 // http://github.com/Mechanical-Advantage
 //
 // This program is free software; you can redistribute it and/or

templates/AdvantageKit_TalonSwerveTemplate-maple-sim/src/main/java/frc/robot/subsystems/drive/PhoenixOdometryThread.java

@@ -1,4 +1,4 @@
-// Copyright 2021-2024 FRC 6328
+// Copyright 2021-2025 FRC 6328
 // http://github.com/Mechanical-Advantage
 //
 // This program is free software; you can redistribute it and/or

templates/AdvantageKit_TalonSwerveTemplate-maple-sim/src/main/java/frc/robot/subsystems/vision/VisionIO.java

@@ -1,4 +1,4 @@
-// Copyright 2021-2024 FRC 6328
+// Copyright 2021-2025 FRC 6328
 // http://github.com/Mechanical-Advantage
 //
 // This program is free software; you can redistribute it and/or

templates/AdvantageKit_TalonSwerveTemplate-maple-sim/src/main/java/frc/robot/subsystems/vision/VisionIOLimelight.java

@@ -1,4 +1,4 @@
-// Copyright 2021-2024 FRC 6328
+// Copyright 2021-2025 FRC 6328
 // http://github.com/Mechanical-Advantage
 //
 // This program is free software; you can redistribute it and/or

templates/AdvantageKit_TalonSwerveTemplate-maple-sim/src/main/java/frc/robot/subsystems/vision/VisionIOPhotonVision.java

@@ -1,4 +1,4 @@
-// Copyright 2021-2024 FRC 6328
+// Copyright 2021-2025 FRC 6328
 // http://github.com/Mechanical-Advantage
 //
 // This program is free software; you can redistribute it and/or

templates/AdvantageKit_TalonSwerveTemplate-maple-sim/src/main/java/frc/robot/util/PhoenixUtil.java

@@ -1,4 +1,4 @@
-// Copyright 2021-2024 FRC 6328
+// Copyright 2021-2025 FRC 6328
 // http://github.com/Mechanical-Advantage
 //
 // This program is free software; you can redistribute it and/or

templates/AdvantageKit_TalonSwerveTemplate_EnhancedPhoenixSimulation/LICENSE-IronMaple.md

@@ -6,7 +6,7 @@
 #### This project uses [maple-sim](https://github.com/Shenzhen-Robotics-Alliance/maple-sim) for simulation.
 If you encounter any bugs related to drivetrain physics simulation, please [submit an issue to maple-sim](https://github.com/Shenzhen-Robotics-Alliance/maple-sim/issues/new/choose).
 
-## MIT License, Copyright (c) 2024 Team5516 "Iron Maple"
+## MIT License, Copyright (c) 2024-2025 Team5516 "Iron Maple"
 
 **Permission is hereby granted, free of charge, to any person obtaining a copyof this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions:**
 

templates/AdvantageKit_TalonSwerveTemplate_EnhancedPhoenixSimulation/src/main/java/frc/robot/subsystems/drive/Drive.java

@@ -1,4 +1,4 @@
-// Copyright 2021-2024 FRC 6328
+// Copyright 2021-2025 FRC 6328
 // http://github.com/Mechanical-Advantage
 //
 // This program is free software; you can redistribute it and/or

templates/AdvantageKit_TalonSwerveTemplate_EnhancedPhoenixSimulation/src/main/java/frc/robot/subsystems/drive/ModuleIO.java

@@ -1,4 +1,4 @@
-// Copyright 2021-2024 FRC 6328
+// Copyright 2021-2025 FRC 6328
 // http://github.com/Mechanical-Advantage
 //
 // This program is free software; you can redistribute it and/or