Ls updated main

build.gradle

@@ -1,6 +1,6 @@
 plugins {
     id "java"
-    id "edu.wpi.first.GradleRIO" version "2026.1.1"
+    id "edu.wpi.first.GradleRIO" version "2026.2.1"
 }
 
 java {

gradle/wrapper/gradle-wrapper.properties

@@ -1,7 +1,6 @@
+#Mon Mar 30 17:42:42 EDT 2026
 distributionBase=GRADLE_USER_HOME
 distributionPath=permwrapper/dists
-distributionUrl=https\://services.gradle.org/distributions/gradle-8.11-bin.zip
-networkTimeout=10000
-validateDistributionUrl=true
+distributionUrl=https\://services.gradle.org/distributions/gradle-8.14.4-bin.zip
 zipStoreBase=GRADLE_USER_HOME
 zipStorePath=permwrapper/dists

src/main/deploy/choreo/Depot_Fast.traj

@@ -0,0 +1,79 @@
+{
+ "name":"Depot_Fast",
+ "version":3,
+ "snapshot":{
+  "waypoints":[
+    {"x":3.596, "y":5.06, "heading":3.141592653589793, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":0.3662222921848297, "y":5.994427680969238, "heading":3.141592653589793, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":0.0, "y":0.0, "w":16.541, "h":8.0692}}, "enabled":false}],
+  "targetDt":0.05
+ },
+ "params":{
+  "waypoints":[
+    {"x":{"exp":"3.596 m", "val":3.596}, "y":{"exp":"5.06 m", "val":5.06}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":21, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false},
+    {"x":{"exp":"0.3662222921848297 m", "val":0.3662222921848297}, "y":{"exp":"5.994427680969238 m", "val":5.994427680969238}, "heading":{"exp":"180 deg", "val":3.141592653589793}, "intervals":40, "split":false, "fixTranslation":true, "fixHeading":true, "overrideIntervals":false}],
+  "constraints":[
+    {"from":"first", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"last", "to":null, "data":{"type":"StopPoint", "props":{}}, "enabled":true},
+    {"from":"first", "to":"last", "data":{"type":"KeepInRectangle", "props":{"x":{"exp":"0 m", "val":0.0}, "y":{"exp":"0 m", "val":0.0}, "w":{"exp":"16.541 m", "val":16.541}, "h":{"exp":"8.0692 m", "val":8.0692}}}, "enabled":false}],
+  "targetDt":{
+   "exp":"0.05 s",
+   "val":0.05
+  }
+ },
+ "trajectory":{
+  "config":{
+   "frontLeft":{
+    "x":0.2794,
+    "y":0.2794
+   },
+   "backLeft":{
+    "x":-0.2794,
+    "y":0.2794
+   },
+   "mass":45.359237,
+   "inertia":6.0,
+   "gearing":6.5,
+   "radius":0.0508,
+   "vmax":628.3185307179587,
+   "tmax":1.2,
+   "cof":1.5,
+   "bumper":{
+    "front":0.43,
+    "side":0.43,
+    "back":0.43
+   },
+   "differentialTrackWidth":0.5588
+  },
+  "sampleType":"Swerve",
+  "waypoints":[0.0,1.04949],
+  "samples":[
+    {"t":0.0, "x":3.596, "y":5.06, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":-12.99576, "ay":3.75989, "alpha":0.0, "fx":[-147.36948,-147.36948,-147.36948,-147.36948], "fy":[42.63641,42.63641,42.63641,42.63641]},
+    {"t":0.04998, "x":3.57977, "y":5.0647, "heading":3.14159, "vx":-0.64947, "vy":0.1879, "omega":0.0, "ax":-12.99392, "ay":3.75935, "alpha":0.0, "fx":[-147.34857,-147.34857,-147.34857,-147.34857], "fy":[42.63036,42.63036,42.63036,42.63036]},
+    {"t":0.09995, "x":3.53109, "y":5.07878, "heading":3.14159, "vx":-1.29886, "vy":0.37578, "omega":0.0, "ax":-12.99134, "ay":3.75861, "alpha":0.0, "fx":[-147.31929,-147.31929,-147.31929,-147.31929], "fy":[42.62189,42.62189,42.62189,42.62189]},
+    {"t":0.14993, "x":3.44995, "y":5.10225, "heading":3.14159, "vx":-1.94811, "vy":0.56362, "omega":0.0, "ax":-12.98746, "ay":3.75748, "alpha":0.0, "fx":[-147.27529,-147.27529,-147.27529,-147.27529], "fy":[42.60916,42.60916,42.60916,42.60916]},
+    {"t":0.1999, "x":3.33637, "y":5.13511, "heading":3.14159, "vx":-2.59717, "vy":0.7514, "omega":0.0, "ax":-12.98098, "ay":3.75561, "alpha":0.0, "fx":[-147.20183,-147.20183,-147.20183,-147.20183], "fy":[42.5879,42.5879,42.5879,42.5879]},
+    {"t":0.24988, "x":3.19037, "y":5.17736, "heading":3.14159, "vx":-3.2459, "vy":0.93909, "omega":0.0, "ax":-12.96798, "ay":3.75185, "alpha":0.0, "fx":[-147.05443,-147.05443,-147.05443,-147.05443], "fy":[42.54526,42.54526,42.54526,42.54526]},
+    {"t":0.29986, "x":3.01196, "y":5.22897, "heading":3.14159, "vx":-3.89399, "vy":1.1266, "omega":0.0, "ax":-12.92876, "ay":3.7405, "alpha":0.0, "fx":[-146.60968,-146.60968,-146.60968,-146.60968], "fy":[42.41658,42.41658,42.41658,42.41658]},
+    {"t":0.34983, "x":2.80121, "y":5.28995, "heading":3.14159, "vx":-4.54012, "vy":1.31353, "omega":0.0, "ax":-3.46426, "ay":1.00227, "alpha":0.0, "fx":[-39.28405,-39.28405,-39.28405,-39.28405], "fy":[11.36552,11.36552,11.36552,11.36552]},
+    {"t":0.39981, "x":2.56998, "y":5.35684, "heading":3.14159, "vx":-4.71325, "vy":1.36362, "omega":0.0, "ax":-0.00026, "ay":0.00008, "alpha":0.0, "fx":[-0.00296,-0.00296,-0.00296,-0.00296], "fy":[0.00086,0.00086,0.00086,0.00086]},
+    {"t":0.44978, "x":2.33443, "y":5.42499, "heading":3.14159, "vx":-4.71326, "vy":1.36362, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.49976, "x":2.09889, "y":5.49314, "heading":3.14159, "vx":-4.71326, "vy":1.36362, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.54973, "x":1.86334, "y":5.56129, "heading":3.14159, "vx":-4.71326, "vy":1.36362, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]},
+    {"t":0.59971, "x":1.62779, "y":5.62944, "heading":3.14159, "vx":-4.71326, "vy":1.36362, "omega":0.0, "ax":0.00026, "ay":-0.00008, "alpha":0.0, "fx":[0.00296,0.00296,0.00296,0.00296], "fy":[-0.00086,-0.00086,-0.00086,-0.00086]},
+    {"t":0.64969, "x":1.39224, "y":5.69758, "heading":3.14159, "vx":-4.71325, "vy":1.36362, "omega":0.0, "ax":3.46426, "ay":-1.00227, "alpha":0.0, "fx":[39.28405,39.28405,39.28405,39.28405], "fy":[-11.36552,-11.36552,-11.36552,-11.36552]},
+    {"t":0.69966, "x":1.16102, "y":5.76448, "heading":3.14159, "vx":-4.54012, "vy":1.31353, "omega":0.0, "ax":12.92876, "ay":-3.7405, "alpha":0.0, "fx":[146.60968,146.60968,146.60968,146.60968], "fy":[-42.41658,-42.41658,-42.41658,-42.41658]},
+    {"t":0.74964, "x":0.95027, "y":5.82545, "heading":3.14159, "vx":-3.89399, "vy":1.1266, "omega":0.0, "ax":12.96798, "ay":-3.75185, "alpha":0.0, "fx":[147.05443,147.05443,147.05443,147.05443], "fy":[-42.54526,-42.54526,-42.54526,-42.54526]},
+    {"t":0.79961, "x":0.77185, "y":5.87707, "heading":3.14159, "vx":-3.2459, "vy":0.93909, "omega":0.0, "ax":12.98098, "ay":-3.75561, "alpha":0.0, "fx":[147.20183,147.20183,147.20183,147.20183], "fy":[-42.5879,-42.5879,-42.5879,-42.5879]},
+    {"t":0.84959, "x":0.62585, "y":5.91931, "heading":3.14159, "vx":-2.59717, "vy":0.7514, "omega":0.0, "ax":12.98746, "ay":-3.75748, "alpha":0.0, "fx":[147.27529,147.27529,147.27529,147.27529], "fy":[-42.60916,-42.60916,-42.60916,-42.60916]},
+    {"t":0.89957, "x":0.51227, "y":5.95217, "heading":3.14159, "vx":-1.94811, "vy":0.56362, "omega":0.0, "ax":12.99134, "ay":-3.75861, "alpha":0.0, "fx":[147.31929,147.31929,147.31929,147.31929], "fy":[-42.62189,-42.62189,-42.62189,-42.62189]},
+    {"t":0.94954, "x":0.43114, "y":5.97565, "heading":3.14159, "vx":-1.29886, "vy":0.37578, "omega":0.0, "ax":12.99392, "ay":-3.75935, "alpha":0.0, "fx":[147.34857,147.34857,147.34857,147.34857], "fy":[-42.63036,-42.63036,-42.63036,-42.63036]},
+    {"t":0.99952, "x":0.38245, "y":5.98973, "heading":3.14159, "vx":-0.64947, "vy":0.1879, "omega":0.0, "ax":12.99576, "ay":-3.75989, "alpha":0.0, "fx":[147.36948,147.36948,147.36948,147.36948], "fy":[-42.63641,-42.63641,-42.63641,-42.63641]},
+    {"t":1.04949, "x":0.36622, "y":5.99443, "heading":3.14159, "vx":0.0, "vy":0.0, "omega":0.0, "ax":0.0, "ay":0.0, "alpha":0.0, "fx":[0.0,0.0,0.0,0.0], "fy":[0.0,0.0,0.0,0.0]}],
+  "splits":[0]
+ },
+ "events":[]
+}

src/main/java/frc/robot/Drive.java

@@ -9,8 +9,8 @@
 
 
 public class Drive extends SubsystemBase{
-    private final PIDController xController = new PIDController(.01, 0.0, 0.0);
-    private final PIDController yController = new PIDController(.01, 0.0, 0.0);
+    private final PIDController xController = new PIDController(10, 0.0, 0.0);
+    private final PIDController yController = new PIDController(10, 0.0, 0.0);
     private final PIDController headingController = new PIDController(.01, 0.0, 0.0);
     private SwerveSubsystem subsystem;
 

src/main/java/frc/robot/Robot.java

@@ -144,7 +144,7 @@ public void robotPeriodic() {
                 SmartDashboard.putString("Alliance Color", Robot.allianceColorString());
                 if (Constants.currentMode == Constants.Mode.SIM) {
                     Logger.recordOutput("SimPose", robotContainer.getDriveBase().getSimulationPose().get());
-                    Logger.recordOutput("OdomPose", robotContainer.getDriveBase().getSimulationPose().get());
+                    Logger.recordOutput("OdomPose", robotContainer.getDriveBase().getSimulationRawOdomPose());
                 }
             }
         }

src/main/java/frc/robot/RobotContainer.java

@@ -11,6 +11,7 @@
 import choreo.auto.AutoFactory;
 import choreo.auto.AutoRoutine;
 import choreo.auto.AutoTrajectory;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.Filesystem;
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -83,6 +84,9 @@
 import frc.robot.utils.simulation.RobotVisualizer;
 import swervelib.SwerveInputStream;
 import swervelib.imu.SwerveIMU;
+import swervelib.simulation.ironmaple.simulation.SimulatedArena;
+import swervelib.simulation.ironmaple.simulation.seasonspecific.rebuilt2026.RebuiltFuelOnField;
+
 
 /**
  * This class is where the bulk of the robot should be declared. Since
@@ -150,15 +154,16 @@ public RobotContainer() {
                                 turretSubsystem = new TurretSubsystem(TurretSubsystem.createRealIo());
                                 climberSubsystem = new ClimberSubsystem(ClimberSubsystem.createRealIo());
                                 feederSubsystem = new FeederSubsystem(FeederSubsystem.createRealIo());
-                                shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createRealIo());
+
                                 RealGyroIo gyroIo = (RealGyroIo) GyroSubsystem.createRealIo();
                                 ThreadedGyro threadedGyro = gyroIo.getThreadedGyro();
                                 gyroSubsystem = new GyroSubsystem(gyroIo);
                                 SwerveIMU swerveIMU = new ThreadedGyroSwerveIMU(threadedGyro);
                                 drivebase = !Constants.TESTBED ? new SwerveSubsystem(
                                                 new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), swerveIMU) : null;
-                            apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createRealIo(), drivebase, truster) : null;
-                            controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase,intakeDeployer, this) : null;
+                            shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createRealIo());
+                                apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createRealIo(), drivebase, truster) : null;
+                            controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase,intakeDeployer, this, turretSubsystem) : null;
                                  lightStripSubsystem = new LightStripSubsystem(drivebase, shootState);
                                 powerDistribution.setSwitchableChannel(true);
                                 }
@@ -169,14 +174,15 @@ public RobotContainer() {
                                 climberSubsystem = new ClimberSubsystem(ClimberSubsystem.createMockIo());
                                 feederSubsystem = new FeederSubsystem(FeederSubsystem.createMockIo());
                                 turretSubsystem = new TurretSubsystem(TurretSubsystem.createMockIo());
-                                shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createMockIo());
+
                                 intakeDeployer = new IntakeDeployerSubsystem(IntakeDeployerSubsystem.createMockIo());
 
                                 // No GyroSubsystem in REPLAY for now
                 // create the drive subsystem with null gyro (use default json)
                 drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), null) : null;
+                            shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createMockIo());
                 apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createMockIo(), drivebase, truster) : null;
-                controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, intakeDeployer, this) : null;
+                controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, intakeDeployer, this, turretSubsystem) : null;
                                 lightStripSubsystem = new LightStripSubsystem(drivebase, shootState);
 
             }
@@ -188,13 +194,14 @@ public RobotContainer() {
                                 climberSubsystem = new ClimberSubsystem(ClimberSubsystem.createSimIo(robotVisualizer));
                                 feederSubsystem = new FeederSubsystem(FeederSubsystem.createSimIo(robotVisualizer));
                                 turretSubsystem = new TurretSubsystem(TurretSubsystem.createSimIo(robotVisualizer));
-                                shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createSimIo(robotVisualizer));
+
                                 intakeDeployer = new IntakeDeployerSubsystem(
                                                 IntakeDeployerSubsystem.createSimIo(robotVisualizer));// No GyroSubsystem in REPLAY for now
 
                                                 // create the drive subsystem with null gyro (use default json)
                 drivebase = !Constants.TESTBED ? new SwerveSubsystem(new File(Filesystem.getDeployDirectory(), "YAGSL/" + Constants.SWERVE_JSON_DIRECTORY), null) : null;
-                controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, intakeDeployer, this) : null;
+                shooterSubsystem = new ShooterSubsystem(ShooterSubsystem.createSimIo(robotVisualizer));
+                controllerSubsystem = !Constants.TESTBED ? new ControllerSubsystem(drivebase, intakeDeployer, this, turretSubsystem) : null;
                 apriltagSubsystem = !Constants.TESTBED ? new ApriltagSubsystem(ApriltagSubsystem.createSimIo(truster,drivebase), drivebase, truster) : null;
                                 lightStripSubsystem = new LightStripSubsystem(drivebase, shootState);
 
@@ -209,6 +216,7 @@ public RobotContainer() {
                 autoChooser = new AutoChooser(drivebase, shootState, autoFactory, shooterSubsystem, climberSubsystem, feederSubsystem, hopperSubsystem, turretSubsystem, anglerSubsystem, controllerSubsystem, intakeDeployer);
                 configureBindings();
                 putShuffleboardCommands();
+
         }
 
 
@@ -336,6 +344,10 @@ private void configureBindings() {
         }
 
         public void putShuffleboardCommands() {
+                                        SmartDashboard.putData(
+                                        "Shooting State: Auto aim",
+                                        new SetShootingState(shootState, ShootState.AUTO_AIM));
+
                 if (Constants.DEBUG) {
                         SmartDashboard.putNumber(RunDashboardShotTest.ANGLER_TARGET_POSITION_KEY, 0.0);
                         SmartDashboard.putNumber(RunDashboardShotTest.SHOOTER_TARGET_RPM_KEY, Constants.SHOOTER_SPEED);

src/main/java/frc/robot/apriltags/SimApriltagIO.java

@@ -44,8 +44,8 @@ public void simReadings() {
                     if (cosIncidenceAngle!=0 && distance / cosIncidenceAngle < Constants.MAX_VISION_DISTANCE_SIMULATION) {
                         VisionMeasurement measurement = new VisionMeasurement(new Pose2d(), distance, 0, 0);
                         Vector<N3> stdDevs = truster.calculateTrust(measurement);
-                        double readingX = pose.getX() + random.nextGaussian() * stdDevs.get(0);
-                        double readingY = pose.getY() + random.nextGaussian() * stdDevs.get(1);
+                        double readingX = pose.getX(); //+ random.nextGaussian() * stdDevs.get(0);
+                        double readingY = pose.getY(); //+ random.nextGaussian() * stdDevs.get(1);
                         double readingYaw = pose.getRotation().getDegrees() + random.nextGaussian() * stdDevs.get(2);
                         Pose2d readingPos = new Pose2d(readingX, readingY, Rotation2d.fromDegrees(readingYaw));
                         distance = readingPos.getTranslation().getDistance(tag.getPose().toPose2d().getTranslation());

src/main/java/frc/robot/autochooser/AutoAction.java

@@ -8,6 +8,7 @@
 public enum AutoAction {
   DO_NOTHING("Do Nothing"),
   SHOOT("Shoot"),
+  FAST_SHOOT("Fast Shoot"),
   //SHOOT_PICKUP("Shoot and Pickup"),
   //DISTURBANCE("Disturbance"),
   //NEUTRAL_ZONE("Shoot and Neutral Zone Pickup"),