code cleanup

diff --git a/.vscode/settings.json b/.vscode/settings.json
index 6f5f6cb7ab6c54a539eef8007651b03bc4d066b2..2af50e31933c6f1eafae4c22cf2c0654edcaf6e6 100644 (file)
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -58,5 +58,4 @@
     "edu.wpi.first.math.**.struct.*",
   ],
   "java.dependency.enableDependencyCheckup": false,
-  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx2G -Xms100m -Xlog:disable"
 }

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 180827f51f205aac3bc44eaab530509f83701132..54e3ec4093b15f05a5f6c1c41bb8b31dff8ee116 100644 (file)
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -73,7 +73,6 @@ public class RobotContainer {
    * Different robots may have different subsystems.
    */
   public RobotContainer(RobotId robotId) {
-    // climb = new Climb();
     // display the current robot id on smartdashboard
     SmartDashboard.putString("RobotID", robotId.toString());
 
@@ -94,21 +93,26 @@ public class RobotContainer {
         intake = new Intake();
         climb = new Climb();
         spindexer = new Spindexer();
+        break;
 
       case WaffleHouse: // AKA Betabot
         turret = new Turret();
         shooter = new Shooter();
-        //hood = new Hood();
+        // hood = new Hood();
+        break;
 
       case SwerveCompetition: // AKA "Vantage"
+        break;
 
       case BetaBot: // AKA "Pancake"
         vision = new Vision(VisionConstants.APRIL_TAG_CAMERAS);
-        // fall-through
+        break;
 
       case Vivace:
+        break;
 
       case Phil: // AKA "IHOP"
+        break;
 
       case Vertigo: // AKA "French Toast"
         drive = new Drivetrain(vision, new GyroIOPigeon2());
@@ -117,11 +121,11 @@ public class RobotContainer {
 
         // Detected objects need access to the drivetrain
         DetectedObject.setDrive(drive);
-        
+
         // SignalLogger.start();
         driver.configureControls();
         operator.configureControls();
-        
+
         initializeAutoBuilder();
         registerCommands();
         PathGroupLoader.loadPathGroups();
@@ -134,7 +138,7 @@ public class RobotContainer {
         }
         drive.setDefaultCommand(new DefaultDriveCommand(drive, driver));
         break;
-      }
+    }
 
     // This is really annoying so it's disabled
     DriverStation.silenceJoystickConnectionWarning(true);
@@ -155,7 +159,6 @@ public class RobotContainer {
       drive.setVisionEnabled(enabled);
   }
 
-
   public void initializeAutoBuilder() {
     AutoBuilder.configure(
         () -> drive.getPose(),
@@ -192,15 +195,14 @@ public class RobotContainer {
   }
 
   public boolean brownout() {
-    if(RobotController.getBatteryVoltage() < 6.0) {
+    if (RobotController.getBatteryVoltage() < 6.0) {
       return true;
-    }
-    else {
+    } else {
       return false;
     }
   }
 
-// Autos 
+  // Autos
 
   /**
    * Initialize the SendableChooser on the SmartDashbboard.
@@ -219,6 +221,7 @@ public class RobotContainer {
 
   /**
    * Gets the auto command from SmartDashboard
+   * 
    * @return
    */
   public Command getAutoCommand() {
@@ -228,16 +231,14 @@ public class RobotContainer {
     return autoSelected;
   }
 
-  public void logComponents(){
-    if(!Constants.LOG_MECHANISMS) return;
-    
+  public void logComponents() {
+    if (!Constants.LOG_MECHANISMS)
+      return;
+
     Logger.recordOutput(
-      "ComponentPoses", 
-      new Pose3d[] {
+        "ComponentPoses",
+        new Pose3d[] {
         // Subsystem Pose3ds
-      }
-    );
+        });
   }
 }
-
-

diff --git a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
index 3ce8df222e5025d8a47797a7352bc9813a210536..7aa4cd543b26fcfcd11ea6c83b70ecd1f6d3c2dd 100644 (file)
--- a/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
+++ b/src/main/java/frc/robot/commands/gpm/AutoShootCommand.java
@@ -34,6 +34,7 @@ public class AutoShootCommand extends Command {
     private Shooter shooter;
     private Spindexer spindexer;
 
+    // TODO: Implement state machine with WantedState/CurrentState
     private enum WantedState {
         IDLE,
         SHOOTING,
@@ -51,12 +52,9 @@ public class AutoShootCommand extends Command {
     private WantedState wantedState = WantedState.IDLE;
     private CurrentState currentState = CurrentState.IDLE;
 
-    private void updateStates(){
-
-    }
-
-    //TODO: find maximum interpolation
-    private Constraints shooterConstraints = new Constraints(Units.inchesToMeters(80.0), 67676767, HoodConstants.MIN_ANGLE, HoodConstants.MAX_ANGLE);
+    // TODO: find maximum interpolation
+    private Constraints shooterConstraints = new Constraints(Units.inchesToMeters(80.0), 67676767,
+            HoodConstants.MIN_ANGLE, HoodConstants.MAX_ANGLE);
 
     private double turretSetpoint;
     private double hoodSetpoint;
@@ -65,7 +63,7 @@ public class AutoShootCommand extends Command {
 
     private final LinearFilter turretAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME));
     private final LinearFilter hoodAngleFilter = LinearFilter.movingAverage((int) (0.1 / Constants.LOOP_TIME));
-    
+
     private Rotation2d lastTurretAngle;
     private Rotation2d turretAngle;
     private double turretVelocity;
@@ -88,83 +86,94 @@ public class AutoShootCommand extends Command {
         this.hood = hood;
         this.shooter = shooter;
         this.spindexer = spindexer;
-        drivepose  = drivetrain.getPose();
-        //drivepose  = new Pose2d(drivepose.getTranslation(), drivepose.getRotation().plus(new Rotation2d(Math.PI)));
+        drivepose = drivetrain.getPose();
+        // drivepose = new Pose2d(drivepose.getTranslation(),
+        // drivepose.getRotation().plus(new Rotation2d(Math.PI)));
 
         goalState = ShooterPhysics.getShotParams(
-                               FieldConstants.getHubTranslation().minus(new Translation3d(drivepose.getTranslation())),
-                               8.0); // Random initial goalState to prevent it being null
-        
+                FieldConstants.getHubTranslation().minus(new Translation3d(drivepose.getTranslation())),
+                8.0); // Random initial goalState to prevent it being null
+
         addRequirements(turret);
     }
 
     public void updateSetpoints(Pose2d drivepose) {
-        Pose2d turretPosition = drivepose.transformBy(new Transform2d((new Translation2d(TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX(),TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY())), new Rotation2d()));
+        Pose2d turretPosition = drivepose
+                .transformBy(new Transform2d((new Translation2d(TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX(),
+                        TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY())), new Rotation2d()));
         double turretToTargetDistance = target.getDistance(turretPosition.getTranslation());
-        
+
         // If the robot is moving, adjust the target position based on velocity
         ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds();
         ChassisSpeeds fieldRelVel = ChassisSpeeds.fromRobotRelativeSpeeds(robotRelVel, drivetrain.getYaw());
 
-        double turretVelocityX =
-            fieldRelVel.vxMetersPerSecond
+        double turretVelocityX = fieldRelVel.vxMetersPerSecond
                 + fieldRelVel.omegaRadiansPerSecond
-                    * (TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY() * Math.cos(drivepose.getRotation().getRadians())
-                        - TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX() * Math.sin(drivepose.getRotation().getRadians()));
-        double turretVelocityY =
-            fieldRelVel.vyMetersPerSecond
+                        * (TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY()
+                                * Math.cos(drivepose.getRotation().getRadians())
+                                - TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX()
+                                        * Math.sin(drivepose.getRotation().getRadians()));
+        double turretVelocityY = fieldRelVel.vyMetersPerSecond
                 + fieldRelVel.omegaRadiansPerSecond
-                    * (TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX() * Math.cos(drivepose.getRotation().getRadians())
-                        - TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY() * Math.sin(drivepose.getRotation().getRadians()));
+                        * (TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getX()
+                                * Math.cos(drivepose.getRotation().getRadians())
+                                - TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getY()
+                                        * Math.sin(drivepose.getRotation().getRadians()));
 
         double timeOfFlight;
         Pose2d lookaheadPose = turretPosition;
-        //double lookaheadTurretToTargetDistance = turretToTargetDistance;
+        // double lookaheadTurretToTargetDistance = turretToTargetDistance;
 
         /*
          * Loop (20) until lookaheadPose converges BECAUSE -->
-         * If you're 8m away (t = 1.0s) and moving at 2m/s towards target, you calculate for 6m (t = 0.8s)
-         * At 6m, we run assuming t = 0.8 but then the 6m isn't correct since it was derived using t = 1.0s
+         * If you're 8m away (t = 1.0s) and moving at 2m/s towards target, you calculate
+         * for 6m (t = 0.8s)
+         * At 6m, we run assuming t = 0.8 but then the 6m isn't correct since it was
+         * derived using t = 1.0s
          * So we make a bunch of guesses until it converges
          */
         for (int i = 0; i < 20; i++) {
-            Translation3d lookahead3d = new Translation3d(lookaheadPose.getX(), lookaheadPose.getY(), TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getZ());
-            Translation3d target3d = new Translation3d(target.getX(), target.getY(), FieldConstants.getHubTranslation().getZ()); // Add if statement so that it's only when it's shooting
+            Translation3d lookahead3d = new Translation3d(lookaheadPose.getX(), lookaheadPose.getY(),
+                    TurretConstants.DISTANCE_FROM_ROBOT_CENTER.getZ());
+            Translation3d target3d = new Translation3d(target.getX(), target.getY(),
+                    FieldConstants.getHubTranslation().getZ()); // Add if statement so that it's only when it's shooting
             goalState = ShooterPhysics.getShotParams(
-                               target3d.minus(lookahead3d),
-                               2.0);
+                    target3d.minus(lookahead3d),
+                    2.0);
 
             timeOfFlight = goalState.timeOfFlight();
             double offsetX = turretVelocityX * timeOfFlight;
             double offsetY = turretVelocityY * timeOfFlight;
-            lookaheadPose =
-                new Pose2d(
+            lookaheadPose = new Pose2d(
                     turretPosition.getTranslation().plus(new Translation2d(offsetX, offsetY)),
                     turretPosition.getRotation());
-            //lookaheadTurretToTargetDistance = target.getDistance(lookaheadPose.getTranslation());
+            // lookaheadTurretToTargetDistance =
+            // target.getDistance(lookaheadPose.getTranslation());
         }
 
         turretAngle = target.minus(lookaheadPose.getTranslation()).getAngle();
         if (lastTurretAngle == null) {
             lastTurretAngle = turretAngle;
         }
-        turretVelocity =
-        turretAngleFilter.calculate(
-            turretAngle.minus(lastTurretAngle).getRadians() / Constants.LOOP_TIME);
+        turretVelocity = turretAngleFilter.calculate(
+                turretAngle.minus(lastTurretAngle).getRadians() / Constants.LOOP_TIME);
         lastTurretAngle = turretAngle;
 
         Logger.recordOutput("Lookahead Pose", lookaheadPose);
 
-        double adjustedTurretSetpoint = MathUtil.angleModulus(turretAngle.getRadians() - drivepose.getRotation().getRadians());
+        double adjustedTurretSetpoint = MathUtil
+                .angleModulus(turretAngle.getRadians() - drivepose.getRotation().getRadians());
 
         // Shortest path
-        double error = MathUtil.inputModulus(Units.radiansToDegrees(adjustedTurretSetpoint) - Units.radiansToDegrees(turret.getPositionRad()), -180, 180);
+        double error = MathUtil.inputModulus(
+                Units.radiansToDegrees(adjustedTurretSetpoint) - Units.radiansToDegrees(turret.getPositionRad()), -180,
+                180);
         double potentialSetpoint = Units.radiansToDegrees(turret.getPositionRad()) + error;
-        // Stay within +/- 200 -- if  shortest path is past 200, we go long way around
+        // Stay within +/- 200 -- if shortest path is past 200, we go long way around
         double turretRange = TurretConstants.MAX_ANGLE - TurretConstants.MIN_ANGLE;
-        if (potentialSetpoint > turretRange/2) {
+        if (potentialSetpoint > turretRange / 2) {
             potentialSetpoint -= 360;
-        } else if (potentialSetpoint < -turretRange/2) {
+        } else if (potentialSetpoint < -turretRange / 2) {
             potentialSetpoint += 360;
         }
 
@@ -172,7 +181,8 @@ public class AutoShootCommand extends Command {
 
         // Pitch is in radians
         hoodAngle = goalState.pitch();
-        hoodSetpoint = MathUtil.clamp(Units.radiansToDegrees(hoodAngle), HoodConstants.MIN_ANGLE, HoodConstants.MAX_ANGLE);
+        hoodSetpoint = MathUtil.clamp(Units.radiansToDegrees(hoodAngle), HoodConstants.MIN_ANGLE,
+                HoodConstants.MAX_ANGLE);
         hoodVelocity = hoodAngleFilter.calculate((hoodAngle - lastHoodAngle) / Constants.LOOP_TIME);
         lastHoodAngle = hoodAngle;
 
@@ -181,19 +191,19 @@ public class AutoShootCommand extends Command {
         }
     }
 
-    public void updateDrivePose(){
+    public void updateDrivePose() {
         Pose2d currentPose = drivetrain.getPose();
         // Add 180 degrees to the rotation bc robot is backwards
         drivepose = new Pose2d(
-            currentPose.getTranslation(), 
-            currentPose.getRotation()//.plus(new Rotation2d(Math.PI))
+                currentPose.getTranslation(),
+                currentPose.getRotation()
         );
         ChassisSpeeds robotRelVel = drivetrain.getChassisSpeeds();
         drivepose.exp(
-            new Twist2d(
-                robotRelVel.vxMetersPerSecond * phaseDelay,
-                robotRelVel.vyMetersPerSecond * phaseDelay,
-                robotRelVel.omegaRadiansPerSecond * phaseDelay));
+                new Twist2d(
+                        robotRelVel.vxMetersPerSecond * phaseDelay,
+                        robotRelVel.vyMetersPerSecond * phaseDelay,
+                        robotRelVel.omegaRadiansPerSecond * phaseDelay));
     }
 
     @Override
@@ -201,17 +211,18 @@ public class AutoShootCommand extends Command {
         updateDrivePose();
         updateSetpoints(drivepose);
 
-        turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)), turretVelocity - drivetrain.getAngularRate(2));
-        //hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(hoodSetpoint)), hoodVelocity);
-        //shooter.setShooter(ShotInterpolation.exitVelocityMap.get(goalState.exitVel()));
+        turret.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(turretSetpoint)),
+                turretVelocity - drivetrain.getAngularRate(2));
+        // hood.setFieldRelativeTarget(new
+        // Rotation2d(Units.degreesToRadians(hoodSetpoint)), hoodVelocity);
+        // shooter.setShooter(ShotInterpolation.exitVelocityMap.get(goalState.exitVel()));
 
         SmartDashboard.putNumber("Turret Calculated Setpoint", turretSetpoint);
         SmartDashboard.putNumber("Hood Calculate Setpoint", hoodSetpoint);
         SmartDashboard.putNumber("Shooter Calculate Velocity", goalState.exitVel());
-        System.out.println("COMMAND IS WORKINNGGG");
 
         /** Spindexer Stuff!! */
-        if(spindexer != null){
+        if (spindexer != null) {
             spindexer.maxSpindexer();
         }
     }
@@ -221,8 +232,9 @@ public class AutoShootCommand extends Command {
         // Set the turret to a safe position when the command ends
         turret.setFieldRelativeTarget(new Rotation2d(0.0), 0.0);
         shooter.setShooter(0.0);
-        //hood.setFieldRelativeTarget(new Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)), 0.0);
-        if(spindexer != null){
+        // hood.setFieldRelativeTarget(new
+        // Rotation2d(Units.degreesToRadians(HoodConstants.MAX_ANGLE)), 0.0);
+        if (spindexer != null) {
             spindexer.stopSpindexer();
         }
     }

diff --git a/vendordeps/REVLib.json b/vendordeps/REVLib.json
index 59be8469e2fc277ba227c917b536da9afe1a3240..d8ba7ce714c4d36b772fcb6c6b4794be31c2ab07 100644 (file)
--- a/vendordeps/REVLib.json
+++ b/vendordeps/REVLib.json
@@ -1,7 +1,7 @@
 {
     "fileName": "REVLib.json",
     "name": "REVLib",
-    "version": "2026.0.1",
+    "version": "2026.0.2",
     "frcYear": "2026",
     "uuid": "3f48eb8c-50fe-43a6-9cb7-44c86353c4cb",
     "mavenUrls": [
@@ -12,14 +12,14 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-java",
-            "version": "2026.0.1"
+            "version": "2026.0.2"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2026.0.1",
+            "version": "2026.0.2",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
@@ -34,7 +34,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "RevLibBackendDriver",
-            "version": "2026.0.1",
+            "version": "2026.0.2",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
@@ -49,7 +49,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "RevLibWpiBackendDriver",
-            "version": "2026.0.1",
+            "version": "2026.0.2",
             "skipInvalidPlatforms": true,
             "isJar": false,
             "validPlatforms": [
@@ -66,7 +66,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-cpp",
-            "version": "2026.0.1",
+            "version": "2026.0.2",
             "libName": "REVLib",
             "headerClassifier": "headers",
             "sharedLibrary": false,
@@ -83,7 +83,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "REVLib-driver",
-            "version": "2026.0.1",
+            "version": "2026.0.2",
             "libName": "REVLibDriver",
             "headerClassifier": "headers",
             "sharedLibrary": false,
@@ -100,7 +100,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "RevLibBackendDriver",
-            "version": "2026.0.1",
+            "version": "2026.0.2",
             "libName": "BackendDriver",
             "sharedLibrary": true,
             "skipInvalidPlatforms": true,
@@ -116,7 +116,7 @@
         {
             "groupId": "com.revrobotics.frc",
             "artifactId": "RevLibWpiBackendDriver",
-            "version": "2026.0.1",
+            "version": "2026.0.2",
             "libName": "REVLibWpi",
             "sharedLibrary": true,
             "skipInvalidPlatforms": true,
@@ -128,7 +128,6 @@
                 "linuxarm32",
                 "osxuniversal"
             ]
-
         }
     ]
 }
\ No newline at end of file

diff --git a/vendordeps/yams.json b/vendordeps/yams.json
deleted file mode 100644 (file)
index 9557c14..0000000
--- a/vendordeps/yams.json
+++ /dev/null
@@ -1,21 +0,0 @@
-{
-    "fileName": "yams.json",
-    "name": "Yet Another Mechanism System",
-    "version": "2026.2.3",
-    "frcYear": "2026",
-    "uuid": "a1051e86-a979-4880-a28b-a0d5362d1d96",
-    "mavenUrls": [
-        "https://yet-another-software-suite.github.io/YAMS/releases/"
-    ],
-    "jsonUrl": "https://yet-another-software-suite.github.io/YAMS/yams.json",
-    "javaDependencies": [
-        {
-            "groupId": "yams",
-            "artifactId": "YAMS-java",
-            "version": "2026.2.3"
-        }
-    ],
-    "cppDependencies": [],
-    "jniDependencies": [],
-    "requires": []
-}
\ No newline at end of file