Team100 · truher · Dec 27, 2025 · Dec 27, 2025 · Dec 27, 2025 · Dec 27, 2025
diff --git a/comp/src/main/java/org/team100/frc2025/CalgamesArm/CalgamesMech.java b/comp/src/main/java/org/team100/frc2025/CalgamesArm/CalgamesMech.java
@@ -17,13 +17,13 @@
 import org.team100.lib.geometry.WaypointSE2;
 import org.team100.lib.logging.Level;
 import org.team100.lib.logging.LoggerFactory;
-import org.team100.lib.logging.LoggerFactory.ConfigLogger;
 import org.team100.lib.logging.LoggerFactory.AccelerationSE2Logger;
-import org.team100.lib.logging.LoggerFactory.VelocitySE2Logger;
+import org.team100.lib.logging.LoggerFactory.ConfigLogger;
 import org.team100.lib.logging.LoggerFactory.JointAccelerationsLogger;
 import org.team100.lib.logging.LoggerFactory.JointForceLogger;
 import org.team100.lib.logging.LoggerFactory.JointVelocitiesLogger;
 import org.team100.lib.logging.LoggerFactory.Pose2dLogger;
+import org.team100.lib.logging.LoggerFactory.VelocitySE2Logger;
 import org.team100.lib.mechanism.LinearMechanism;
 import org.team100.lib.mechanism.RotaryMechanism;
 import org.team100.lib.motor.MotorPhase;
@@ -41,8 +41,8 @@
 import org.team100.lib.sensor.position.absolute.wpi.AS5048RotaryPositionSensor;
 import org.team100.lib.sensor.position.incremental.IncrementalBareEncoder;
 import org.team100.lib.sensor.position.incremental.ctre.Talon6Encoder;
-import org.team100.lib.state.ControlR3;
-import org.team100.lib.state.ModelR3;
+import org.team100.lib.state.ControlSE2;
+import org.team100.lib.state.ModelSE2;
 import org.team100.lib.subsystems.prr.AnalyticalJacobian;
 import org.team100.lib.subsystems.prr.EAWConfig;
 import org.team100.lib.subsystems.prr.ElevatorArmWristKinematics;
@@ -51,7 +51,7 @@
 import org.team100.lib.subsystems.prr.JointVelocities;
 import org.team100.lib.subsystems.prr.SubsystemPRR;
 import org.team100.lib.subsystems.prr.commands.FollowJointProfiles;
-import org.team100.lib.subsystems.r3.PositionSubsystemR3;
+import org.team100.lib.subsystems.se2.PositionSubsystemSE2;
 import org.team100.lib.util.CanId;
 import org.team100.lib.util.RoboRioChannel;
 import org.team100.lib.util.StrUtil;
@@ -61,7 +61,7 @@
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
-public class CalgamesMech extends SubsystemBase implements Music, PositionSubsystemR3, SubsystemPRR {
+public class CalgamesMech extends SubsystemBase implements Music, PositionSubsystemSE2, SubsystemPRR {
     private static final boolean DEBUG = false;
     private boolean DISABLED = false;
     ////////////////////////////////////////////////////////
@@ -313,19 +313,19 @@ public JointVelocities getJointVelocity() {
     }
 
     @Override
-    public ModelR3 getState() {
+    public ModelSE2 getState() {
         EAWConfig c = getConfig();
         JointVelocities jv = getJointVelocity();
         Pose2d p = m_kinematics.forward(c);
         VelocitySE2 v = m_jacobian.forward(c, jv);
-        return new ModelR3(p, v);
+        return new ModelSE2(p, v);
     }
 
     // for testing only
     public void setVelocity(VelocitySE2 v) {
         Pose2d pose = getState().pose();
         AccelerationSE2 a = new AccelerationSE2(0, 0, 0);
-        ControlR3 control = new ControlR3(pose, v, a);
+        ControlSE2 control = new ControlSE2(pose, v, a);
 
         JointVelocities jv = m_jacobian.inverse(control.model());
         JointAccelerations ja = m_jacobian.inverseA(control);
@@ -343,7 +343,7 @@ public void setVelocity(VelocitySE2 v) {
 
     /** There are no profiles here, so this control needs to be feasible. */
     @Override
-    public void set(ControlR3 control) {
+    public void set(ControlSE2 control) {
         Pose2d pose = control.pose();
         EAWConfig config = m_kinematics.inverse(pose);
         if (DEBUG) {

diff --git a/comp/src/main/java/org/team100/frc2025/CalgamesArm/MechTrajectories.java b/comp/src/main/java/org/team100/frc2025/CalgamesArm/MechTrajectories.java
@@ -8,13 +8,13 @@
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.subsystems.prr.AnalyticalJacobian;
 import org.team100.lib.subsystems.prr.ElevatorArmWristKinematics;
-import org.team100.lib.subsystems.r3.commands.GoToPosePosition;
+import org.team100.lib.subsystems.se2.commands.GoToPosePosition;
 import org.team100.lib.trajectory.TrajectoryPlanner;
 import org.team100.lib.trajectory.path.PathFactory;
 import org.team100.lib.trajectory.timing.ConstantConstraint;
-import org.team100.lib.trajectory.timing.TrajectoryFactory;
 import org.team100.lib.trajectory.timing.TimingConstraint;
 import org.team100.lib.trajectory.timing.TorqueConstraint;
+import org.team100.lib.trajectory.timing.TrajectoryFactory;
 import org.team100.lib.trajectory.timing.YawRateConstraint;
 
 import edu.wpi.first.wpilibj2.command.Command;

diff --git a/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreCoralSmart.java b/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreCoralSmart.java
@@ -12,11 +12,11 @@
 import org.team100.frc2025.CalgamesArm.CalgamesMech;
 import org.team100.frc2025.grip.Manipulator;
 import org.team100.lib.config.ElevatorUtil.ScoringLevel;
-import org.team100.lib.controller.r3.ControllerR3;
+import org.team100.lib.controller.se2.ControllerSE2;
 import org.team100.lib.field.FieldConstants;
 import org.team100.lib.field.FieldConstants.ReefPoint;
 import org.team100.lib.logging.LoggerFactory;
-import org.team100.lib.profile.r3.ProfileR3;
+import org.team100.lib.profile.se2.ProfileSE2;
 import org.team100.lib.subsystems.swerve.SwerveDriveSubsystem;
 
 import edu.wpi.first.math.geometry.Pose2d;
@@ -30,8 +30,8 @@ public static Command get(
             LoggerFactory logger,
             CalgamesMech mech,
             Manipulator manipulator,
-            ControllerR3 controller,
-            ProfileR3 profile,
+            ControllerSE2 controller,
+            ProfileSE2 profile,
             SwerveDriveSubsystem drive,
             DoubleConsumer heedRadiusM,
             Supplier<ScoringLevel> level,

diff --git a/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreCoralSmartLuke.java b/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreCoralSmartLuke.java
@@ -9,11 +9,11 @@
 import org.team100.frc2025.CalgamesArm.CalgamesMech;
 import org.team100.frc2025.grip.Manipulator;
 import org.team100.lib.config.ElevatorUtil.ScoringLevel;
-import org.team100.lib.controller.r3.ControllerR3;
+import org.team100.lib.controller.se2.ControllerSE2;
 import org.team100.lib.field.FieldConstants.ReefPoint;
 import org.team100.lib.field.FieldConstantsLuke;
 import org.team100.lib.logging.LoggerFactory;
-import org.team100.lib.profile.r3.ProfileR3;
+import org.team100.lib.profile.se2.ProfileSE2;
 import org.team100.lib.subsystems.swerve.SwerveDriveSubsystem;
 
 import edu.wpi.first.math.geometry.Pose2d;
@@ -27,8 +27,8 @@ public static Command get(
             LoggerFactory logger,
             CalgamesMech mech,
             Manipulator manipulator,
-            ControllerR3 controller,
-            ProfileR3 profile,
+            ControllerSE2 controller,
+            ProfileSE2 profile,
             SwerveDriveSubsystem drive,
             DoubleConsumer heedRadiusM,
             Supplier<ScoringLevel> level,

diff --git a/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreL1Smart.java b/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreL1Smart.java
@@ -9,10 +9,10 @@
 import org.team100.frc2025.CalgamesArm.CalgamesMech;
 import org.team100.frc2025.grip.Manipulator;
 import org.team100.lib.commands.MoveAndHold;
-import org.team100.lib.controller.r3.ControllerR3;
+import org.team100.lib.controller.se2.ControllerSE2;
 import org.team100.lib.logging.LoggerFactory;
-import org.team100.lib.profile.r3.ProfileR3;
-import org.team100.lib.subsystems.r3.commands.DriveToPoseWithProfile;
+import org.team100.lib.profile.se2.ProfileSE2;
+import org.team100.lib.subsystems.se2.commands.DriveToPoseWithProfile;
 import org.team100.lib.subsystems.swerve.SwerveDriveSubsystem;
 
 import edu.wpi.first.math.geometry.Pose2d;
@@ -23,8 +23,8 @@ public static Command get(
             LoggerFactory logger,
             CalgamesMech mech,
             Manipulator manipulator,
-            ControllerR3 controller,
-            ProfileR3 profile,
+            ControllerSE2 controller,
+            ProfileSE2 profile,
             SwerveDriveSubsystem drive,
             Supplier<Pose2d> goal) {
         DriveToPoseWithProfile toReef = new DriveToPoseWithProfile(

diff --git a/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreL2Smart.java b/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreL2Smart.java
@@ -9,10 +9,10 @@
 import org.team100.frc2025.CalgamesArm.CalgamesMech;
 import org.team100.frc2025.grip.Manipulator;
 import org.team100.lib.commands.MoveAndHold;
-import org.team100.lib.controller.r3.ControllerR3;
+import org.team100.lib.controller.se2.ControllerSE2;
 import org.team100.lib.logging.LoggerFactory;
-import org.team100.lib.profile.r3.ProfileR3;
-import org.team100.lib.subsystems.r3.commands.DriveToPoseWithProfile;
+import org.team100.lib.profile.se2.ProfileSE2;
+import org.team100.lib.subsystems.se2.commands.DriveToPoseWithProfile;
 import org.team100.lib.subsystems.swerve.SwerveDriveSubsystem;
 
 import edu.wpi.first.math.geometry.Pose2d;
@@ -23,8 +23,8 @@ public static Command get(
             LoggerFactory logger,
             CalgamesMech mech,
             Manipulator manipulator,
-            ControllerR3 controller,
-            ProfileR3 profile,
+            ControllerSE2 controller,
+            ProfileSE2 profile,
             SwerveDriveSubsystem drive,
             Supplier<Pose2d> goal) {
         DriveToPoseWithProfile toReef = new DriveToPoseWithProfile(

diff --git a/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreL3Smart.java b/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreL3Smart.java
@@ -9,10 +9,10 @@
 import org.team100.frc2025.CalgamesArm.CalgamesMech;
 import org.team100.frc2025.grip.Manipulator;
 import org.team100.lib.commands.MoveAndHold;
-import org.team100.lib.controller.r3.ControllerR3;
+import org.team100.lib.controller.se2.ControllerSE2;
 import org.team100.lib.logging.LoggerFactory;
-import org.team100.lib.profile.r3.ProfileR3;
-import org.team100.lib.subsystems.r3.commands.DriveToPoseWithProfile;
+import org.team100.lib.profile.se2.ProfileSE2;
+import org.team100.lib.subsystems.se2.commands.DriveToPoseWithProfile;
 import org.team100.lib.subsystems.swerve.SwerveDriveSubsystem;
 
 import edu.wpi.first.math.geometry.Pose2d;
@@ -23,8 +23,8 @@ public static Command get(
             LoggerFactory logger,
             CalgamesMech mech,
             Manipulator manipulator,
-            ControllerR3 controller,
-            ProfileR3 profile,
+            ControllerSE2 controller,
+            ProfileSE2 profile,
             SwerveDriveSubsystem m_drive,
             Supplier<Pose2d> goal) {
         DriveToPoseWithProfile toReef = new DriveToPoseWithProfile(

diff --git a/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreL4Smart.java b/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreL4Smart.java
@@ -9,10 +9,10 @@
 import org.team100.frc2025.CalgamesArm.CalgamesMech;
 import org.team100.frc2025.grip.Manipulator;
 import org.team100.lib.commands.MoveAndHold;
-import org.team100.lib.controller.r3.ControllerR3;
+import org.team100.lib.controller.se2.ControllerSE2;
 import org.team100.lib.logging.LoggerFactory;
-import org.team100.lib.profile.r3.ProfileR3;
-import org.team100.lib.subsystems.r3.commands.DriveToPoseWithProfile;
+import org.team100.lib.profile.se2.ProfileSE2;
+import org.team100.lib.subsystems.se2.commands.DriveToPoseWithProfile;
 import org.team100.lib.subsystems.swerve.SwerveDriveSubsystem;
 
 import edu.wpi.first.math.geometry.Pose2d;
@@ -23,8 +23,8 @@ public static Command get(
             LoggerFactory logger,
             CalgamesMech mech,
             Manipulator manipulator,
-            ControllerR3 controller,
-            ProfileR3 profile,
+            ControllerSE2 controller,
+            ProfileSE2 profile,
             SwerveDriveSubsystem m_drive,
             Supplier<Pose2d> goal) {
         DriveToPoseWithProfile toReef = new DriveToPoseWithProfile(

diff --git a/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreL4SmartBack.java b/comp/src/main/java/org/team100/frc2025/CommandGroups/ScoreSmart/ScoreL4SmartBack.java
@@ -9,10 +9,10 @@
 import org.team100.frc2025.CalgamesArm.CalgamesMech;
 import org.team100.frc2025.grip.Manipulator;
 import org.team100.lib.commands.MoveAndHold;
-import org.team100.lib.controller.r3.ControllerR3;
+import org.team100.lib.controller.se2.ControllerSE2;
 import org.team100.lib.logging.LoggerFactory;
-import org.team100.lib.profile.r3.ProfileR3;
-import org.team100.lib.subsystems.r3.commands.DriveToPoseWithProfile;
+import org.team100.lib.profile.se2.ProfileSE2;
+import org.team100.lib.subsystems.se2.commands.DriveToPoseWithProfile;
 import org.team100.lib.subsystems.swerve.SwerveDriveSubsystem;
 
 import edu.wpi.first.math.geometry.Pose2d;
@@ -23,8 +23,8 @@ public static Command get(
             LoggerFactory logger,
             CalgamesMech mech,
             Manipulator manipulator,
-            ControllerR3 controller,
-            ProfileR3 profile,
+            ControllerSE2 controller,
+            ProfileSE2 profile,
             SwerveDriveSubsystem m_drive,
             Supplier<Pose2d> goal) {
         DriveToPoseWithProfile toReef = new DriveToPoseWithProfile(

diff --git a/comp/src/main/java/org/team100/frc2025/Swerve/ManualWithBargeAssist.java b/comp/src/main/java/org/team100/frc2025/Swerve/ManualWithBargeAssist.java
@@ -14,7 +14,7 @@
 import org.team100.lib.profile.incremental.TrapezoidIncrementalProfile;
 import org.team100.lib.state.Control100;
 import org.team100.lib.state.Model100;
-import org.team100.lib.state.ModelR3;
+import org.team100.lib.state.ModelSE2;
 import org.team100.lib.subsystems.swerve.commands.manual.FieldRelativeDriver;
 import org.team100.lib.subsystems.swerve.commands.manual.HeadingLatch;
 import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics;
@@ -90,7 +90,7 @@ public ManualWithBargeAssist(
     }
 
     @Override
-    public void reset(ModelR3 state) {
+    public void reset(ModelSE2 state) {
         m_thetaSetpoint = state.theta().control();
         m_goal = null;
         m_latch.unlatch();
@@ -113,7 +113,7 @@ public void reset(ModelR3 state) {
      */
     @Override
     public VelocitySE2 apply(
-            final ModelR3 state,
+            final ModelSE2 state,
             final Velocity twist1_1) {
         final VelocitySE2 control = clipAndScale(twist1_1);
 

diff --git a/comp/src/main/java/org/team100/frc2025/Swerve/ManualWithProfiledReefLock.java b/comp/src/main/java/org/team100/frc2025/Swerve/ManualWithProfiledReefLock.java
@@ -15,7 +15,7 @@
 import org.team100.lib.profile.incremental.TrapezoidIncrementalProfile;
 import org.team100.lib.state.Control100;
 import org.team100.lib.state.Model100;
-import org.team100.lib.state.ModelR3;
+import org.team100.lib.state.ModelSE2;
 import org.team100.lib.subsystems.swerve.commands.manual.FieldRelativeDriver;
 import org.team100.lib.subsystems.swerve.kinodynamics.SwerveKinodynamics;
 import org.team100.lib.util.Math100;
@@ -82,7 +82,7 @@ public ManualWithProfiledReefLock(
     }
 
     @Override
-    public void reset(ModelR3 state) {
+    public void reset(ModelSE2 state) {
         m_thetaSetpoint = state.theta().control();
         m_thetaFeedback.reset();
     }
@@ -103,7 +103,7 @@ public void reset(ModelR3 state) {
      */
     @Override
     public VelocitySE2 apply(
-            final ModelR3 state,
+            final ModelSE2 state,
             final Velocity twist1_1) {
         final VelocitySE2 control = clipAndScale(twist1_1);
 

diff --git a/comp/src/main/java/org/team100/frc2025/indicator/LEDIndicator.java b/comp/src/main/java/org/team100/frc2025/indicator/LEDIndicator.java
@@ -4,6 +4,7 @@
 import org.team100.frc2025.grip.Manipulator;
 import org.team100.lib.coherence.Takt;
 import org.team100.lib.localization.AprilTagRobotLocalizer;
+import org.team100.lib.localization.NudgingVisionUpdater;
 
 import edu.wpi.first.wpilibj.AddressableLED;
 import edu.wpi.first.wpilibj.AddressableLEDBuffer;
@@ -42,7 +43,7 @@ public class LEDIndicator {
     private final AddressableLEDBuffer m_blackBuffer;
     private final AddressableLEDBuffer m_whiteBuffer;
 
-    private final AprilTagRobotLocalizer m_localizer;
+    private final NudgingVisionUpdater m_updater;
     private final Manipulator m_manipulator;
     private final ClimberIntake m_climberIntake;
 
@@ -55,7 +56,7 @@ public class LEDIndicator {
      * Since this has logic about the 2025 game it should be in the "comp" project.
      */
     public LEDIndicator(
-            AprilTagRobotLocalizer localizer,
+            NudgingVisionUpdater updater,
             Manipulator manipulator,
             ClimberIntake climberIntake) {
         m_led = new AddressableLED(0);
@@ -68,7 +69,7 @@ public LEDIndicator(
         m_blackBuffer = fill(Color.kBlack);
         m_led.setData(m_redBuffer);
         m_led.start();
-        m_localizer = localizer;
+        m_updater = updater;
         m_manipulator = manipulator;
         m_climberIntake = climberIntake;
     }
@@ -79,7 +80,7 @@ public LEDIndicator(
     public void periodic() {
 
         if (RobotState.isDisabled()) {
-            if (m_localizer.getPoseAgeSec() < 1) {
+            if (m_updater.getPoseAgeSec() < 1) {
                 m_led.setData(m_greenBuffer);
             } else {
                 m_led.setData(m_redBuffer);