Merge pull request #6 from Team4388/MiraOrg

Mira org

elastic-layout.json

@@ -8,10 +8,10 @@
         "layouts": [
           {
             "title": "Tag Processed",
-            "x": 384.0,
+            "x": 896.0,
             "y": 0.0,
             "width": 256.0,
-            "height": 256.0,
+            "height": 384.0,
             "type": "List Layout",
             "properties": {
               "label_position": "TOP"
@@ -55,27 +55,11 @@
           }
         ],
         "containers": [
-          {
-            "title": "MatchTime",
-            "x": 0.0,
-            "y": 0.0,
-            "width": 384.0,
-            "height": 256.0,
-            "type": "Match Time",
-            "properties": {
-              "topic": "/AdvantageKit/DriverStation/MatchTime",
-              "period": 0.06,
-              "data_type": "double",
-              "time_display_mode": "Minutes and Seconds",
-              "red_start_time": 15,
-              "yellow_start_time": 30
-            }
-          },
           {
             "title": "RetractedLimit",
-            "x": 0.0,
+            "x": 512.0,
-            "y": 384.0,
+            "y": 0.0,
-            "width": 128.0,
+            "width": 256.0,
             "height": 128.0,
             "type": "Boolean Box",
             "properties": {
@@ -90,9 +74,9 @@
           },
           {
             "title": "Auto Chooser",
-            "x": 1024.0,
+            "x": 896.0,
-            "y": 256.0,
+            "y": 384.0,
-            "width": 384.0,
+            "width": 256.0,
             "height": 128.0,
             "type": "ComboBox Chooser",
             "properties": {
@@ -103,9 +87,9 @@
           },
           {
             "title": "Roller Percent Output",
-            "x": 0.0,
+            "x": 512.0,
             "y": 256.0,
-            "width": 256.0,
+            "width": 384.0,
             "height": 128.0,
             "type": "Number Slider",
             "properties": {
@@ -120,8 +104,8 @@
           },
           {
             "title": "Shooter OVERRIDE Velocity",
-            "x": 128.0,
+            "x": 768.0,
-            "y": 384.0,
+            "y": 0.0,
             "width": 128.0,
             "height": 128.0,
             "type": "Text Display",
@@ -134,8 +118,8 @@
           },
           {
             "title": "TRIM SHOOTER SPEED",
-            "x": 256.0,
+            "x": 512.0,
-            "y": 256.0,
+            "y": 384.0,
             "width": 384.0,
             "height": 128.0,
             "type": "Number Slider",
@@ -151,8 +135,8 @@
           },
           {
             "title": "Mode",
-            "x": 256.0,
+            "x": 512.0,
-            "y": 384.0,
+            "y": 128.0,
             "width": 384.0,
             "height": 128.0,
             "type": "Text Display",
@@ -164,30 +148,49 @@
             }
           },
           {
-            "title": "Time to rev",
+            "title": "IsActive",
-            "x": 768.0,
+            "x": 0.0,
-            "y": 128.0,
+            "y": 0.0,
-            "width": 256.0,
+            "width": 384.0,
-            "height": 128.0,
+            "height": 256.0,
-            "type": "Large Text Display",
+            "type": "Boolean Box",
             "properties": {
-              "topic": "/AdvantageKit/RealOutputs/Time to rev",
+              "topic": "/AdvantageKit/RealOutputs/HubShift/IsActive",
               "period": 0.06,
-              "data_type": "double"
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
             }
           },
           {
-            "title": "Arm angle extended",
+            "title": "RemainingInShift",
-            "x": 1152.0,
+            "x": 0.0,
-            "y": 128.0,
+            "y": 256.0,
-            "width": 256.0,
+            "width": 384.0,
             "height": 128.0,
-            "type": "Text Display",
+            "type": "Match Time",
             "properties": {
-              "topic": "/SmartDashboard/Arm angle extended",
+              "topic": "/AdvantageKit/RealOutputs/HubShift/RemainingInShift",
               "period": 0.06,
               "data_type": "double",
-              "show_submit_button": true
+              "time_display_mode": "Minutes and Seconds",
+              "red_start_time": 15,
+              "yellow_start_time": 30
+            }
+          },
+          {
+            "title": "Phase",
+            "x": 0.0,
+            "y": 384.0,
+            "width": 384.0,
+            "height": 128.0,
+            "type": "Large Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/HubShift/Phase",
+              "period": 0.06,
+              "data_type": "string"
             }
           }
         ]
@@ -527,8 +530,8 @@
           },
           {
             "title": "Shooter Idle max current",
-            "x": 384.0,
+            "x": 1024.0,
-            "y": 512.0,
+            "y": 384.0,
             "width": 256.0,
             "height": 128.0,
             "type": "Text Display",
@@ -555,8 +558,8 @@
           },
           {
             "title": "Shooter OVERRIDE Velocity",
-            "x": 640.0,
+            "x": 1024.0,
-            "y": 512.0,
+            "y": 256.0,
             "width": 256.0,
             "height": 128.0,
             "type": "Text Display",
@@ -575,12 +578,40 @@
       "grid_layout": {
         "layouts": [],
         "containers": [
+          {
+            "title": "Stalled Motor: ",
+            "x": 512.0,
+            "y": 128.0,
+            "width": 384.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/Stalled Motor: ",
+              "period": 0.06,
+              "data_type": "string",
+              "show_submit_button": false
+            }
+          },
+          {
+            "title": "Shooter idle % output",
+            "x": 128.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Text Display",
+            "properties": {
+              "topic": "/SmartDashboard/Shooter idle % output",
+              "period": 0.06,
+              "data_type": "double",
+              "show_submit_button": true
+            }
+          },
           {
             "title": "Auto Chooser",
-            "x": 0.0,
+            "x": 512.0,
-            "y": 0.0,
+            "y": 256.0,
-            "width": 640.0,
+            "width": 384.0,
-            "height": 768.0,
+            "height": 128.0,
             "type": "ComboBox Chooser",
             "properties": {
               "topic": "/SmartDashboard/Auto Chooser",

src/main/deploy/pathplanner/autos/DriveShoot-Hori.auto

@@ -0,0 +1,44 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Robot Rev Up"
+          }
+        },
+        {
+          "type": "parallel",
+          "data": {
+            "commands": [
+              {
+                "type": "named",
+                "data": {
+                  "name": "Robot Shoot"
+                }
+              },
+              {
+                "type": "path",
+                "data": {
+                  "pathName": "Shoot driving across"
+                }
+              },
+              {
+                "type": "named",
+                "data": {
+                  "name": "Robot Shoot Driving"
+                }
+              }
+            ]
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/autos/ShootPreloadBump.auto

@@ -0,0 +1,43 @@
+{
+  "version": "2025.0",
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "Intake Extended"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "Bump test"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "BumpOffsetForward"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "BumpToCenter"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "HubFarLeft-NeutralZone 2-2"
+          }
+        }
+      ]
+    }
+  },
+  "resetOdom": true,
+  "folder": null,
+  "choreoAuto": false
+}

src/main/deploy/pathplanner/paths/Bump test.path

@@ -0,0 +1,75 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 3.4908076923076936,
+        "y": 6.633345195729537
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.3977564102564113,
+        "y": 6.0868195547038955
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 3.4908076923076936,
+        "y": 5.6745769230769225
+      },
+      "prevControl": {
+        "x": 2.6330609248104158,
+        "y": 5.743196664476699
+      },
+      "nextControl": {
+        "x": 3.7815128205128214,
+        "y": 5.651320512820514
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 5.967615384615385,
+        "y": 5.709461538461539
+      },
+      "prevControl": {
+        "x": 4.746653846153847,
+        "y": 5.430384615384616
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [
+    {
+      "waypointRelativePos": 1.5211640211640187,
+      "rotationDegrees": -112.5
+    }
+  ],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 10.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -91.24536426676825
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 179.59775645089275
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/BumpToCenter.path

@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 5.935255041518388,
+        "y": 5.6650177935943065
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 6.073930090574693,
+        "y": 5.873030367178768
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 7.559893238434164,
+        "y": 7.547876631079477
+      },
+      "prevControl": {
+        "x": 7.1940806642941855,
+        "y": 7.182064056939502
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 3.0,
+    "maxAcceleration": 3.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 10.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": -90.98776039963968
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": -91.19348942398209
+  },
+  "useDefaultConstraints": true
+}

src/main/deploy/pathplanner/paths/CPlayerStation-CShoot.path

@@ -58,7 +58,7 @@
     "maxAcceleration": 3.0,
     "maxAngularVelocity": 600.0,
     "maxAngularAcceleration": 750.0,
-    "nominalVoltage": 12.0,
+    "nominalVoltage": 10.0,
     "unlimited": false
   },
   "goalEndState": {

src/main/deploy/pathplanner/paths/HubCenter-PlayerStation.path

@@ -104,7 +104,7 @@
     "maxAcceleration": 3.0,
     "maxAngularVelocity": 600.0,
     "maxAngularAcceleration": 750.0,
-    "nominalVoltage": 12.0,
+    "nominalVoltage": 10.0,
     "unlimited": false
   },
   "goalEndState": {

src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 1-2.path

@@ -48,7 +48,7 @@
     "maxAcceleration": 3.0,
     "maxAngularVelocity": 600.0,
     "maxAngularAcceleration": 750.0,
-    "nominalVoltage": 12.0,
+    "nominalVoltage": 10.0,
     "unlimited": false
   },
   "goalEndState": {

src/main/deploy/pathplanner/paths/HubFarLeft-NeutralZone 2-2.path

@@ -88,7 +88,7 @@
     "maxAcceleration": 3.0,
     "maxAngularVelocity": 600.0,
     "maxAngularAcceleration": 750.0,
-    "nominalVoltage": 12.0,
+    "nominalVoltage": 10.0,
     "unlimited": false
   },
   "goalEndState": {

src/main/deploy/pathplanner/paths/HubFarRight-NeutralZone 2-2.path

@@ -88,7 +88,7 @@
     "maxAcceleration": 3.0,
     "maxAngularVelocity": 600.0,
     "maxAngularAcceleration": 750.0,
-    "nominalVoltage": 12.0,
+    "nominalVoltage": 10.0,
     "unlimited": false
   },
   "goalEndState": {

src/main/deploy/pathplanner/paths/HubRight-PlayerStation.path

@@ -64,7 +64,7 @@
     "maxAcceleration": 3.0,
     "maxAngularVelocity": 600.0,
     "maxAngularAcceleration": 750.0,
-    "nominalVoltage": 12.0,
+    "nominalVoltage": 10.0,
     "unlimited": false
   },
   "goalEndState": {

src/main/deploy/pathplanner/paths/PlayerStation-Shoot.path

@@ -58,7 +58,7 @@
     "maxAcceleration": 3.0,
     "maxAngularVelocity": 600.0,
     "maxAngularAcceleration": 750.0,
-    "nominalVoltage": 12.0,
+    "nominalVoltage": 10.0,
     "unlimited": false
   },
   "goalEndState": {

src/main/deploy/pathplanner/paths/ReadyPush.path

@@ -48,7 +48,7 @@
     "maxAcceleration": 3.0,
     "maxAngularVelocity": 600.0,
     "maxAngularAcceleration": 750.0,
-    "nominalVoltage": 12.0,
+    "nominalVoltage": 10.0,
     "unlimited": false
   },
   "goalEndState": {

src/main/deploy/pathplanner/paths/Shoot driving across.path

@@ -0,0 +1,102 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.9326282051282067,
+        "y": 6.5815769230769225
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 1.9932621113872893,
+        "y": 6.3390412980405895
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.9326282051282067,
+        "y": 1.6628461538461545
+      },
+      "prevControl": {
+        "x": 1.936134248900721,
+        "y": 1.9128215679513527
+      },
+      "nextControl": {
+        "x": 1.9291221613556924,
+        "y": 1.4128707397409563
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.9326282051282067,
+        "y": 6.5815769230769225
+      },
+      "prevControl": {
+        "x": 1.9198835421653975,
+        "y": 6.331901987278464
+      },
+      "nextControl": {
+        "x": 1.9453728680910158,
+        "y": 6.8312518588753814
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.9326282051282067,
+        "y": 1.6628461538461545
+      },
+      "prevControl": {
+        "x": 1.9337461053582308,
+        "y": 1.912843654431812
+      },
+      "nextControl": {
+        "x": 1.9315103048981825,
+        "y": 1.4128486532604971
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.9326282051282067,
+        "y": 6.5815769230769225
+      },
+      "prevControl": {
+        "x": 1.8878500514131944,
+        "y": 6.335619778537727
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 0.8,
+    "maxAcceleration": 10.0,
+    "maxAngularVelocity": 600.0,
+    "maxAngularAcceleration": 750.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 180.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0.0,
+    "rotation": 180.0
+  },
+  "useDefaultConstraints": false
+}

src/main/java/frc4388/robot/Robot.java

@@ -15,11 +15,14 @@ import org.littletonrobotics.junction.networktables.NT4Publisher;
 import org.littletonrobotics.junction.wpilog.WPILOGReader;
 import org.littletonrobotics.junction.wpilog.WPILOGWriter;
 
+import edu.wpi.first.wpilibj.GenericHID.RumbleType;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc4388.robot.constants.BuildConstants;
 import frc4388.robot.constants.Constants.SimConstants;
 import frc4388.utility.DeferredBlock;
+import frc4388.utility.compute.HubShiftTimer;
+import frc4388.utility.compute.HubShiftTimer.ShiftInfo;
 import frc4388.utility.compute.RobotTime;
 import frc4388.utility.compute.Trim;
 import frc4388.utility.status.FaultReporter;
@@ -113,6 +116,7 @@ public class Robot extends LoggedRobot {
       m_autonomousCommand.schedule();
     }
     m_robotTime.startMatchTime();
+    HubShiftTimer.initializeAuto();
   }
 
   /**
@@ -138,6 +142,7 @@ public class Robot extends LoggedRobot {
 
     }
     m_robotTime.startMatchTime();
+    HubShiftTimer.initializeTeleop();
   }
 
   /**
@@ -145,7 +150,12 @@ public class Robot extends LoggedRobot {
    */
   @Override
   public void teleopPeriodic() {
-  //  m_robotContainer.m_robotMap.rightFront.go(m_robotContainer.getDeadbandedDriverController().getLeft());
+    var info = HubShiftTimer.getShiftInfo();
+
+    double rumble = (info.remainingInShift() < 5.  && info.remainingInShift() > 0.1) ? 1 : 0;
+
+    // m_robotContainer.getDeadbandedDriverController().setRumble(RumbleType.kBothRumble, rumble);
+    // m_robotContainer.getDeadbandedOperatorController().setRumble(RumbleType.kBothRumble, rumble);
   }
 
   /**

src/main/java/frc4388/robot/RobotContainer.java

@@ -5,9 +5,14 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
 package frc4388.robot;
 
 import java.io.File;
+import java.util.Optional;
 
 import com.pathplanner.lib.auto.NamedCommands;
 import com.pathplanner.lib.commands.PathPlannerAuto;
@@ -24,24 +29,27 @@ import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 // Commands
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import edu.wpi.first.wpilibj2.command.WaitUntilCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
+import frc4388.robot.commands.Swerve.StayInPosition;
 import frc4388.robot.commands.alignment.AutoAlign;
 import frc4388.robot.constants.Constants;
+import frc4388.robot.constants.FieldConstants;
 import frc4388.robot.constants.Constants.OIConstants;
 import frc4388.robot.constants.Constants.SimConstants.Mode;
-// Subsystems
-import frc4388.robot.subsystems.LED;
-import frc4388.robot.subsystems.Lidar;
 import frc4388.robot.subsystems.intake.Intake;
 import frc4388.robot.subsystems.intake.Intake.IntakeMode;
+import frc4388.robot.subsystems.led.LED;
 import frc4388.robot.subsystems.shooter.Shooter;
 import frc4388.robot.subsystems.shooter.ShooterConstants;
+import frc4388.robot.subsystems.swerve.SimpleSwerveSim;
 import frc4388.robot.subsystems.swerve.SwerveDrive;
+import frc4388.robot.subsystems.vision.Lidar;
 import frc4388.robot.subsystems.vision.Vision;
 import frc4388.utility.DeferredBlock;
 import frc4388.utility.compute.FieldPositions;
@@ -61,7 +69,7 @@ import frc4388.utility.controller.XboxController;
 public class RobotContainer {
     /* RobotMap */
     
-    public final RobotMap m_robotMap = new RobotMap(Mode.REAL);
+    public final RobotMap m_robotMap = new RobotMap(RobotBase.isReal() ? Mode.REAL : Mode.SIM);
     
     /*Limit Switch */
     // public final DigitalInput m_armLimitSwitch = new DigitalInput(9);
@@ -69,6 +77,7 @@ public class RobotContainer {
     /* Subsystems */
     // public final Lidar m_lidar = new Lidar();
     public final LED m_robotLED = new LED(Constants.LEDConstants.LED_SPARK_ID);
+    public final SimpleSwerveSim m_robotSwerveSIM = new SimpleSwerveSim();
     //Testing of Colors
     public final Vision m_vision = new Vision(m_robotMap.rightCamera, m_robotMap.leftCamera);
     public final SwerveDrive m_robotSwerveDrive = new SwerveDrive(m_robotMap.swerveDrivetrain, m_vision);
@@ -84,156 +93,212 @@ public class RobotContainer {
     // private final ButtonBox m_buttonBox = new ButtonBox(OIConstants.BUTTONBOX_ID);
 
     // public List<Subsystem> subsystems = new ArrayList<>();
-
+    private final StayInPosition m_stayInPosition = new StayInPosition(m_robotSwerveDrive);
-    // ! Teleop Commands
-    public void stop() {
-        new InstantCommand(()->{}, m_robotSwerveDrive).schedule();
-        m_robotSwerveDrive.stopModules();
-        Constants.AutoConstants.Y_OFFSET_TRIM.set(0);
-    }
-
-    // ! /*  Autos */
-    private SendableChooser<String> autoChooser;
-    private Command autoCommand;
-
-
-    private Command IntakeExtended = new SequentialCommandGroup(
-        new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Extended), m_robotIntake)
-    );
-
-    // private Command LidarIntake = new SequentialCommandGroup(
-    //     // Right now this will just go to the closest ball constantly updating - need to make it so it locks on one ball
-    //     // RobotIntakeDown,
-    //     // new WaitCommand(1),
-    //     // new InstantCommand(() -> System.out.println("Closest Ball: " + m_lidar.getClosestBall())),
-    //     new AutoAlign(m_robotSwerveDrive, m_vision, m_lidar, true)
-
-    //     // new RunCommand(
-    //     // () -> m_robotSwerveDrive.driveWithInput(
-    //     //         m_lidar.getClosestBall(),
-    //     //         new Translation2d(m_lidar.getLatestBallAngle().getCos() * 0.1, 0),
-    //     //         false
-    //     //     ),
-    //     //     m_robotSwerveDrive
-    //     // )
-    //     // .withTimeout(5.0)
-    //     // .andThen(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive))
-    // );
-
-    private Command RobotRev = new SequentialCommandGroup(
-        new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter),
-        IntakeExtended,
-        new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RollerStop), m_robotIntake)
-    );
-
-    private Command IntakeRetracted = new SequentialCommandGroup(
-        new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted), m_robotIntake)
-    );
-
-    private Command RobotShoot = new SequentialCommandGroup(
-        // TEST NEW AUTO ALIGN
-        //new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(FieldPositions.HUB_POSITION,  new Rotation2d(0)), false),
-        new WaitUntilCommand(m_robotShooter::isShooterUpToSpeed),
-        new InstantCommand(()-> m_robotShooter.allowShooting(), m_robotShooter),
-        new WaitCommand(2),
-        IntakeRetracted,
-        new WaitCommand(5),
-        new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter),
-        new InstantCommand(()->m_robotShooter.spinUpIdle(), m_robotShooter)
-    );
-
     
-    public RobotContainer() {
+    private Pose2d currentPose = new Pose2d(0, 0, new Rotation2d());
-        
+        // ! Teleop Commands
-        configureButtonBindings();
+        public void stop() {
-        
+            new InstantCommand(()->{}, m_robotSwerveDrive).schedule();
-        // Called on first robot enable
+            m_robotSwerveDrive.stopModules();
-        DeferredBlock.addBlock(() -> {
+            Constants.AutoConstants.Y_OFFSET_TRIM.set(0);
-            m_robotSwerveDrive.resetGyro();
+        }
-        }, false);
-
-        // Called on every robot enable
-        DeferredBlock.addBlock(() -> {
-            TimesNegativeOne.update();
-            FieldPositions.update();
-
-            m_robotIntake.io.updateGains();
-            m_robotShooter.io.updateGains();
-        }, true);
-
-        NamedCommands.registerCommand("Robot Rev Up", RobotRev);
-        NamedCommands.registerCommand("Intake Retracted", IntakeRetracted);
-        NamedCommands.registerCommand("Robot Shoot", RobotShoot);
-        // NamedCommands.registerCommand("Lidar Intake", LidarIntake);
-        NamedCommands.registerCommand("Intake Extended", IntakeExtended);
-
-
-        DriverStation.silenceJoystickConnectionWarning(true);
-
-        // Drive normally
-        m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
-            m_robotSwerveDrive.driveWithInput(
-                getDeadbandedDriverController().getLeft(),
-                getDeadbandedDriverController().getRight(),true);
-
-        }, m_robotSwerveDrive)
-        .withName("SwerveDrive DefaultCommand"));
-        
-        m_robotSwerveDrive.setToSlow();
-        
-        makeAutoChooser();
-        SmartDashboard.putData("Auto Chooser", autoChooser);
-
-    }
     
+        // ! /*  Autos */
+        private SendableChooser<String> autoChooser;
+        private Command autoCommand;
+    
+    
+        private Command IntakeExtended = new SequentialCommandGroup(
+            new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Extended), m_robotIntake)
+        );
+    
+        // private Command LidarIntake = new SequentialCommandGroup(
+        //     // Right now this will just go to the closest ball constantly updating - need to make it so it locks on one ball
+        //     // RobotIntakeDown,
+        //     // new WaitCommand(1),
+        //     // new InstantCommand(() -> System.out.println("Closest Ball: " + m_lidar.getClosestBall())),
+        //     new AutoAlign(m_robotSwerveDrive, m_vision, m_lidar, true)
+    
+        //     // new RunCommand(
+        //     // () -> m_robotSwerveDrive.driveWithInput(
+        //     //         m_lidar.getClosestBall(),
+        //     //         new Translation2d(m_lidar.getLatestBallAngle().getCos() * 0.1, 0),
+        //     //         false
+        //     //     ),
+        //     //     m_robotSwerveDrive
+        //     // )
+        //     // .withTimeout(5.0)
+        //     // .andThen(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive))
+        // );
+    
+        private Command RobotRev = new SequentialCommandGroup(
+            new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter),
+            IntakeExtended,
+            new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.RollerStop), m_robotIntake)
+        );
 
-
+        private Command RobotShootDriving = new SequentialCommandGroup(
-
+            new RunCommand(() -> 
-    /**
+                m_robotSwerveDrive.enableRotationOverride(FieldPositions.HUB_POSITION, ShooterConstants.AIM_LEAD_TIME.get(), FieldPositions.HUB_POSITION)
-     * Use this method to define your button->command mappings. Buttons can be
+            ).withTimeout(20)
-     * created by instantiating a {@link GenericHID} or one of its subclasses
+            ).finallyDo((interrupted) -> 
-     * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
+             m_robotSwerveDrive.disableRotationOverride()
-     * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+        );
-     */
+    
-    private void configureButtonBindings() {
+        private Command IntakeRetracted = new SequentialCommandGroup(
-        
+            new InstantCommand(() -> m_robotIntake.setMode(IntakeMode.Retracted), m_robotIntake)
-        //Driver controls
+        );
-        new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
+    
-            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
+        private Command RobotShoot = new SequentialCommandGroup(
-
+            // TEST NEW AUTO ALIGN
-        new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
+            //new AutoAlign(m_robotSwerveDrive, m_vision, new Pose2d(FieldPositions.HUB_POSITION,  new Rotation2d(0)), false),
-            .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.shiftUp()));
+            new WaitUntilCommand(m_robotShooter::isShooterUpToSpeed),
-        
+            new InstantCommand(()-> m_robotShooter.allowShooting(), m_robotShooter),
-        new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
+            new WaitCommand(5),
-            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
+            IntakeRetracted,
-
+            new WaitCommand(10),
+            new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter),
+            new InstantCommand(()->m_robotShooter.spinUpIdle(), m_robotShooter)
+        );
     
         
-        new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON)
+        public RobotContainer() {
-            .onTrue(new InstantCommand(()  -> {
+            
+            configureSINGLEBindings();
+            
+            // Called on first robot enable
+            DeferredBlock.addBlock(() -> {
+                m_robotSwerveDrive.resetGyro();
+            }, false);
+    
+            // Called on every robot enable
+            DeferredBlock.addBlock(() -> {
+                TimesNegativeOne.update();
+                FieldPositions.update();
+    
                 m_robotIntake.io.updateGains();
                 m_robotShooter.io.updateGains();
+            }, true);
+    
+            NamedCommands.registerCommand("Robot Rev Up", RobotRev);
+            NamedCommands.registerCommand("Intake Retracted", IntakeRetracted);
+            NamedCommands.registerCommand("Robot Shoot", RobotShoot);
+            // NamedCommands.registerCommand("Lidar Intake", LidarIntake);
+            NamedCommands.registerCommand("Intake Extended", IntakeExtended);
+            NamedCommands.registerCommand("Robot Shoot Driving", RobotShootDriving);
+
+            NamedCommands.registerCommand("WaitShooter", new WaitUntilCommand(m_robotShooter::isShooterUpToSpeed));
+            NamedCommands.registerCommand("AllowShooting", new InstantCommand(() -> m_robotShooter.allowShooting(), m_robotShooter));
+            NamedCommands.registerCommand("DenyShooting", new InstantCommand(() -> m_robotShooter.denyShooting(), m_robotShooter));
+            NamedCommands.registerCommand("SpinUpShooting", new InstantCommand(() -> m_robotShooter.spinUpShooting(), m_robotShooter));
+            NamedCommands.registerCommand("SpinUpIdle", new InstantCommand(() -> m_robotShooter.spinUpIdle(), m_robotShooter));
+    
+            NamedCommands.registerCommand("BumpOffsetForward", new InstantCommand(() -> {
+                if (TimesNegativeOne.isRed) {
+                    m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_RED);
+                } else {
+                    m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_BLUE);
+                }
+            }));
+
+            NamedCommands.registerCommand("BumpOffsetReverse", new InstantCommand(() -> {
+                if (!TimesNegativeOne.isRed) {
+                    m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_RED);
+                } else {
+                    m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_BLUE);
+                }
+            }));
+
+    
+            DriverStation.silenceJoystickConnectionWarning(true);
+    
+            // Drive normally
+            m_robotSwerveDrive.setDefaultCommand(new RunCommand(() -> {
+                m_robotSwerveDrive.driveWithInput(
+                    getDeadbandedDriverController().getLeft(),
+                    getDeadbandedDriverController().getRight(),true);
+    
+            }, m_robotSwerveDrive)
+            .withName("SwerveDrive DefaultCommand"));
+            
+            m_robotSwerveDrive.setToSlow();
+            
+            makeAutoChooser();
+            SmartDashboard.putData("Auto Chooser", autoChooser);
+    
+        }
+        
+    
+    
+    
+        /**
+         * Use this method to define your button->command mappings. Buttons can be
+         * created by instantiating a {@link GenericHID} or one of its subclasses
+         * ({@link edu.wpi.first.wpilibj.Joystick} or {@link XboxController}), and then
+         * passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
+         */
+        private void configureButtonBindings() {
+            
+            //Driver controls
+            new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
+                .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
+    
+            new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
+                .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.shiftUp()));
+            
+            new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
+                .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
+    
+        
+            
+            new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON)
+                .onTrue(new InstantCommand(()  -> {
+                    m_robotIntake.io.updateGains();
+                    m_robotShooter.io.updateGains();
+                }));
+    
+                
+            // TEST-> the driver is holding the left trigger, drive slow and rotation up 
+            new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5)
+                .onTrue(new InstantCommand(() -> {
+                    m_robotSwerveDrive.setToSlow();
+                    m_robotSwerveDrive.shiftUpRot();
+                }))
+                .onFalse(new InstantCommand(() -> {
+                    m_robotSwerveDrive.setToFast();
+                    m_robotSwerveDrive.shiftDownRot();
+            }));
+    
+            //TEST - > X positino on wheels
+            new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
+                .whileTrue(new RunCommand(() -> {
+                m_robotSwerveDrive.defenseXPosition();
+            }, m_robotSwerveDrive))
+                .onFalse(new InstantCommand(() -> {
+                m_robotSwerveDrive.stopDefenseXPosition();
+            }));
+
+            //TEST - > PID positinon
+            new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
+            .onTrue(new InstantCommand(() -> {
+                    currentPose = m_robotSwerveDrive.getCurrentPose();
+            }))
+            .whileTrue(new RunCommand(() -> {
+                m_stayInPosition.goToTargetPose(currentPose);
+            }, m_robotSwerveDrive))
+            .onFalse(new InstantCommand(() -> {
+                m_robotSwerveDrive.softStop();
+                
             }));
 
             
-        // IF the driver is holding the left trigger, intake driving
-        new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5)
-            .whileTrue(new RunCommand(
-                () -> {
-                    m_robotSwerveDrive.driveIntakeOrientation(
-                        getDeadbandedDriverController().getLeft(),
-                        getDeadbandedDriverController().getRight()
-                        
-                    );
-                }, m_robotSwerveDrive))
-            .onFalse(new InstantCommand(() -> {
-                m_robotSwerveDrive.softStop();
-            }));
 
         // IF the driver is holding the aim button, aim the robot towards the hub and shooter ready
         new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5)
-            .whileTrue(new RunCommand(
+            .onTrue(new InstantCommand(() -> {
-                () -> {
+                    m_robotSwerveDrive.setToSlow();
+            }))
+            .whileTrue(new RunCommand(() -> {
                 m_robotSwerveDrive.driveFacingPosition(
                     getDeadbandedDriverController().getLeft(),
                     FieldPositions.HUB_POSITION,
@@ -326,6 +391,126 @@ public class RobotContainer {
 
    }
 
+
+   private void configureSINGLEBindings() {
+            
+            //Driver controls
+            // new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
+            //     .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro()));
+                
+            new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
+                .onTrue(new InstantCommand(() -> m_robotSwerveDrive.offsetOdoPosition(FieldConstants.BUMP_OFFSET_RED)));
+    
+            new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
+                .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.shiftUp()));
+            
+            new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
+                .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
+    
+    
+    
+                
+            // TEST-> the driver is holding the left trigger, drive slow and rotation up 
+            // new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5)
+            //     .onTrue(new InstantCommand(() -> {
+            //         m_robotSwerveDrive.setToSlow();
+            //         m_robotSwerveDrive.shiftUpRot();
+            //     }))
+            //     .onFalse(new InstantCommand(() -> {
+            //         m_robotSwerveDrive.setToFast();
+            //         m_robotSwerveDrive.shiftDownRot();
+            // }));
+    
+            //TEST - > X positino on wheels
+            new JoystickButton(getDeadbandedDriverController(), XboxController.BACK_BUTTON)
+                .whileTrue(new RunCommand(() -> {
+                m_robotSwerveDrive.defenseXPosition();
+            }, m_robotSwerveDrive))
+                .onFalse(new InstantCommand(() -> {
+                m_robotSwerveDrive.stopDefenseXPosition();
+            }));
+
+            //TEST - > PID positinon
+            new JoystickButton(getDeadbandedDriverController(), XboxController.B_BUTTON)
+            .onTrue(new InstantCommand(() -> {
+                    currentPose = m_robotSwerveDrive.getCurrentPose();
+            }))
+            .whileTrue(new RunCommand(() -> {
+                m_stayInPosition.goToTargetPose(currentPose);
+            }, m_robotSwerveDrive))
+            .onFalse(new InstantCommand(() -> {
+                m_robotSwerveDrive.softStop();
+                
+            }));
+
+            
+
+        // IF the driver is holding the aim button, aim the robot towards the hub and shooter ready
+        new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5)
+            .onTrue(new InstantCommand(() -> {
+                    m_robotSwerveDrive.setToSlow();
+            }))
+            .whileTrue(new RunCommand(() -> {
+                m_robotSwerveDrive.driveFacingPosition(
+                    getDeadbandedDriverController().getLeft(),
+                    FieldPositions.HUB_POSITION,
+                    ShooterConstants.AIM_LEAD_TIME.get()
+                    );
+                }, m_robotSwerveDrive)
+            );
+
+        // D-PAD fine alignment
+        new Trigger(() -> getDeadbandedDriverController().getPOV() != -1)
+            .whileTrue(new RunCommand(
+                () -> m_robotSwerveDrive.driveFine(
+                    new Translation2d(
+                        1, 
+                        Rotation2d.fromDegrees(getDeadbandedDriverController().getPOV())
+                    ), 
+                    getDeadbandedDriverController().getRight(), 0.15
+                ), m_robotSwerveDrive))
+            .onFalse(new InstantCommand(() -> m_robotSwerveDrive.softStop(), m_robotSwerveDrive));
+
+
+        //allow shooting with right trigger
+        new Trigger(() -> getDeadbandedDriverController().getRightTriggerAxis() >= 0.5)
+            .onTrue(new InstantCommand(() -> {
+                m_robotShooter.allowShooting();
+            })).onFalse(new InstantCommand(() -> {
+                m_robotShooter.denyShooting();
+            }));
+        
+
+
+        //set shooter ready (rev) with left trigger hold
+        new Trigger(() -> getDeadbandedDriverController().getLeftTriggerAxis() >= 0.5)
+            .onTrue(new InstantCommand(() -> {
+                m_robotIntake.setMode(IntakeMode.Idle);
+                m_robotIntake.rollerStop();
+                m_robotShooter.spinUpShooting();
+            }))
+            .onFalse(new InstantCommand(() -> {
+                m_robotShooter.spinUpIdle();
+            }));
+
+        new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
+            .onTrue(new InstantCommand(() -> {
+                m_robotIntake.setMode(IntakeMode.Extending);
+            }))
+            .onFalse(new InstantCommand(() -> {
+                m_robotIntake.setMode(IntakeMode.Idle);
+            }));
+
+        new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
+            .onTrue(new InstantCommand(() -> {
+                m_robotIntake.setMode(IntakeMode.Retracting);
+            }))
+            .onFalse(new InstantCommand(() -> {
+                m_robotIntake.setMode(IntakeMode.Idle);
+            }));
+
+        }
+
 //.onTrue(new InstantCommand(()  -> m_robotLED.setMode(LEDPatterns.SOLID_PINK_HOT)));
 
     /**

src/main/java/frc4388/robot/RobotMap.java

@@ -25,6 +25,7 @@ import frc4388.robot.subsystems.intake.IntakeReal;
 import frc4388.robot.subsystems.shooter.ShooterConstants;
 import frc4388.robot.subsystems.shooter.ShooterIO;
 import frc4388.robot.subsystems.shooter.ShooterReal;
+import frc4388.robot.subsystems.swerve.SimpleSwerveSim;
 // import frc4388.robot.subsystems.elevator.ElevatorIO;
 // import frc4388.robot.subsystems.elevator.ElevatorReal;
 import frc4388.robot.subsystems.swerve.SwerveDriveConstants;
@@ -133,8 +134,15 @@ public class RobotMap {
                 FaultCANCoder.addDevice(swerveDrivetrainReal.getModule(3).getEncoder(), "Module 3 CANCoder");
 
                 break;
-            // case SIM:
+            case SIM:
-            //     break;
+                leftCamera = new VisionIO() {};
+                rightCamera = new VisionIO() {};
+
+                swerveDrivetrain = new SimpleSwerveSim() {};
+
+                shooterIO = new ShooterIO() {};
+                intakeIO = new IntakeIO() {};
+                break;
             default:
                 leftCamera = new VisionIO() {};
                 rightCamera = new VisionIO() {};

src/main/java/frc4388/robot/commands/MoveForTimeCommand.java

@@ -1,49 +0,0 @@
-package frc4388.robot.commands;
-
-import java.time.Instant;
-
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc4388.robot.subsystems.swerve.SwerveDrive;
-
-// Command to repeat a joystick movement for a specific time.
-public class MoveForTimeCommand extends Command {
-    private final SwerveDrive swerveDrive;
-    private final Translation2d leftStick;
-    private final Translation2d rightStick;
-    private final long duration;
-    private final boolean robotRelative;
-
-    private Instant startTime;
-
-    public MoveForTimeCommand(
-        SwerveDrive swerveDrive, 
-        Translation2d leftStick, 
-        Translation2d rightStick, 
-        long millis,
-        boolean robotRelative) {
-        
-        addRequirements(swerveDrive);
-
-        this.swerveDrive = swerveDrive;
-        this.leftStick = leftStick;
-        this.rightStick = rightStick;
-        this.duration = millis;
-        this.robotRelative = robotRelative;
-    }
-
-    @Override
-    public void initialize() {
-        startTime = Instant.now();
-    }
-
-    @Override
-    public void execute() {
-        swerveDrive.driveWithInput(leftStick, rightStick, !robotRelative);
-    }
-
-    @Override
-    public boolean isFinished() {
-        return Math.abs(startTime.toEpochMilli() - Instant.now().toEpochMilli()) > duration;
-    }
-}

src/main/java/frc4388/robot/commands/MoveUntilSuply.java

@@ -1,45 +0,0 @@
-package frc4388.robot.commands;
-
-import java.util.function.Supplier;
-
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc4388.robot.subsystems.swerve.SwerveDrive;
-
-// Command to repeat a joystick movement for a specific time.
-public class MoveUntilSuply extends Command {
-    private final SwerveDrive swerveDrive;
-    private final Translation2d leftStick;
-    private final Translation2d rightStick;
-    private final Supplier<Boolean> truth;
-    private final boolean robotRelative;
-
-    public MoveUntilSuply(
-        SwerveDrive swerveDrive, 
-        Translation2d leftStick, 
-        Translation2d rightStick, 
-        Supplier<Boolean> truth,
-        boolean robotRelative) {
-            addRequirements(swerveDrive);
-
-        this.swerveDrive = swerveDrive;
-        this.leftStick = leftStick;
-        this.rightStick = rightStick;
-        this.truth = truth;
-        this.robotRelative = robotRelative;
-    }
-
-    @Override
-    public void initialize() {
-    }
-
-    @Override
-    public void execute() {
-        swerveDrive.driveWithInput(leftStick, rightStick, !robotRelative);
-    }
-
-    @Override
-    public boolean isFinished() {
-        return truth.get();
-    }
-}

src/main/java/frc4388/robot/commands/Swerve/RotateToAngle.java

@@ -1,35 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc4388.robot.commands.Swerve;
-
-import edu.wpi.first.math.geometry.Translation2d;
-import frc4388.robot.commands.PID;
-import frc4388.robot.subsystems.swerve.SwerveDrive;
-
-public class RotateToAngle extends PID {
-
-  SwerveDrive drive;
-  double targetAngle;
-
-  /** Creates a new RotateToAngle. */
-  public RotateToAngle(SwerveDrive drive, double targetAngle) {
-    super(0.3, 0.0, 0.0, 0.0, 1);
-    
-    this.drive = drive;
-    this.targetAngle = targetAngle;
-
-    addRequirements(drive);
-  }
-
-  @Override
-  public double getError() {
-    return targetAngle - drive.getGyroAngle();
-  }
-
-  @Override
-  public void runWithOutput(double output) {
-    drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(output / Math.abs(getError()), 0.0), true);    
-  }
-}

src/main/java/frc4388/robot/commands/Swerve/StayInPosition.java

@@ -0,0 +1,54 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc4388.robot.commands.Swerve;
+
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.AutoLogOutput;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import frc4388.robot.commands.PID;
+import frc4388.robot.subsystems.swerve.SwerveDrive;
+
+public class StayInPosition extends PID {
+    SwerveDrive drive;
+    Translation2d driveTranslation = new Translation2d();
+
+    public StayInPosition(SwerveDrive drive) {
+        super(0.3, 0.0, 0.0, 0.0, 1);
+        this.drive = drive;
+        addRequirements(drive);
+    }
+
+    public void goToTargetPose(Pose2d targetPose) {
+        Pose2d currentPose = drive.getCurrentPose();
+        double translationX = targetPose.getX() - currentPose.getX();
+        double translationY = targetPose.getY() - currentPose.getY();
+        if (translationX > 0.2){
+        //If below is changed make this change too so it never goes over 1
+          translationX = 0.2;
+        }
+        if (translationY > 0.2){
+        //Same here
+          translationY = 0.2;
+        }
+        if (Math.abs(translationX) < 0.08 && Math.abs(translationY) < 0.08) {
+            driveTranslation = new Translation2d();
+        } else {
+            //TODO: Tweak for best corrector against another bot
+            driveTranslation = new Translation2d(translationX * 4.5, translationY * 4.5); 
+        }
+
+        drive.driveFieldAngleSIP(driveTranslation, targetPose.getRotation());
+    }
+
+    @Override
+    public double getError() {
+        return 0;
+    }
+
+    @Override
+    public void runWithOutput(double output) {}
+}

src/main/java/frc4388/robot/commands/Swerve/neoJoystickPlayback.java

@@ -1,197 +0,0 @@
-package frc4388.robot.commands.Swerve;
-
-import java.io.FileInputStream;
-import java.util.ArrayList;
-import java.util.function.Supplier;
-
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc4388.robot.subsystems.swerve.SwerveDrive;
-import frc4388.utility.compute.DataUtils;
-import frc4388.utility.controller.VirtualController;
-import frc4388.utility.structs.UtilityStructs.AutoRecordingControllerFrame;
-import frc4388.utility.structs.UtilityStructs.AutoRecordingFrame;
-
-
-/**
- * The NEO autonomus playback system, designed based the old {@link JoystickPlayback} System but with {@link VirtualController}s 
- * @author Zachary Wilke
- */
-public class neoJoystickPlayback extends Command {
-    private final SwerveDrive                   swerve;
-    private final VirtualController[]           controllers;
-    private final ArrayList<AutoRecordingFrame> frames         = new ArrayList<>();
-    private final Supplier<String>              filenameGetter;
-    private       String                        filename;
-    private       int                           frame_index    = 0;
-    // private       long                          startTime      = 0;
-    // private       long                          playbackTime   = 0;
-    private       boolean                       m_finished     = false; // ! There is no better way.
-    private       boolean                       m_shouldfree   = false; // should free memory on ending
-    
-    private byte  m_numAxes = 0;
-    private byte  m_numPOVs = 0;
-    private byte  m_numControllers = 0;
-    private short m_numFrames = -1;
-
-    /**
-     * Creates an new NEO Joystick Playback with specifyed pramiters.
-     * @param swerve m_robotSwerveDrive
-     * @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board. 
-     * @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
-     * @param shouldfree Unloads the auto on compleation or intruption.
-     * @param instantload Load the auto on object instantiation
-     */
-    public neoJoystickPlayback(SwerveDrive swerve, Supplier<String> filenameGetter, VirtualController[] controllers, boolean shouldfree, boolean instantload) {
-        this.swerve = swerve;
-        this.filenameGetter = filenameGetter;
-        this.controllers = controllers;
-        this.m_shouldfree = shouldfree;
-
-        if (instantload) loadAuto();
-        addRequirements(this.swerve);
-    }
-
-    /**
-     * Creates an new NEO Joystick Playback with specifyed pramiters.
-     * @param swerve m_robotSwerveDrive
-     * @param filename a String containing the name of the auto file you wish to playback. 
-     * @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
-     * @param shouldfree unloads the auto on compleation or intruption.
-     * @param instantload load the auto on object instantiation
-     */
-    public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers, boolean shouldfree, boolean instantload) {
-        this(swerve, () -> filename, controllers, shouldfree, instantload);
-    }
-
-    /**
-     * Creates an new NEO Joystick Playback with specifyed pramiters.
-     * @param swerve m_robotSwerveDrive
-     * @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board.
-     * @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
-     */
-    public neoJoystickPlayback(SwerveDrive swerve, Supplier<String> filenameGetter, VirtualController[] controllers) {
-        this(swerve, filenameGetter, controllers, true, false);
-    }
-
-    /**
-     * Creates an new NEO Joystick Playback with specifyed pramiters.
-     * @param swerve m_robotSwerveDrive
-     * @param filename a String containing the name of the auto file you wish to playback. 
-     * @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
-     */
-    public neoJoystickPlayback(SwerveDrive swerve, String filename, VirtualController[] controllers) {
-        this(swerve, () -> filename, controllers, true, false);
-    }
-    
-    /**
-     * Load the auto file from disk into memory
-     * @return Returns true if loading was successful, else wise; return false
-     * @implNote if the auto is already loaded, it will return true. 
-     */
-    public boolean loadAuto() {
-        filename = filenameGetter.get();
-        try (FileInputStream stream = new FileInputStream("/home/lvuser/autos/" + filename)) {
-            if (m_numFrames != -1 && m_numFrames == frames.size()) {
-                System.out.println("AUTOPLAYBACK: Auto Already loaded.");
-                return true;
-            }
-            
-            m_numAxes = stream.readNBytes(1)[0];
-            m_numPOVs = stream.readNBytes(1)[0];
-            m_numControllers = stream.readNBytes(1)[0];
-            m_numFrames = DataUtils.byteArrayToShort(stream.readNBytes(2));
-
-            if (m_numControllers > controllers.length) {
-                System.out.println("AUTOPLAYBACK: The auto file `" + filename + "` wants " + m_numControllers 
-                + " virtual controllers but only " + controllers.length + " were given");
-                return false;
-            }
-
-            for (int i = 0; i < m_numFrames; i++) {
-                AutoRecordingFrame frame = new AutoRecordingFrame();
-                for (int j = 0; j < m_numControllers; j++) {
-                    AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame();
-                    double[] axes = new double[m_numAxes];
-                    for (int k = 0; k < m_numAxes; k++) { // we love third level for loops.
-                        axes[k] = DataUtils.byteArrayToDouble(stream.readNBytes(8));
-                    }
-                    short button = DataUtils.byteArrayToShort(stream.readNBytes(2));
-                    short[] POV = new short[m_numPOVs];
-                    for (int k = 0; k < m_numPOVs; k++) {
-                        POV[k] = DataUtils.byteArrayToShort(stream.readNBytes(2));
-                    }
-                    controllerFrame.axes = axes;
-                    controllerFrame.button = button;
-                    controllerFrame.POV = POV;
-                    frame.controllerFrames[j] = controllerFrame;
-                }
-                frame.timeStamp = DataUtils.byteArrayToInt(stream.readNBytes(4));
-                frames.add(frame);
-            }
-
-            System.out.println("AUTOPLAYBACK: Read Auto `" + filename + "` that is " + m_numFrames + " frames long");
-            return true;
-
-        } catch (Exception e) {
-            e.printStackTrace();
-            System.out.println("AUTOPLAYBACK: Unable to read auto file `" + filename + '`');
-            return false;
-        }
-    }
-    
-    /**
-     * Unloads the auto.
-     */
-    public void unloadAuto() {
-        System.out.println("AUTOPLAYBACK: Auto unloaded");
-        frames.clear();
-    }
-
-    @Override
-    public void initialize() {
-        // startTime = System.currentTimeMillis();
-        // playbackTime = 0;
-        frame_index = 0;
-
-        m_finished = !loadAuto();
-    }
-
-    @Override
-    public void execute() {
-        if (frame_index >= m_numFrames) m_finished = true;
-        if (m_finished) return;
-
-        // if (frame_index == 0) {
-        //    startTime = System.currentTimeMillis();
-        //    playbackTime = 0;
-        // } else {
-        //    playbackTime = System.currentTimeMillis() - startTime;
-        // }
-
-        AutoRecordingFrame frame = frames.get(frame_index);
-        for (int i = 0; i < controllers.length; i++) {
-            AutoRecordingControllerFrame controllerFrame = frame.controllerFrames[i];
-            controllers[i].setFrame(controllerFrame.axes, controllerFrame.button, controllerFrame.POV);
-            if (i == 0) {
-                this.swerve.driveWithInput(
-                    new Translation2d(controllers[i].getRawAxis(0), controllers[i].getRawAxis(1)),
-                    new Translation2d(controllers[i].getRawAxis(4), controllers[i].getRawAxis(5)),
-                    true);
-            }
-        }
-        frame_index++;
-    }
-
-    @Override
-    public void end(boolean interrupted) {
-        for (VirtualController controller : controllers) controller.zeroControls();
-        swerve.stopModules();
-        if (m_shouldfree) unloadAuto();
-    }
-    
-    @Override
-    public boolean isFinished() {
-        return m_finished;
-    }
-}

src/main/java/frc4388/robot/commands/Swerve/neoJoystickRecorder.java

@@ -1,129 +0,0 @@
-package frc4388.robot.commands.Swerve;
-
-import java.io.FileOutputStream;
-import java.util.ArrayList;
-import java.util.function.Supplier;
-
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.wpilibj.XboxController;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc4388.robot.subsystems.swerve.SwerveDrive;
-import frc4388.utility.compute.DataUtils;
-import frc4388.utility.controller.DeadbandedXboxController;
-import frc4388.utility.structs.UtilityStructs.AutoRecordingControllerFrame;
-import frc4388.utility.structs.UtilityStructs.AutoRecordingFrame;
-
-/**
- * The NEO autonomus recording system, designed based the old {@link JoystickRecorder} System but with {@link frc4388.utility.controller.VirtualController VirtualController}s 
- * @author Zachary Wilke
- */
-public class neoJoystickRecorder extends Command {
-    private final SwerveDrive swerve;
-    private final XboxController[] controllers;
-    private       String filename;
-    private final Supplier<String> filenameGetter;
-    private       long startTime = -1;
-    private final ArrayList<AutoRecordingFrame> frames = new ArrayList<>();
-
-    /**
-     * Creates an new NEO Joystick Playback with specifyed pramiters.
-     * @param swerve m_robotSwerveDrive
-     * @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
-     * @param filenameGetter a String Supplier, designed for quickly changing auto names in shuffle board.
-     */
-    public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, Supplier<String> filenameGetter) {
-        this.swerve = swerve;
-        this.controllers = controllers;
-        this.filenameGetter = filenameGetter;
-        this.filename = "";
-
-        addRequirements(this.swerve);
-    }
-
-    /**
-     * Creates an new NEO Joystick Playback with specifyed pramiters.
-     * @param swerve m_robotSwerveDrive
-     * @param controllers an <b>Order-Specific</b> Array of Virtual controllers, index 0 means driver, index 1 means operator, etc.
-     * @param filename a String containing the name of the auto file you wish to playback. 
-     */
-    public neoJoystickRecorder(SwerveDrive swerve, DeadbandedXboxController[] controllers, String filename) {
-        this(swerve, controllers, () -> filename);
-    }
-
-    @Override
-    public void initialize() {
-        frames.clear();
-
-        this.startTime = System.currentTimeMillis();
-        AutoRecordingFrame frame = new AutoRecordingFrame();
-        frame.controllerFrames = new AutoRecordingControllerFrame[] {new AutoRecordingControllerFrame(), new AutoRecordingControllerFrame()};
-        frames.add(frame);
-        this.filename = this.filenameGetter.get();
-    }
-
-
-    @Override
-    public void execute() {
-        System.out.println("AUTORECORD: RECORDING");
-        AutoRecordingFrame frame = new AutoRecordingFrame();
-        frame.timeStamp = (int) (System.currentTimeMillis() - startTime);
-        for (int i = 0; i < controllers.length; i++) {
-            XboxController controller = controllers[i];
-            AutoRecordingControllerFrame controllerFrame = new AutoRecordingControllerFrame();
-            double[] axes = {controller.getLeftX(), controller.getLeftY(), 
-                            controller.getLeftTriggerAxis(), controller.getRightTriggerAxis(),
-                            controller.getRightX(), controller.getRightY()};
-            short button = 0;
-            for (int j = 0; j < 10; j++)
-                if (controller.getRawButton(j+1))
-                    button |= 1 << j;
-            short[] POV = {(short)(controller.getPOV())};
-            controllerFrame.axes = axes;
-            controllerFrame.button = button;
-            controllerFrame.POV = POV;
-            frame.controllerFrames[i] = controllerFrame;
-        }
-
-        frames.add(frame);
-
-        swerve.driveWithInput(new Translation2d(frame.controllerFrames[0].axes[0], frame.controllerFrames[0].axes[1]),
-                            new Translation2d(frame.controllerFrames[0].axes[4], frame.controllerFrames[0].axes[5]), 
-                            true); // Really jank way of doing this.
-        
-    }
-    @Override
-    public void end(boolean interrupted) {
-        try (FileOutputStream stream = new FileOutputStream("/home/lvuser/autos/" + filename)) {
-            // header: size of 0x5
-            // byte Number of axes per controller
-            // byte Number of POVs per controller
-            // byte Number of controllers
-            // short Number of frames 
-            stream.write(new byte[]{6, 1, (byte) controllers.length}); 
-            stream.write(DataUtils.shortToByteArray((short) frames.size())); 
-
-            // frame
-            // controller frame * number of controllers
-            // int unix time stamp.
-            for (AutoRecordingFrame frame : frames) {
-                // controller frame
-                // double axis * Number of axes per controller
-                // short button states
-                // short POV * Number of POVs per controller
-                for (AutoRecordingControllerFrame controllerFrame: frame.controllerFrames) {
-                    for (double axis: controllerFrame.axes) {
-                        stream.write(DataUtils.doubleToByteArray(axis));
-                    }
-                    stream.write(DataUtils.shortToByteArray(controllerFrame.button));
-                    for (short POV: controllerFrame.POV) {
-                        stream.write(DataUtils.shortToByteArray(POV));
-                    }
-                }
-                stream.write(DataUtils.intToByteArray(frame.timeStamp));
-            }
-        System.out.println("AUTORECORD: Wrote auto `" + filename + "` that is " + frames.size() + " frames long.");
-        } catch (Exception e) {
-            e.printStackTrace();
-        } 
-    }
-}

src/main/java/frc4388/robot/commands/WhileTrueCommand.java

@@ -1,104 +0,0 @@
-package frc4388.robot.commands;
-
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-import static edu.wpi.first.util.ErrorMessages.requireNonNullParam;
-
-import edu.wpi.first.util.sendable.SendableBuilder;
-import edu.wpi.first.wpilibj2.command.Command;
-
-import java.util.function.BooleanSupplier;
-
-/**
- * A command composition that runs one of two commands, depending on the value of the given
- * condition when this command is initialized.
- *
- * <p>The rules for command compositions apply: command instances that are passed to it cannot be
- * added to any other composition or scheduled individually, and the composition requires all
- * subsystems its components require.
- *
- * <p>This class is provided by the NewCommands VendorDep
- */
-public class WhileTrueCommand extends Command {
-  private final Command m_whileTrue;
-  private final BooleanSupplier m_condition;
-
-  /**
-   * Creates a new WhileTrueCommand.
-   *
-   * @param whileTrue the command to run while the condition is true
-   * @param condition the condition to determine which command to run
-   */
-  @SuppressWarnings("this-escape")
-  public WhileTrueCommand(Command whileTrue, BooleanSupplier condition) {
-    m_whileTrue = requireNonNullParam(whileTrue, "whileTrue", "WhileTrueCommand");
-    m_condition = requireNonNullParam(condition, "condition", "WhileTrueCommand");
-
-    //CommandScheduler.getInstance().registerComposedCommands(whileTrue);
-
-    // addRequirements(whileTrue.getRequirements());
-  }
-
-  @Override
-  public void initialize() {
-    if(m_condition.getAsBoolean())
-      m_whileTrue.initialize();
-  }
-
-  @Override
-  public void execute() {
-    m_whileTrue.execute();
-
-    System.out.println("Loop, " + !m_whileTrue.isFinished() + ", " +  m_condition.getAsBoolean());
-
-    if(!m_whileTrue.isFinished())
-      return;
-
-    if(m_condition.getAsBoolean()){
-      m_whileTrue.end(false);
-      m_whileTrue.initialize();
-    }
-  }
-
-  @Override
-  public void end(boolean interrupted) {
-    m_whileTrue.end(interrupted);
-  }
-
-  @Override
-  public boolean isFinished() {
-    return !m_condition.getAsBoolean() && m_whileTrue.isFinished();
-  }
-
-  @Override
-  public boolean runsWhenDisabled() {
-    return m_whileTrue.runsWhenDisabled();
-  }
-
-  @Override
-  public InterruptionBehavior getInterruptionBehavior() {
-    if (m_whileTrue.getInterruptionBehavior() == InterruptionBehavior.kCancelSelf) {
-      return InterruptionBehavior.kCancelSelf;
-    } else {
-      return InterruptionBehavior.kCancelIncoming;
-    }
-  }
-
-  @Override
-  public void initSendable(SendableBuilder builder) {
-    super.initSendable(builder);
-    builder.addStringProperty("whileTrue", m_whileTrue::getName, null);
-    builder.addStringProperty(
-        "selected",
-        () -> {
-          if (m_whileTrue == null) {
-            return "null";
-          } else {
-            return m_whileTrue.getName();
-          }
-        },
-        null);
-  }
-}

src/main/java/frc4388/robot/commands/alignment/AutoAlign.java

@@ -6,8 +6,8 @@ import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc4388.robot.constants.Constants.AutoConstants;
-import frc4388.robot.subsystems.Lidar;
 import frc4388.robot.subsystems.swerve.SwerveDrive;
+import frc4388.robot.subsystems.vision.Lidar;
 import frc4388.robot.subsystems.vision.Vision;
 import frc4388.utility.compute.FieldPositions;
 import frc4388.utility.structs.Gains;

src/main/java/frc4388/robot/commands/alignment/RotTo45.java

@@ -1,37 +0,0 @@
-package frc4388.robot.commands.alignment;
-
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.wpilibj2.command.Command;
-import frc4388.robot.subsystems.swerve.SwerveDrive;
-
-public class RotTo45 extends Command {
-
-    SwerveDrive m_SwerveDrive;
-    Rotation2d targetAngle;
-
-
-    public RotTo45(SwerveDrive swerveDrive) {
-        m_SwerveDrive = swerveDrive;
-        
-        addRequirements(swerveDrive);
-    }
-
-    @Override
-    public void initialize() {
-        targetAngle = new Rotation2d();
-    }
-
-    @Override
-    public void execute() {
-        m_SwerveDrive.driveRelativeAngle(new Translation2d(), targetAngle);
-    }
-
-    @Override
-    public boolean isFinished() {
-        Rotation2d curRot = m_SwerveDrive.getPose2d().getRotation();
-        // TODO: Tune
-        return Math.abs(curRot.getDegrees() - targetAngle.getDegrees()) < 5;
-    }
-    
-}

src/main/java/frc4388/robot/commands/wait/waitSupplier.java

@@ -1,36 +0,0 @@
-// Copyright (c) FIRST and other WPILib contributors.
-// Open Source Software; you can modify and/or share it under the terms of
-// the WPILib BSD license file in the root directory of this project.
-
-package frc4388.robot.commands.wait;
-
-import java.util.function.Supplier;
-
-import edu.wpi.first.wpilibj2.command.Command;
-
-/* You should consider using the more terse Command factories API instead https://docs.wpilib.org/en/stable/docs/software/commandbased/organizing-command-based.html#defining-commands */
-public class waitSupplier extends Command {
-  /** Creates a new waitSupplier. */
-  private final Supplier<Boolean> truth;
-  public waitSupplier(Supplier<Boolean> truth) {
-    this.truth = truth;
-  }
-
-  // Called when the command is initially scheduled.
-  @Override
-  public void initialize() {}
-
-  // Called every time the scheduler runs while the command is scheduled.
-  @Override
-  public void execute() {}
-
-  // Called once the command ends or is interrupted.
-  @Override
-  public void end(boolean interrupted) {}
-
-  // Returns true when the command should end.
-  @Override
-  public boolean isFinished() {
-    return truth.get();
-  }
-}

src/main/java/frc4388/robot/constants/BuildConstants.java

@@ -7,12 +7,12 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "2026KPopRobotHunters";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 147;
+  public static final int GIT_REVISION = 182;
-  public static final String GIT_SHA = "b073fad27703951ba251eaffc56d86f791e531e6";
+  public static final String GIT_SHA = "75cab187e2225e51259336e5569f15c41f9169d5";
-  public static final String GIT_DATE = "2026-03-07 20:08:15 MST";
+  public static final String GIT_DATE = "2026-03-21 13:38:34 MDT";
-  public static final String GIT_BRANCH = "PikesPeak";
+  public static final String GIT_BRANCH = "MiraOrg";
-  public static final String BUILD_DATE = "2026-03-08 14:27:19 MDT";
+  public static final String BUILD_DATE = "2026-03-21 15:45:59 MDT";
-  public static final long BUILD_UNIX_TIME = 1773001639109L;
+  public static final long BUILD_UNIX_TIME = 1774129559544L;
   public static final int DIRTY = 1;
 
   private BuildConstants(){}

src/main/java/frc4388/robot/constants/Constants.java

@@ -117,6 +117,11 @@ public final class Constants {
         // Operator ready to shoot, but the driver conditions are bad
         public static final LEDPatterns OPREADY_BADPHYS = LEDPatterns.WHITE_STROBE;
 
+        // LED color for when the indexer was stuck on ball and going in reverse
+        public static final LEDPatterns INDEXER_REVERSE = LEDPatterns.SOLID_VIOLET;
+
+        // LED color for when the indexer, intake roller, or shooter is not going at right speed for more than 5 seconds and is likely stalled
+        public static final LEDPatterns MOTOR_STALLED = LEDPatterns.SOLID_RED;
 
         public static final LEDPatterns OPREADY_FEED = LEDPatterns.SOLID_BLUE;
         // public static final LEDPatterns OPREADY_FEED_BADPHYS = LEDPatterns.BLUE_STROBE;

src/main/java/frc4388/robot/constants/FieldConstants.java

@@ -1,18 +1,27 @@
 package frc4388.robot.constants;
 
-import java.util.Arrays;
+import static edu.wpi.first.units.Units.Meters;
 
-import edu.wpi.first.apriltag.AprilTag;
 import edu.wpi.first.apriltag.AprilTagFieldLayout;
 import edu.wpi.first.apriltag.AprilTagFields;
-import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform2d;
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.math.geometry.Translation3d;
 
 public final class FieldConstants {
     public static final AprilTagFieldLayout kTagLayout = AprilTagFieldLayout.loadField(AprilTagFields.k2026RebuiltAndymark);
     
+    public static final double BUMP_OFFSET = 0.4;
+    public static final Transform2d BUMP_OFFSET_RED = new Transform2d(
+        Meters.of(BUMP_OFFSET),
+        Meters.of(0),
+        new Rotation2d()
+    );
+    public static final Transform2d BUMP_OFFSET_BLUE = new Transform2d(
+        Meters.of(-BUMP_OFFSET),
+        Meters.of(0),
+        new Rotation2d()
+    );
+
     // Test april tag field layout
         // public static final AprilTagFieldLayout kTagLayout = new AprilTagFieldLayout(
         //     Arrays.asList(new AprilTag[] {

src/main/java/frc4388/robot/subsystems/intake/Intake.java

@@ -45,6 +45,13 @@ public class Intake extends SubsystemBase {
         io.setRollerOutput(state, 0);
     }
 
+    public double getRollerTarget() {
+        return state.rollerTargetOutput;
+    }
+
+    public double getRollerSpeed() {
+        return state.rollerOutput;
+    }
 
     // public enum FieldZone {
     //     // The robot should aim at the hub

src/main/java/frc4388/robot/subsystems/led/LED.java

@@ -5,7 +5,7 @@
 /* the project.                                                               */
 /*----------------------------------------------------------------------------*/
 
-package frc4388.robot.subsystems;
+package frc4388.robot.subsystems.led;
 
 import java.util.concurrent.TimeUnit;
 

src/main/java/frc4388/robot/subsystems/shooter/Shooter.java

@@ -9,16 +9,21 @@ import org.littletonrobotics.junction.AutoLogOutput;
 import org.littletonrobotics.junction.Logger;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.constants.Constants;
-import frc4388.robot.subsystems.LED;
 import frc4388.robot.subsystems.intake.Intake;
+import frc4388.robot.subsystems.led.LED;
 import frc4388.robot.subsystems.swerve.SwerveDrive;
 import frc4388.utility.compute.FieldPositions;
+import frc4388.utility.compute.HubShiftTimer;
+import frc4388.utility.compute.HubShiftTimer.ShiftInfo;
+import frc4388.utility.compute.TimesNegativeOne;
 
 public class Shooter extends SubsystemBase {
     public ShooterIO io;
@@ -31,6 +36,9 @@ public class Shooter extends SubsystemBase {
 
     // Supplier<Pose2d> m_swervePoseSupplier;
     public boolean badShooterVelocity;
+    public double distanceToHub = 5;
+    public double chassisXSpeed = 0;
+
 
 
     public Shooter(
@@ -76,16 +84,22 @@ public class Shooter extends SubsystemBase {
         this.mode = ShooterMode.Idle;
     }
 
-
+    public double getDistanceToHub(){
+        return distanceToHub;
+    }
 
     public void allowShooting() {
         shooterButtonReady = true;
-    }
+    }   
 
     public void denyShooting() {
         shooterButtonReady = false;
     }
 
+    public double getBallVelocity() {
+        return Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) * ShooterConstants.SHOOTER_RADIUS * 2 * Math.PI;
+        //Math.abs(state.indexerForwardVelocity.in(RotationsPerSecond))*ShooterConstants.INDEXER_RADIUS)
+    }
 
     @AutoLogOutput
     public ShooterMode getMode() {
@@ -102,40 +116,45 @@ public class Shooter extends SubsystemBase {
     @Override
     public void periodic() {
         // FaultReporter.register(this); // TODO Implement fault reporter
-
+        //Hub Shift logs
+        ShiftInfo info = HubShiftTimer.getShiftInfo();
+        Logger.recordOutput("HubShift/IsActive", info.isActive());
+        Logger.recordOutput("HubShift/RemainingInShift", info.remainingInShift());
+        Logger.recordOutput("HubShift/Phase", info.phase().name());
         Logger.processInputs("Shooter", state);
+
         io.updateInputs(state);
 
 
         // Get robot positon and speeds
         ChassisSpeeds chassisSpeeds = m_SwerveDrive.chassisSpeeds;
-        double XYSpeed = Math.sqrt(Math.pow(chassisSpeeds.vxMetersPerSecond,2) + Math.pow(chassisSpeeds.vyMetersPerSecond,2));
-        double AngSpeed  = Math.abs(chassisSpeeds.omegaRadiansPerSecond * (180/Math.PI));
-
-        Pose2d robotPose2d = m_SwerveDrive.getPose2d();
-
-
-        // Calculate aim lead
-        // Get the current speed of the robot
         Translation2d robotSpeed = new Translation2d(
             chassisSpeeds.vxMetersPerSecond, 
             chassisSpeeds.vyMetersPerSecond
         );
+        Translation2d fieldPosLead = robotSpeed.times(ShooterConstants.AIM_LEAD_TIME.get()).plus(FieldPositions.HUB_POSITION);
+        Rotation2d ang = m_SwerveDrive.getPose2d().getTranslation().minus(fieldPosLead).getAngle().minus(m_SwerveDrive.getPose2d().getRotation());
+        Pose2d robotPose2d = m_SwerveDrive.getPose2d();
 
-        // Calculate a point to aim ahead of the actual position.
+        if (TimesNegativeOne.isRed) {
-        Translation2d fieldPosLead = robotSpeed.times(ShooterConstants.AIM_LEAD_TIME.get()).plus(robotPose2d.getTranslation());
+            chassisXSpeed = chassisSpeeds.vxMetersPerSecond;
+        } else {
+            chassisXSpeed = -chassisSpeeds.vxMetersPerSecond;
+        }
 
+        Translation2d fieldPos = robotPose2d.getTranslation();
         // Get the robot's aim distance to hub
-        double distanceToHub = (fieldPosLead.minus(FieldPositions.HUB_POSITION).getNorm());
+        distanceToHub = (fieldPos.minus(FieldPositions.HUB_POSITION).getNorm());
 
-        //Center of hub to cameras in inches
+        //Center of hub to cameras in meters
         Logger.recordOutput("Hub Dist", distanceToHub);
 
         boolean driverError = 
             // XYSpeed <= ShooterConstants.ROBOT_SPEED_TOLERANCE.get() |
             // AngSpeed <= ShooterConstants.ROBOT_ANG_SPEED_TOLERANCE.get() |
             distanceToHub <= ShooterConstants.ROBOT_MIN_HUB.get() | 
-            distanceToHub >= ShooterConstants.ROBOT_MAX_HUB.get();
+            distanceToHub >= ShooterConstants.ROBOT_MAX_HUB.get() |
+            Math.abs(ang.getDegrees()) > ShooterConstants.AIM_ANGLE.get();
 
 
         double shooterSpeed = Math.abs(state.motor1Velocity.in(RotationsPerSecond) + state.motor2Velocity.in(RotationsPerSecond)) / 2;
@@ -145,14 +164,14 @@ public class Shooter extends SubsystemBase {
 
         //revtime calculations
         // double shooterAcceleration = 
-        double shooterSpeedTargetPretend = ShooterConstants.getTargetShooterSpeed(distanceToHub).in(RotationsPerSecond);
+        double shooterSpeedTargetPretend = ShooterConstants.getTargetShooterSpeed(distanceToHub, chassisXSpeed).in(RotationsPerSecond);
         double revTime = (Math.abs(shooterSpeed - shooterSpeedTargetPretend)/((7 - shooterSpeedTargetPretend)/ShooterConstants.T_CONSTANT));
         // double revTimeExp = ShooterConstants.T_CONSTANT * Math.log(1 - Math.abs(shooterSpeed/shooterSpeedTargetPretend));
         Logger.recordOutput("Time to rev", revTime);
 
         switch (mode) {
             case Shooting:
-                io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub));
+                io.setShooterVelocity(state, ShooterConstants.getTargetShooterSpeed(distanceToHub, chassisXSpeed));
 
                 int bitmask = (
                     (shooterButtonReady ? 1 : 0) +
@@ -166,7 +185,7 @@ public class Shooter extends SubsystemBase {
                         m_robotLED.setMode(Constants.LEDConstants.OPREADY);
                         break;
 
-                    case 0b001: // No errors and shoot button is pressed
+                    case 0b001: // No errors and shoot button is pressed     
                         io.setIndexerOutput(state, ShooterConstants.INDEXER_FORWARD_OUTPUT.get());
                         m_robotLED.setMode(Constants.LEDConstants.OPREADY);
                         break;
@@ -179,13 +198,13 @@ public class Shooter extends SubsystemBase {
 
                     case 0b100: // Driver error, button is not pressed
                     case 0b101: // Driver error, button is pressed
-                        io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
+                        io.setIndexerOutput(state, 0);
                         m_robotLED.setMode(Constants.LEDConstants.OPREADY_BADPHYS);
                         break;
 
                     case 0b110: // Driver error, bad shooter vel, button is not pressed
                     case 0b111: // Driver error, bad shooter vel, button is pressed
-                        io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
+                        io.setIndexerOutput(state, 0);
                         m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL_BADPHYS);
                         break;
                 }
@@ -200,7 +219,7 @@ public class Shooter extends SubsystemBase {
 
                 switch (bitmask2) {
                     case 0b000: // No errors but button is not pressed
-                        io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
+                        io.setIndexerOutput(state, 0);
                         m_robotLED.setMode(Constants.LEDConstants.OPREADY_FEED);
                         break;
 
@@ -211,7 +230,7 @@ public class Shooter extends SubsystemBase {
 
                     case 0b010: // Bad shooter velocity, button is not pressed
                     case 0b011: // Bad shooter velocty, button is pressed
-                        io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
+                        io.setIndexerOutput(state, 0);
                         m_robotLED.setMode(Constants.LEDConstants.BAD_FLYWEEL);
                         break;
 
@@ -237,12 +256,11 @@ public class Shooter extends SubsystemBase {
                     // Amps.of(ShooterConstants.SHOOTER_IDLE_MAX_CURRENT.get()), 
                     // RotationsPerSecond.of(ShooterConstants.INDEXER_REVERSE_OUTPUT.get())
                 );
-                io.setIndexerOutput(state, ShooterConstants.INDEXER_REVERSE_OUTPUT.get());
+                io.setIndexerOutput(state, 0);
                 m_robotLED.setMode(Constants.LEDConstants.DEFAULT_PATTERN);
                 break;
-            
-
             }
-
+            
+        io.motorStalled(state, m_Intake, m_robotLED);
     }
 }

src/main/java/frc4388/robot/subsystems/shooter/ShooterConstants.java

@@ -15,30 +15,37 @@ import frc4388.utility.status.CanDevice;
 public class    ShooterConstants {
     // Motor conversions
     
-    public static final double SHOOTERMOTOR_GEAR_RATIO = 1.5;
+    public static final double SHOOTERMOTOR_GEAR_RATIO = 1.286; // TODO: supposed to be 9 rotations in to 7 out    -- 0.77 or 1.29
-    public static final double INDEXER_GEAR_RATIO = 1.;
+    public static final double INDEXER_GEAR_RATIO = 1.72; // TODO: change it is supposed to be 18 to 31 
     public static final double T_CONSTANT = 2;
-
+    public static final double SHOOTER_RADIUS = 2/39.37;
+    public static final double INDEXER_RADIUS = 0.625/39.37;
     public static final ConfigurableDouble SHOOTER_MAX_VELOCITY = new ConfigurableDouble("Shooter MAX Velocity", 60);
     public static final ConfigurableDouble SHOOTER_OVERRIDE_VELOCITY = new ConfigurableDouble("Shooter OVERRIDE Velocity", -42);
     // public static final ConfigurableDouble SHOOTER_FEED_VELOCITY = new ConfigurableDouble("Shooter Feed Velocity", -35);
     // public static final ConfigurableDouble SHOOTER_RESTING_VELOCITY = new ConfigurableDouble("Shooter Resting Velocity", 0.0);
     
+    
     public static final ConfigurableDouble SHOOTER_IDLE_PERCENT_OUTPUT = new ConfigurableDouble("Shooter idle % output", -0.15);
     // public static final ConfigurableDouble SHOOTER_IDLE_TARGET_VEL = new ConfigurableDouble("Shooter idle target velocity", 20.);
     // public static final ConfigurableDouble SHOOTER_IDLE_MAX_CURRENT = new ConfigurableDouble("Shooter Idle max current", 10);
 
     public static final ConfigurableDouble INDEXER_FORWARD_OUTPUT = new ConfigurableDouble("Indexer FWD % Output", -0.4);
-    public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse % Output", 0.0);
+    public static final ConfigurableDouble INDEXER_REVERSE_OUTPUT = new ConfigurableDouble("Indexer reverse % Output", 0.2);
-    public static final ConfigurableDouble MODEL_TRIM = new ConfigurableDouble("TRIM SHOOTER SPEED", -1.5);
+    public static final ConfigurableDouble MODEL_TRIM = new ConfigurableDouble("TRIM SHOOTER SPEED", 0);
 
     
-    public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", 0);
+    public static final ConfigurableDouble NEG_OFFSET = new ConfigurableDouble("Negative offset", 8.);
+    public static final ConfigurableDouble POS_OFFSET = new ConfigurableDouble("Positive offset", 8.);
+
+    
+    public static final ConfigurableDouble AIM_LEAD_TIME = new ConfigurableDouble("Aim lead time", -1.1);
 
     // Shoot mode tolerances
     public static final ConfigurableDouble ROBOT_MIN_HUB = new ConfigurableDouble("Shoot min dist M", 1.8);
     public static final ConfigurableDouble ROBOT_MAX_HUB = new ConfigurableDouble("Shoot max dist M", 4.8);
 
+    public static final ConfigurableDouble AIM_ANGLE = new ConfigurableDouble("Aim angle tolerance", 15);
     public static final ConfigurableDouble ROBOT_ANG_TOLERANCE = new ConfigurableDouble("Ang tolerance DEG", 360);
 
     public static final ConfigurableDouble ROBOT_SPEED_TOLERANCE = new ConfigurableDouble("Speed tolerance MS", 1);
@@ -47,20 +54,23 @@ public class    ShooterConstants {
     public static final ConfigurableDouble SHOOTER_SPEED_TOLERANCE = new ConfigurableDouble("Shooter speed tolerance RPS", 3); 
 
     // 
-    public static AngularVelocity getTargetShooterSpeed(double hubDistMeters) {
+    public static AngularVelocity getTargetShooterSpeed(double hubDistMeters, double chassisXSpeed) {
-        // Model derived from points
+            double speed = 0;
-        // double speed = 
-        //     1.11576*hubDistMeters*hubDistMeters +
-        //     0.318464*hubDistMeters +
-        //     30.6293;
-        double speed = 
-            5.6939*hubDistMeters +
-            22.76545 + MODEL_TRIM.get();
 
-        // double speed =
+        if (Math.abs(chassisXSpeed) < 0.1){
-        //     0.00610938*hubDistMeters*hubDistMeters
+            speed = 0.0593402*hubDistMeters*hubDistMeters +
-        //     5.65235*hubDistMeters +
+            4.90561*hubDistMeters +
-        //     22.82825;
+            30.35696 + MODEL_TRIM.get();
+        } else if (chassisXSpeed > 0){
+            speed = 0.0593402*hubDistMeters*hubDistMeters +
+            4.90561*hubDistMeters +
+            30.35696 + chassisXSpeed * POS_OFFSET.get() + MODEL_TRIM.get();
+            
+        } else { // Negative is closer to hub
+            speed = 0.0593402*hubDistMeters*hubDistMeters +
+            4.90561*hubDistMeters +
+            30.35696 + chassisXSpeed * NEG_OFFSET.get() + MODEL_TRIM.get();
+        }
         
         double max = SHOOTER_MAX_VELOCITY.get();
 
@@ -79,7 +89,7 @@ public class    ShooterConstants {
     // Motor Configuration
     public static Slot0Configs SHOOTER_PID = new Slot0Configs()
         .withKV(0.0)
-        .withKP(0.08)
+        .withKP(0.02)
         .withKI(0.15)
         .withKD(0.0);
 
@@ -88,7 +98,7 @@ public class    ShooterConstants {
     
     
     
-    public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.08);
+    public static ConfigurableDouble shooter_kP = new ConfigurableDouble("Shooter KP", 0.02);
     public static ConfigurableDouble shooter_kI = new ConfigurableDouble("Shooter KI", 0.15);
     public static ConfigurableDouble shooter_kD = new ConfigurableDouble("Shooter KD", 0);
 

src/main/java/frc4388/robot/subsystems/shooter/ShooterIO.java

@@ -7,6 +7,8 @@ import org.littletonrobotics.junction.AutoLog;
 
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Current;
+import frc4388.robot.subsystems.intake.Intake;
+import frc4388.robot.subsystems.led.LED;
 
 public interface ShooterIO {
     @AutoLog
@@ -49,6 +51,7 @@ public interface ShooterIO {
 
     public default void updateInputs(ShooterState state) {}
 
+    public default void motorStalled(ShooterState state, Intake m_Intake, LED m_robotLED) {}
     
     public default void updateGains() {}
 }

src/main/java/frc4388/robot/subsystems/shooter/ShooterReal.java

@@ -3,11 +3,16 @@ package frc4388.robot.subsystems.shooter;
 import static edu.wpi.first.units.Units.Amps;
 import static edu.wpi.first.units.Units.RotationsPerSecond;
 
+import org.littletonrobotics.junction.Logger;
 import com.ctre.phoenix6.controls.VelocityDutyCycle;
 import com.ctre.phoenix6.hardware.TalonFX;
 
 import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.wpilibj.Timer;
+import frc4388.robot.constants.Constants;
+import frc4388.robot.subsystems.intake.Intake;
+import frc4388.robot.subsystems.led.LED;
 
 public class ShooterReal implements ShooterIO {
 
@@ -19,6 +24,14 @@ public class ShooterReal implements ShooterIO {
     VelocityDutyCycle shooter2Velocity = new VelocityDutyCycle(0);
     // VelocityDutyCycle m_indexerVelocity = new VelocityDutyCycle(0);
 
+    private final Timer m_stallTimerShooter = new Timer();
+    private final Timer m_stallTimerIndexer = new Timer();
+    private final Timer m_stallTimerRoller = new Timer();
+    private boolean m_shooterStalling = false;
+    private boolean m_indexerStalling = false;
+    private boolean m_rollerStalling = false;
+    public String motorStall = "";
+
 
     public ShooterReal(
          TalonFX shooter1Motor,
@@ -41,6 +54,59 @@ public class ShooterReal implements ShooterIO {
         // m_indexerVelocity.Slot = 0;
     }
     
+    @Override
+    public void motorStalled(ShooterState state, Intake m_Intake, LED m_robotLED) {    
+            motorStall = "";
+        if (Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) - Math.abs(state.motor1Velocity.in(RotationsPerSecond)) > 40) {
+            if (!m_shooterStalling) {
+                m_shooterStalling = true;
+                m_stallTimerShooter.restart();
+            }
+            if (m_stallTimerShooter.hasElapsed(5.0)) {
+                m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED);
+                motorStall = "Shooter Stalled";
+                System.out.println("Shooter Stalled: " + (Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)) - Math.abs(state.motor1Velocity.in(RotationsPerSecond))));
+                System.out.println("Target Velocity: " + Math.abs(state.motor1TargetVelocity.in(RotationsPerSecond)));
+                System.out.println("Actual Velocity: " + Math.abs(state.motor1Velocity.in(RotationsPerSecond)));
+            }
+        } else {
+            m_shooterStalling = false;
+            m_stallTimerShooter.reset();
+        }
+
+        if (Math.abs(state.indexerTargetOutput) - Math.abs(state.indexerOutput) > 0.3) {
+            if (!m_indexerStalling) {
+                m_indexerStalling = true;
+                m_stallTimerIndexer.restart();
+            }
+            if (m_stallTimerIndexer.hasElapsed(5.0)) {
+                m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED);
+                motorStall = "Indexer Stalled";
+                System.out.println("Indexer Stalled: " + (Math.abs(state.indexerTargetOutput) - Math.abs(state.indexerOutput)));
+            }
+        } else {
+            m_indexerStalling = false;
+            m_stallTimerIndexer.reset();
+        }
+
+        if (Math.abs(m_Intake.getRollerTarget()) - Math.abs(m_Intake.getRollerSpeed()) > 0.4) {
+            if (!m_rollerStalling) {
+                m_rollerStalling = true;
+                m_stallTimerRoller.restart();
+            }
+            if (m_stallTimerRoller.hasElapsed(5.0)) {
+                m_robotLED.setMode(Constants.LEDConstants.MOTOR_STALLED);
+                motorStall = "Roller Stalled";
+                System.out.println("Roller Stalled: " + (Math.abs(m_Intake.getRollerTarget()) - Math.abs(m_Intake.getRollerSpeed())));
+            }
+        } else {
+            m_rollerStalling = false;
+            m_stallTimerRoller.reset();
+        }
+        Logger.recordOutput("Stalled Motor: ", motorStall);
+        
+    }
+    
     @Override
     public void setShooterVelocity(ShooterState state, AngularVelocity target) {
         state.motor1TargetVelocity = target;
@@ -75,6 +141,8 @@ public class ShooterReal implements ShooterIO {
         //     Math.abs(currentLimit.in(Amps)) > current &&
         //     Math.abs(targetVelocity.in(RotationsPerSecond)) > velocity
         // ) {
+            state.motor1TargetVelocity = RotationsPerSecond.of(percentOutput);
+            state.motor2TargetVelocity = RotationsPerSecond.of(percentOutput);
             m_shooter1Motor.set(percentOutput);
             m_shooter2Motor.set(percentOutput);
         // } else {

src/main/java/frc4388/robot/subsystems/swerve/SimpleSwerveSim.java

@@ -0,0 +1,204 @@
+package frc4388.robot.subsystems.swerve;
+
+import java.util.List;
+
+import com.ctre.phoenix6.swerve.SwerveRequest;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.RobotBase;
+import frc4388.robot.subsystems.vision.VisionIO.PoseObservation;
+
+public class SimpleSwerveSim implements SwerveIO {
+    private Pose2d pose = new Pose2d();
+    private Pose2d lastPose = pose;
+    private double vx = 0.0;
+    private double vy = 0.0;
+    private double omega = 0.0;
+
+    private long lastTimeNs = System.nanoTime();
+
+    public SimpleSwerveSim() {
+    }
+
+    public synchronized void setControl(SwerveRequest ctrl) {
+        if (ctrl == null) return;
+
+        if (ctrl instanceof SwerveRequest.FieldCentricFacingAngle facingAngle) {
+        vx = facingAngle.VelocityX;
+        vy = facingAngle.VelocityY;
+
+        double currentAngle = pose.getRotation().getRadians();
+        double targetAngle  = facingAngle.TargetDirection.getRadians();
+        double error = targetAngle - currentAngle;
+
+            error = Math.atan2(Math.sin(error), Math.cos(error));
+
+            double kP = 5.0;
+            omega = error * kP;
+            return;
+        }
+
+        if (ctrl instanceof SwerveRequest.FieldCentric fc) {
+            vx = fc.VelocityX;
+            vy = fc.VelocityY;
+            omega = fc.RotationalRate;
+            return;
+        }
+
+        if (ctrl instanceof SwerveRequest.RobotCentric rc) {
+            double cos = pose.getRotation().getCos();
+            double sin = pose.getRotation().getSin();
+            double vxRobot = rc.VelocityX;
+            double vyRobot = rc.VelocityY;
+            vx = vxRobot * cos - vyRobot * sin;
+            vy = vxRobot * sin + vyRobot * cos;
+            omega = rc.RotationalRate;
+            return;
+        }
+
+        if (ctrl instanceof SwerveRequest.SwerveDriveBrake) {
+            vx = 0; vy = 0; omega = 0;
+            return;
+        }
+
+        ChassisSpeeds cs = tryGetSpeedsField(ctrl);
+        if (cs != null) {
+            vx = cs.vxMetersPerSecond;
+            vy = cs.vyMetersPerSecond;
+            omega = cs.omegaRadiansPerSecond;
+        }
+    }
+
+    private ChassisSpeeds tryGetSpeedsField(SwerveRequest ctrl) {
+        try {
+            java.lang.reflect.Field f = ctrl.getClass().getDeclaredField("Speeds");
+            f.setAccessible(true);
+            Object o = f.get(ctrl);
+            if (o instanceof ChassisSpeeds) {
+                return (ChassisSpeeds) o;
+            }
+        } catch (NoSuchFieldException nsf) {
+        } catch (IllegalAccessException iae) {
+        } catch (SecurityException se) {
+        }
+        return null;
+    }
+
+    private double tryGetDoubleField(Object obj, Class<?> cls, String... names) {
+        for (String n : names) {
+            try {
+                java.lang.reflect.Field f = cls.getDeclaredField(n);
+                f.setAccessible(true);
+                Object val = f.get(obj);
+                if (val instanceof Number) {
+                    return ((Number) val).doubleValue();
+                }
+            } catch (NoSuchFieldException nsf) {
+            } catch (IllegalAccessException iae) {
+            } catch (Throwable t) {
+            }
+        }
+        return 0.0;
+    }
+
+    
+    public synchronized void pointAtXY(double x, double y) {
+        Translation2d target = new Translation2d(x, y);
+        Translation2d toTarget = target.minus(pose.getTranslation());
+        if (toTarget.getNorm() < 1e-9) return;
+        Rotation2d desired = toTarget.getAngle();
+        pose = new Pose2d(pose.getTranslation(), desired);
+        lastPose = pose;
+        vx = 0.0;
+        vy = 0.0;
+        omega = 0.0;
+    }
+
+    @Override
+    public synchronized void updateInputs(SwerveState state) {
+        if (state == null) return;
+
+        long now = System.nanoTime();
+        double dt = Math.max(1e-6, (now - lastTimeNs) / 1.0e9);
+        lastTimeNs = now;
+
+        lastPose = pose;
+        double dxField;
+        double dyField;
+        
+        if (DriverStation.isAutonomous()) {
+            double dxRobot = vx * dt;
+            double dyRobot = vy * dt;
+
+            Rotation2d r = pose.getRotation();
+            double cos = r.getCos();
+            double sin = r.getSin();
+
+            dxField = dxRobot * cos - dyRobot * sin;
+            dyField = dxRobot * sin + dyRobot * cos;
+
+        } else {
+            dxField = vx * dt;
+            dyField = vy * dt;
+
+        }
+
+
+        double dTheta = omega * dt;
+
+        Translation2d newTrans = pose.getTranslation().plus(new Translation2d(dxField, dyField));
+        Rotation2d newRot = pose.getRotation().plus(Rotation2d.fromRadians(dTheta));
+        pose = new Pose2d(newTrans, newRot);
+
+        state.lastPose = lastPose;
+        state.currentPose = pose;
+        state.speeds = new ChassisSpeeds(vx, vy, omega);
+        state.odometryRate = dt;
+    }
+
+    @Override
+    public synchronized void resetPose(Pose2d p) {
+        if (p == null) return;
+        pose = p;
+        lastPose = p;
+        lastTimeNs = System.nanoTime();
+    }
+
+    @Override
+    public synchronized void tareEverything() {
+        pose = new Pose2d();
+        lastPose = pose;
+        vx = 0.0;
+        vy = 0.0;
+        omega = 0.0;
+        lastTimeNs = System.nanoTime();
+    }
+
+    @Override
+    public synchronized void addVisionMeasurement(List<PoseObservation> poses) {
+        if (poses == null || poses.isEmpty()) return;
+        Pose2d visPose = poses.get(poses.size() - 1).pose();
+        if (visPose != null) {
+            pose = visPose;
+            lastPose = visPose;
+        }
+    }
+
+    public synchronized void pointAt(Translation2d target) {
+        if (target == null) return;
+        Translation2d toTarget = target.minus(pose.getTranslation());
+        if (toTarget.getNorm() < 1e-9) return;
+        Rotation2d desired = toTarget.getAngle();
+        pose = new Pose2d(pose.getTranslation(), desired);
+        lastPose = pose;
+        omega = 0.0;
+    }
+
+    @Override
+    public synchronized void setLimits(double limitInAmps) {
+    }
+}

src/main/java/frc4388/robot/subsystems/swerve/SwerveDrive.java

@@ -5,7 +5,9 @@
 package frc4388.robot.subsystems.swerve;
 
 import static edu.wpi.first.units.Units.Rotations;
+import static edu.wpi.first.units.Units.RotationsPerSecond;
 
+import java.util.Optional;
 import java.util.function.Supplier;
 
 import org.littletonrobotics.junction.AutoLogOutput;
@@ -19,10 +21,13 @@ import com.pathplanner.lib.controllers.PPHolonomicDriveController;
 import com.pathplanner.lib.trajectory.PathPlannerTrajectory;
 import com.pathplanner.lib.util.PathPlannerLogging;
 
+import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.units.measure.AngularVelocity;
 import edu.wpi.first.wpilibj.smartdashboard.Field2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -63,6 +68,14 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
     public Rotation2d orientRotTarget = new Rotation2d();
     public ChassisSpeeds chassisSpeeds = new ChassisSpeeds();
 
+    private final PIDController m_rotationOverridePID = new PIDController(
+        SwerveDriveConstants.PIDConstants.AIM_kP.get(),
+        SwerveDriveConstants.PIDConstants.AIM_kI.get(),
+        SwerveDriveConstants.PIDConstants.AIM_kD.get()
+    );
+    private boolean m_useRotationOverride = false;
+    private Translation2d m_rotationOverrideTarget = new Translation2d();
+
     /** Creates a new SwerveDrive. */
     public SwerveDrive(SwerveIO swerveDriveTrain, Vision vision) {
         // public SwerveDrive(SwerveDrivetrain<TalonFX, TalonFX, CANcoder>
@@ -81,6 +94,20 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
             // Handle exception as needed
             config = null;
         }
+
+        PPHolonomicDriveController driveController = new PPHolonomicDriveController(
+            new PIDConstants(5.0, 0.0, 0.0), // Translation PID
+            new PIDConstants(5.0, 0.0, 0.0)  // Rotation PID (used when override is OFF)
+        );
+        driveController.setRotationTargetOverride(() -> {
+            if (!m_useRotationOverride) return Optional.empty();
+            Rotation2d targetAngle = getPose2d()
+                .getTranslation()
+                .minus(m_rotationOverrideTarget)
+                .getAngle();
+            return Optional.of(targetAngle);
+        });
+
         // DoubleSupplier a = () -> 1.d;
         AutoBuilder.configure(
                 () -> {
@@ -92,11 +119,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
                 (speeds, feedforwards) -> io.setControl(new SwerveRequest.ApplyRobotSpeeds()
                         .withSpeeds(speeds)), // Method that will drive the robot given ROBOT RELATIVE ChassisSpeeds.
                                               // Also optionally outputs individual module feedforwards
-                new PPHolonomicDriveController( // PPHolonomicController is the built in path following controller for
+                driveController, // <-- use the variable, not inline new PPHolonomicDriveController(...)
-                                                // holonomic drive trains
-                        new PIDConstants(5.0, 0.0, 0.0), // Translation PID constants
-                        new PIDConstants(5.0, 0.0, 0.0) // Rotation PID constants
-                ),
                 config, // The robot configuration
                 () -> {
                     // Boolean supplier that controls when the path will be mirrored for the red
@@ -148,14 +171,6 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
     public void setInitalPose(Pose2d startingAutoPose){
         initalPose2d = startingAutoPose;
     }
-    // MIRA public void setOdoPose(Pose2d pose) {
-    //     if (pose == null) return;
-    //     io.tareEverything();
-    //     initalPose2d = pose;
-    //     io.resetPose(pose);
-    //     robotKnowsWhereItIs = true;
-    //     rotTarget = pose.getRotation().getDegrees();
-    // }
 
 
     // public void oneModuleTest(SwerveModule module, Translation2d leftStick,
@@ -172,6 +187,14 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
     // module.setDesiredState(state);
     // }
 
+    public double chassisXSpeeds(){
+        if (TimesNegativeOne.isRed) {
+            return chassisSpeeds.vxMetersPerSecond;
+        } else {
+            return -chassisSpeeds.vxMetersPerSecond;
+        }
+    }
+
     public void driveWithInput(Translation2d leftStick, Translation2d rightStick, boolean fieldRelative) {
         if (rightStick.getNorm() < 0.05 && leftStick.getNorm() < 0.05 && stopped == false) // if no imput and the swerve drive is still going:
             stopModules(); // stop the swerve
@@ -233,6 +256,27 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         
     }
 
+    
+
+    public void aimAtPosition(Translation2d fieldPos, double aimLeadTime) {
+        Translation2d robotSpeed = new Translation2d(
+            chassisSpeeds.vxMetersPerSecond, 
+            chassisSpeeds.vyMetersPerSecond
+        );
+        Translation2d fieldPosLead = robotSpeed.times(aimLeadTime).plus(fieldPos);
+        Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle();
+
+        var ctrl = new SwerveRequest.FieldCentricFacingAngle()
+        .withVelocityX(chassisSpeeds.vxMetersPerSecond)
+        .withVelocityY(chassisSpeeds.vyMetersPerSecond)
+        .withTargetDirection(ang);
+        ctrl.HeadingController.setPID(
+        SwerveDriveConstants.PIDConstants.AIM_kP.get(),
+        SwerveDriveConstants.PIDConstants.AIM_kI.get(),
+        SwerveDriveConstants.PIDConstants.AIM_kD.get()
+        );
+        io.setControl(ctrl);
+    }
 
     public void driveWithInputOrientation(Translation2d leftStick, Translation2d rightStick) { // there is no practical
                                                                                                // reason to have a robot
@@ -301,7 +345,20 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         // SmartDashboard.putBoolean("drift correction", true);
     }
 
-    
+    public void driveFieldAngleSIP(Translation2d leftStick, Rotation2d heading) {
+        
+       rotTarget = heading.getDegrees();
+        var ctrl = new SwerveRequest.FieldCentricFacingAngle()
+            .withVelocityX(leftStick.getX() * speedAdjust)
+            .withVelocityY(leftStick.getY() * speedAdjust)
+            .withTargetDirection(heading);
+        ctrl.HeadingController.setPID(
+            SwerveDriveConstants.PIDConstants.AIM_kP.get(),
+            SwerveDriveConstants.PIDConstants.AIM_kI.get(),
+            SwerveDriveConstants.PIDConstants.AIM_kD.get()
+        );
+        io.setControl(ctrl);
+    }
     
     public void driveIntake(Translation2d leftStick, boolean invertRotation){
         // if (invert){
@@ -360,23 +417,57 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
 
     // Drive with the robot facing towards a specific position
     public void driveFacingPosition(Translation2d leftStick, Translation2d fieldPos, double aimLeadTime) {
-
+        Translation2d robotSpeedYOnly = new Translation2d(0, chassisSpeeds.vyMetersPerSecond);
-        // Get the current speed of the robot
+        double yDistance = Math.abs(getPose2d().getTranslation().getY() - fieldPos.getY());
-        Translation2d robotSpeed = new Translation2d(
+        if ((chassisSpeeds.vyMetersPerSecond >0 &&getPose2d().getTranslation().getY() >4)||(chassisSpeeds.vyMetersPerSecond <0 &&getPose2d().getTranslation().getY() <4)){
-            chassisSpeeds.vxMetersPerSecond, 
+        if (Math.abs(chassisSpeeds.vyMetersPerSecond) > 0.2) {
-            chassisSpeeds.vyMetersPerSecond
+            if (TimesNegativeOne.isRed){
-        );
+                    robotSpeedYOnly = new Translation2d(-SwerveDriveConstants.FAR_OFFSET.get() * yDistance * (getPose2d().getTranslation().getX()-7.28989525), chassisSpeeds.vyMetersPerSecond);
-
+            } else {
-        // Calculate a point to aim ahead of the actual position.
+                robotSpeedYOnly = new Translation2d((getPose2d().getTranslation().getX())* yDistance* SwerveDriveConstants.FAR_OFFSET.get(), chassisSpeeds.vyMetersPerSecond);
-        Translation2d fieldPosLead = robotSpeed.times(aimLeadTime).plus(fieldPos);
+            }
-
+        } }
-        // Calculate the angle between the current position and the lead position
+        Translation2d fieldPosLead = robotSpeedYOnly.times(aimLeadTime).plus(fieldPos);
         Rotation2d ang = getPose2d().getTranslation().minus(fieldPosLead).getAngle();
+        Pose2d fieldPosLeadLog = new Pose2d(fieldPosLead, new Rotation2d());
+        Logger.recordOutput("Lead Aim", fieldPosLeadLog);
+        driveFieldAngle(leftStick, ang);
+    }
 
+    public void offsetOdoPosition(Transform2d offset) {
+        // Manually performing an addittion on the pose
+        // WHY doesn't WPILIB have the ability to not transform poses
+        Pose2d new_pose = new Pose2d(
+            new Translation2d(
+                state.currentPose.getX() + offset.getX(),
+                state.currentPose.getY() + offset.getY()
+            ),
+            state.currentPose.getRotation()
+        );
+        this.io.resetPose(new_pose);
+    }
+
+    public void defenseXPosition(){
+        io.setControl(new SwerveRequest.SwerveDriveBrake());
+    }
+
+    public void stopDefenseXPosition(){
+        stopModules();
+    }
+
+    public void driveFacingPosition(Translation2d leftStick, Translation2d fieldPos) {
+        // Calculate the angle between the current position and the lead position
+        //Rotation2d ang = getPose2d().getTranslation().minus(fieldPos).getAngle();
+        Rotation2d ang = new Rotation2d(0,1);
+        System.out.println(ang);
 
         driveFieldAngle(leftStick, ang);
     }
 
+    public Pose2d getCurrentPose(){
+        return state.currentPose;
+    }
+
     public void driveRelativeLockedAngle(Translation2d leftStick, Rotation2d heading) {
         leftStick = leftStick.rotateBy(heading);
 
@@ -464,6 +555,18 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         softStop();
     }
 
+    public void enableRotationOverride(Translation2d fieldTarget, double aimLeadTime, Translation2d fieldPos) {
+        Translation2d robotSpeedYOnly = new Translation2d(0, chassisSpeeds.vyMetersPerSecond);
+
+        Translation2d fieldPosLead = robotSpeedYOnly.times(aimLeadTime).plus(fieldPos);
+        m_rotationOverrideTarget = fieldPosLead;
+        m_useRotationOverride = true;
+    }
+
+    public void disableRotationOverride() {
+        m_useRotationOverride = false;
+    }
+
     @Override
     public void periodic() {
         // This method will be called once per scheduler run\
@@ -476,6 +579,12 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         vision.setLastOdomPose(state.currentPose);
         setLastOdomSpeed(state.currentPose, state.lastPose, state.odometryRate);
 
+        if (state.speeds != null) {
+            this.chassisSpeeds = state.speeds;
+        } else {
+            this.chassisSpeeds = new ChassisSpeeds();
+        }
+        
         if (vision.isTag()) {
             Pose2d pose = vision.getPose2d();
             if (!robotKnowsWhereItIs) {
@@ -485,6 +594,11 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
             }
 
             io.addVisionMeasurement(vision.getPosesToAdd());
+            io.updateInputs(state);
+            Logger.processInputs("SwerveDrive", state);
+
+            vision.setLastOdomPose(state.currentPose);
+            setLastOdomSpeed(state.currentPose, state.lastPose, state.odometryRate);
         }
 
         // if(e.isPresent())
@@ -515,6 +629,7 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
         gear_index = i;
     }
 
+
     public void setPercentOutput(double speed) {
         speedAdjust = SwerveDriveConstants.MAX_SPEED_MEETERS_PER_SEC * speed;
         gear_index = -1;
@@ -589,5 +704,4 @@ public class SwerveDrive extends SubsystemBase implements Queryable {
 
         return status;
     }
-}
+}
-

src/main/java/frc4388/robot/subsystems/swerve/SwerveDriveConstants.java

@@ -73,9 +73,10 @@ public final class SwerveDriveConstants {
     public static final boolean INVERT_Y = true;
     public static final boolean INVERT_ROTATION = false;
 
+    public static ConfigurableDouble FAR_OFFSET = new ConfigurableDouble("Far Offset", 0.05);
     // public static final Trim POINTLESS_TRIM = new Trim("Pointless Trim", Double.MAX_VALUE, Double.MIN_VALUE, 0.1, 0);
 
-    private static final class ModuleSpecificConstants { //2025
+    public static final class ModuleSpecificConstants { //2026
         //Front Left
         private static final Angle FRONT_LEFT_ENCODER_OFFSET = Rotations.of(0.49707+0.350-0.03+0.0134+0.06-0.043);
         private static final boolean FRONT_LEFT_DRIVE_MOTOR_INVERTED = false;

src/main/java/frc4388/robot/subsystems/swerve/SwerveIO.java

@@ -7,8 +7,10 @@ import org.littletonrobotics.junction.AutoLog;
 import com.ctre.phoenix6.swerve.SwerveRequest;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.ChassisSpeeds;
 import frc4388.robot.subsystems.vision.VisionIO.PoseObservation;
+import frc4388.utility.status.CanDevice;
 
 public interface SwerveIO {
     @AutoLog

src/main/java/frc4388/robot/subsystems/swerve/SwerveReal.java

@@ -11,8 +11,10 @@ import com.ctre.phoenix6.swerve.SwerveModule;
 import com.ctre.phoenix6.swerve.SwerveRequest;
 
 import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
 import frc4388.robot.subsystems.vision.Vision;
 import frc4388.robot.subsystems.vision.VisionIO.PoseObservation;
+import frc4388.utility.status.CanDevice;
 
 public class SwerveReal implements SwerveIO {
     SwerveDrivetrain<TalonFX, TalonFX, CANcoder> swerveDriveTrain;
@@ -54,7 +56,7 @@ public class SwerveReal implements SwerveIO {
             swerveDriveTrain.addVisionMeasurement(pose, Utils.fpgaToCurrentTime(Vision.getTime()));
         }
     }
-
+    
     @Override
     public void setLimits(double limitInAmps) {
         for (SwerveModule<TalonFX, TalonFX, CANcoder> module : swerveDriveTrain.getModules()) {

src/main/java/frc4388/robot/subsystems/vision/Lidar.java

@@ -1,4 +1,4 @@
-package frc4388.robot.subsystems;
+package frc4388.robot.subsystems.vision;
 
 import java.util.ArrayList;
 import java.util.LinkedList;
@@ -20,8 +20,8 @@ import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import frc4388.robot.subsystems.RPLidarA1.PolarPoint;
+import frc4388.robot.subsystems.vision.RPLidarA1.PolarPoint;
-import frc4388.robot.subsystems.RPLidarA1.ScanListener;
+import frc4388.robot.subsystems.vision.RPLidarA1.ScanListener;
 import frc4388.utility.configurable.ConfigurableDouble;
 import frc4388.utility.status.FaultA1M8;
 

src/main/java/frc4388/robot/subsystems/vision/RPLidarA1.java

@@ -1,4 +1,4 @@
-package frc4388.robot.subsystems;
+package frc4388.robot.subsystems.vision;
 
 import com.fazecast.jSerialComm.SerialPort;
 import edu.wpi.first.wpilibj.DriverStation;

src/main/java/frc4388/utility/compute/HubShiftTimer.java

@@ -0,0 +1,154 @@
+
+package frc4388.utility.compute;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.Timer;
+public class HubShiftTimer {
+
+  public enum ShiftPhase {
+    DISABLED,
+    AUTO,
+    TRANSITION, // 0 – 10 s
+    SHIFT1,     // 10 – 35 s
+    SHIFT2,     // 35 – 60 s
+    SHIFT3,     // 60 – 85 s
+    SHIFT4,     // 85 – 110 s
+    ENDGAME     // 110 – 140 s
+  }
+
+  public record ShiftInfo(
+      ShiftPhase phase,
+      double elapsedInShift,
+      double remainingInShift,
+      boolean isActive) {}
+//total teleop time
+  public static final double TELEOP_DURATION = 140.0;
+//total auto time
+  public static final double AUTO_DURATION = 20.0;
+
+//shift start and end times for calculations
+  private static final double[] SHIFT_STARTS = {0.0,  10.0, 35.0, 60.0,  85.0, 110.0};
+  private static final double[] SHIFT_ENDS   = {10.0, 35.0, 60.0, 85.0, 110.0, 140.0};
+
+//hub active schedule, true is active and false is inactive
+//starts always as active becasue transition is first and is active, but then is inactive for winner or active for loser
+  private static final boolean[] WINNER_SCHEDULE = {true,  false, true,  false, true,  true};
+  private static final boolean[] LOSER_SCHEDULE  = {true,  true,  false, true,  false, true};
+
+//shift phase names during teleop
+  private static final ShiftPhase[] SHIFT_PHASES = {
+    ShiftPhase.TRANSITION,
+    ShiftPhase.SHIFT1,
+    ShiftPhase.SHIFT2,
+    ShiftPhase.SHIFT3,
+    ShiftPhase.SHIFT4,
+    ShiftPhase.ENDGAME
+  };
+
+//timer to track time
+  private static final Timer teleopTimer = new Timer();
+  private static double timerOffset = 0.0;
+
+//fms syncing idk other team did it too
+  private static final double RESYNC_THRESHOLD = 3.0;
+
+
+//call at start of auto to start timer 
+  public static void initializeAuto() {
+    teleopTimer.restart();
+  }
+  
+//call at start of teleop to start timer again because sometimes delay between auto and telop
+  public static void initializeTeleop() {
+    timerOffset = 0.0;
+    teleopTimer.restart();
+  }
+
+
+
+//returns the updated shift info based on the winner of auto
+  public static ShiftInfo getShiftInfo() {
+    if (!DriverStation.isEnabled()) {
+      return new ShiftInfo(ShiftPhase.DISABLED, 0.0, 0.0, false);
+    }
+    if (DriverStation.isAutonomousEnabled()) {
+      double autoElapsed = teleopTimer.get(); // timer restarts in initialize()
+      return new ShiftInfo(
+          ShiftPhase.AUTO,
+          autoElapsed,
+          Math.max(0.0, AUTO_DURATION - teleopTimer.get()),
+          true);
+    }
+    return computeTeleopShift();
+  }
+
+//find auto winner, R = red wins, B = blue wins
+  public static Alliance autoWinnerAlliance() {
+    String msg = DriverStation.getGameSpecificMessage();
+    if (msg != null && msg.length() > 0) {
+      char c = msg.charAt(0);
+      if (c == 'R') return Alliance.Red;
+      if (c == 'B') return Alliance.Blue;
+    }
+    // backup if no msg, returns auto winner as opposite of our alliance. if we red -> blue wins auto
+    Alliance ours = DriverStation.getAlliance().orElse(Alliance.Blue);
+    return (ours == Alliance.Blue) ? Alliance.Red : Alliance.Blue;
+  }
+
+
+  //return our schedule for the shifts
+  private static boolean[] getSchedule() {
+    Alliance ours   = DriverStation.getAlliance().orElse(Alliance.Blue);
+    Alliance winner = autoWinnerAlliance();
+    return (ours == winner) ? WINNER_SCHEDULE : LOSER_SCHEDULE;
+  }
+
+//time since start of teleop
+  private static double getTeleopElapsed() {
+    double localTime = teleopTimer.get() - timerOffset;
+
+    // Re-sync to FMS time if we drift too far (only when FMS is attached)
+    if (DriverStation.isFMSAttached()) {
+      double fmsElapsed = TELEOP_DURATION - DriverStation.getMatchTime();
+      if (fmsElapsed <= TELEOP_DURATION - 5.0 // ignore the first few seconds of jitter
+          && Math.abs(localTime - fmsElapsed) >= RESYNC_THRESHOLD) {
+        timerOffset += localTime - fmsElapsed;
+        localTime = fmsElapsed;
+      }
+    }
+    return Math.max(0.0, Math.min(TELEOP_DURATION, localTime));
+  }
+
+  private static ShiftInfo computeTeleopShift() {
+    boolean[] schedule = getSchedule();
+    double elapsed = getTeleopElapsed();
+
+    // Find which shift we're in
+    int phaseIndex = SHIFT_STARTS.length - 1; // default to last shift if past all bounds
+    for (int i = 0; i < SHIFT_STARTS.length; i++) {
+      if (elapsed >= SHIFT_STARTS[i] && elapsed < SHIFT_ENDS[i]) {
+        phaseIndex = i;
+        break;
+      }
+    }
+
+    double shiftElapsed   = elapsed - SHIFT_STARTS[phaseIndex];
+    double shiftRemaining = SHIFT_ENDS[phaseIndex] - elapsed;
+
+    // merge time for elapsed if same active/inactive
+    if (phaseIndex > 0 && schedule[phaseIndex] == schedule[phaseIndex - 1]) {
+      shiftElapsed = elapsed - SHIFT_STARTS[phaseIndex - 1];
+    }
+
+    // merge time for remaining time if same active/inactive status 
+    if (phaseIndex < SHIFT_ENDS.length - 1 && schedule[phaseIndex] == schedule[phaseIndex + 1]) {
+      shiftRemaining = SHIFT_ENDS[phaseIndex + 1] - elapsed;
+    }
+
+    return new ShiftInfo(
+        SHIFT_PHASES[phaseIndex],
+        shiftElapsed,
+        shiftRemaining,
+        schedule[phaseIndex]);
+  }
+}

src/main/java/frc4388/utility/status/CanDevice.java

@@ -49,5 +49,10 @@ public class CanDevice {
         return s;
     }
 
+
+    public Object getId() {
+        return id;        
+    }
+
     
 }

src/main/java/frc4388/utility/status/FaultA1M8.java

@@ -1,7 +1,7 @@
 package frc4388.utility.status;
 
-import frc4388.robot.subsystems.RPLidarA1;
+import frc4388.robot.subsystems.vision.RPLidarA1;
-import frc4388.robot.subsystems.RPLidarA1.ConnectionStatus;
+import frc4388.robot.subsystems.vision.RPLidarA1.ConnectionStatus;
 import frc4388.utility.status.Status.ReportLevel;
 
 // Fault reporter for the RPLidar A1M8 Revolving lidar sensor