a

Author: Superbro525Alt

build.gradle

@@ -299,7 +299,7 @@ task(compileVisionNuitka) {
     description = "Compile vision runtime to a standalone binary with Nuitka."
     onlyIf { !visionSkipNuitka }
     doLast {
-        def pythonCmd = (project.findProperty("visionPython") ?: System.getenv("VISION_PYTHON") ?: "python").toString()
+        def pythonCmd = (project.findProperty("visionPython") ?: System.getenv("VISION_PYTHON") ?: "./.venv/Scripts/python.exe").toString()
         def nuitkaScript = (project.findProperty("visionNuitkaScript") ?: System.getenv("VISION_NUITKA_SCRIPT") ?: "vision/run.py").toString()
         def outputDir = layout.buildDirectory.dir("visionNuitka").get().asFile
         delete(outputDir)
@@ -317,6 +317,8 @@ task(compileVisionNuitka) {
             "--output-dir=${outputDir.absolutePath.replace("\\", "/")}",
             "--output-filename=vision_runtime",
             "--include-data-dir=${configDir}=vision/config",
+            "--nofollow-import-to=torch",
+            "--nofollow-import-to=torch.*",
             nuitkaScript
         ]
         if (!extraArgsRaw.isEmpty()) {

calibration.json

@@ -140,4 +140,4 @@
     "C:\\Users\\paulh\\2026-Rebuilt\\calib_images\\calib_0049.png"
   ],
   "rejected_files": []
-}
+}

requirements.txt

@@ -9,4 +9,6 @@ trimesh[easy]
 rtree
 opencv-python==4.13.0.92
 pyright
-opencv-contrib-python
+opencv-contrib-python
+matplotlib
+nuitka

src/main/java/org/curtinfrc/frc2026/Robot.java

@@ -64,6 +64,7 @@
 import org.curtinfrc.frc2026.util.Repulsor.Profiler.Profiler;
 import org.curtinfrc.frc2026.util.Repulsor.Reasoning.Rebuilt2026Reasoner;
 import org.curtinfrc.frc2026.util.Repulsor.Repulsor;
+import org.curtinfrc.frc2026.util.Repulsor.StaticInstance;
 import org.curtinfrc.frc2026.util.Repulsor.Setpoints.HeightSetpoint;
 import org.curtinfrc.frc2026.util.Repulsor.Setpoints.RepulsorSetpoint;
 import org.curtinfrc.frc2026.util.Repulsor.Setpoints.Setpoints;
@@ -164,6 +165,8 @@ void wireRepulsor() {
     repulsor.setReasoner(reasoner);
 
     repulsor.setup();
+
+    StaticInstance.initialize(repulsor);
   }
 
   public Robot() {
@@ -296,11 +299,11 @@ public Robot() {
       hoodedShooter = new HoodedShooter(new ShooterIO() {}, new HoodIO() {}, drive::getPose);
     }
 
-    // drive.setPose(
-    //     new Pose2d(
-    //         15.391 - (Constants.ROBOT_X / 2), 3.84 + (Constants.ROBOT_Y / 2), new Rotation2d()));
+    drive.setPose(
+        new Pose2d(
+            15.391 - (Constants.ROBOT_X / 2), 3.84 + (Constants.ROBOT_Y / 2), new Rotation2d()));
 
-    drive.setPose(new Pose2d(0, 0, new Rotation2d()));
+    // drive.setPose(new Pose2d(0, 0, new Rotation2d()));
 
     DriverStation.silenceJoystickConnectionWarning(true);
 

src/main/java/org/curtinfrc/frc2026/util/Repulsor/DriveRepulsor.java

@@ -19,9 +19,17 @@
 
 package org.curtinfrc.frc2026.util.Repulsor;
 
+import org.curtinfrc.frc2026.util.Repulsor.Fields.Rebuilt2026;
+import org.curtinfrc.frc2026.util.Repulsor.Fields.FieldMapBuilder.CategorySpec;
+import org.curtinfrc.frc2026.util.Repulsor.Setpoints.HeightSetpoint;
+import org.curtinfrc.frc2026.util.Repulsor.Setpoints.RepulsorSetpoint;
+import org.curtinfrc.frc2026.util.Repulsor.Setpoints.SetpointType;
+import org.curtinfrc.frc2026.util.Repulsor.Setpoints.StaticPoseSetpoint;
+
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public abstract class DriveRepulsor extends SubsystemBase {
@@ -30,4 +38,15 @@ public abstract class DriveRepulsor extends SubsystemBase {
   public abstract Pose2d getPose();
 
   public abstract PIDController getOmegaPID();
+
+  public Command alignTo(Pose2d targetPose) {
+    Repulsor re = StaticInstance.getInstance();
+
+    RepulsorSetpoint setpoint = new RepulsorSetpoint(new StaticPoseSetpoint("ALIGN_TARGET_" + targetPose.hashCode(), SetpointType.kOther, targetPose), HeightSetpoint.L1);
+
+    assert re != null : "Repulsor instance is not initialized";
+    assert re.isSameDrive(this) : "Repulsor instance does not match this DriveRepulsor";
+
+    return re.alignTo(setpoint, CategorySpec.kEndgame);
+  }
 }

src/main/java/org/curtinfrc/frc2026/util/Repulsor/Repulsor.java

@@ -83,6 +83,10 @@ public enum UsageType {
   private DriveRepulsor m_drive;
   private UsageType m_usageType = UsageType.kAutoDrive;
 
+  public boolean isSameDrive(DriveRepulsor other) {
+    return this.m_drive == other;
+  }
+
   private RepulsorSetpoint m_currentGoal =
       new RepulsorSetpoint(Setpoints.Rebuilt2026.CENTER_COLLECT, HeightSetpoint.NONE);
 
@@ -203,11 +207,7 @@ public Repulsor(
     SimMatchDriver.simInit(false);
   }
 
-  public Repulsor(
-      DriveRepulsor drive,
-      double robot_x,
-      double robot_y,
-      Supplier<Boolean> hasPiece) {
+  public Repulsor(DriveRepulsor drive, double robot_x, double robot_y, Supplier<Boolean> hasPiece) {
     this(drive, UsageType.kFullAuto, robot_x, robot_y, hasPiece);
   }
 
@@ -282,6 +282,10 @@ public void update() {
       enabled = ds.getConfigBool("force_controller_override");
     }
 
+    if (!m_usageType.equals(UsageType.kFullAuto)) {
+      return;
+    }
+
     if (enabled) {
       m_behaviourManager.stop();
       return;

src/main/java/org/curtinfrc/frc2026/util/Repulsor/Setpoints/Specific/_Rebuilt2026.java

@@ -49,6 +49,7 @@
 import org.curtinfrc.frc2026.util.Repulsor.Shooting.ShotLibrary;
 import org.curtinfrc.frc2026.util.Repulsor.Shooting.ShotLibraryBuilder;
 import org.curtinfrc.frc2026.util.Repulsor.Shooting.ShotSolution;
+import org.curtinfrc.frc2026.util.Repulsor.Setpoints.StaticPoseSetpoint;
 
 public class _Rebuilt2026 {
   protected _Rebuilt2026() {}
@@ -447,20 +448,6 @@ private static int obstaclesStableHash(List<? extends Obstacle> obs) {
     return (System.identityHashCode(obs) * 31) ^ obs.size();
   }
 
-  private static final class StaticPoseSetpoint extends GameSetpoint {
-    private final Pose2d bluePose;
-
-    StaticPoseSetpoint(String name, SetpointType type, Pose2d bluePose) {
-      super(name, type);
-      this.bluePose = bluePose == null ? Pose2d.kZero : bluePose;
-    }
-
-    @Override
-    public Pose2d bluePose(SetpointContext ctx) {
-      return bluePose;
-    }
-  }
-
   private static final class ApproachFromTagSetpoint extends GameSetpoint {
     private final int tagId;
     private final double standoffMeters;

src/main/java/org/curtinfrc/frc2026/util/Repulsor/Setpoints/StaticPoseSetpoint.java

@@ -0,0 +1,17 @@
+package org.curtinfrc.frc2026.util.Repulsor.Setpoints;
+
+import edu.wpi.first.math.geometry.Pose2d;
+
+public class StaticPoseSetpoint extends GameSetpoint {
+    private final Pose2d bluePose;
+
+    public StaticPoseSetpoint(String name, SetpointType type, Pose2d bluePose) {
+      super(name, type);
+      this.bluePose = bluePose == null ? Pose2d.kZero : bluePose;
+    }
+
+    @Override
+    public Pose2d bluePose(SetpointContext ctx) {
+      return bluePose;
+    }
+  }

src/main/java/org/curtinfrc/frc2026/util/Repulsor/StaticInstance.java

@@ -0,0 +1,28 @@
+package org.curtinfrc.frc2026.util.Repulsor;
+
+public class StaticInstance {
+    public static Repulsor repulsor;
+
+      public static Repulsor getInstance() {
+    Repulsor local = repulsor;
+    if (local == null) {
+      synchronized (Repulsor.class) {
+        local = repulsor;
+        if (local == null) {
+            throw new IllegalStateException("Repulsor instance has not been initialized. Please initialize it before calling getInstance().");
+        }
+      }
+    }
+    return local;
+  }
+
+  public static void initialize(Repulsor repulsorInstance) {
+    synchronized (Repulsor.class) {
+      if (repulsor == null) {
+        repulsor = repulsorInstance;
+      } else {
+        throw new IllegalStateException("Repulsor instance has already been initialized.");
+      }
+    }
+  }
+}

vision/config/runtime.json

@@ -31,41 +31,40 @@
         "roll_deg": 0.0
       },
       "calibration": {
-      "K": [
-        [
-          854.9288633679033,
-          0.0,
-          646.6629754587065
+        "K": [
+          [
+            854.9288633679033,
+            0.0,
+            646.6629754587065
+          ],
+          [
+            0.0,
+            853.5201517704674,
+            353.80122201918516
+          ],
+          [
+            0.0,
+            0.0,
+            1.0
+          ]
         ],
-        [
+        "dist": [
+          -2.8742702562830074,
+          -15.964065766291867,
+          -0.005370936357486865,
+          0.0008317338423974286,
+          66.33662248059912,
+          -3.101767730188295,
+          -14.269791592285904,
+          62.68118871650012,
           0.0,
-          853.5201517704674,
-          353.80122201918516
-        ],
-        [
           0.0,
           0.0,
-          1.0
+          0.0,
+          0.0,
+          0.0
         ]
-      ],
-      "dist": [
-        -2.8742702562830074,
-        -15.964065766291867,
-        -0.005370936357486865,
-        0.0008317338423974286,
-        66.33662248059912,
-        -3.101767730188295,
-        -14.269791592285904,
-        62.68118871650012,
-        0.0,
-        0.0,
-        0.0,
-        0.0,
-        0.0,
-        0.0
-      ]
-  
-  },
+      },
       "axis_cam_from_cv": [
         [
           0.0,