cleaning

comp/src/main/java/org/team100/frc2025/Elevator/Elevator.java

@@ -8,29 +8,16 @@
 import org.team100.lib.config.Identity;
 import org.team100.lib.config.PIDConstants;
 import org.team100.lib.dashboard.Glassy;
-import org.team100.lib.encoder.AS5048RotaryPositionSensor;
-import org.team100.lib.encoder.CombinedEncoder;
-import org.team100.lib.encoder.EncoderDrive;
-import org.team100.lib.encoder.IncrementalBareEncoder;
-import org.team100.lib.encoder.RotaryPositionSensor;
 import org.team100.lib.encoder.SimulatedBareEncoder;
-import org.team100.lib.encoder.SimulatedRotaryPositionSensor;
 import org.team100.lib.encoder.Talon6Encoder;
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.motion.mechanism.LinearMechanism;
-import org.team100.lib.motion.mechanism.RotaryMechanism;
 import org.team100.lib.motion.mechanism.SimpleLinearMechanism;
-import org.team100.lib.motion.mechanism.SimpleRotaryMechanism;
-import org.team100.lib.motion.servo.AngularPositionServo;
-import org.team100.lib.motion.servo.GravityServoInterface;
-import org.team100.lib.motion.servo.OutboardAngularPositionServo;
-import org.team100.lib.motion.servo.OutboardGravityServo;
 import org.team100.lib.motion.servo.OutboardLinearPositionServo;
 import org.team100.lib.motor.Kraken6Motor;
 import org.team100.lib.motor.MotorPhase;
 import org.team100.lib.motor.SimulatedBareMotor;
 import org.team100.lib.profile.TrapezoidProfile100;
-import org.team100.lib.state.Control100;
 
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -71,7 +58,8 @@ public Elevator(
         PIDConstants elevatorPID = PIDConstants.makePositionPID(2);
 
         Feedforward100 elevatorFF = Feedforward100.makeKraken6Elevator();
-        // TrapezoidProfile100 elevatorProfile = new TrapezoidProfile100(220, 220, 0.05); // TODO CHANGE THESE
+        // TrapezoidProfile100 elevatorProfile = new TrapezoidProfile100(220, 220,
+        // 0.05); // TODO CHANGE THESE
         TrapezoidProfile100 elevatorProfile = new TrapezoidProfile100(100, 100, 0.05); // TODO CHANGE THESE
 
         int wristSupplyLimit = 60;
@@ -93,7 +81,6 @@ public Elevator(
                 Kraken6Motor portMotor = new Kraken6Motor(portMotorLogger, portID, MotorPhase.REVERSE,
                         elevatorSupplyLimit, elevatorStatorLimit, elevatorPID, elevatorFF);
 
-
                 LinearMechanism starboardMech = new SimpleLinearMechanism(
                         starboardMotor,
                         new Talon6Encoder(starboardLogger, starboardMotor),
@@ -110,13 +97,6 @@ public Elevator(
 
                 portServo = new OutboardLinearPositionServo(portLogger, portMech, elevatorProfile);
 
-
-
-
-
-
-
-
                 break;
             }
             default -> {
@@ -158,8 +138,8 @@ public void resetWristProfile() {
     }
 
     public void setPosition(double x) {
-        starboardServo.setPosition(x, 1.3); //54 max
-        portServo.setPosition(x, 1.3); //54 max
+        starboardServo.setPosition(x, 1.3); // 54 max
+        portServo.setPosition(x, 1.3); // 54 max
     }
 
     public void setDutyCycle(double value) {
@@ -173,6 +153,4 @@ public double getPosition() {
         return starboardServo.getPosition().orElse(0);
     }
 
-
-
 }

comp/src/main/java/org/team100/frc2025/Elevator/ElevatorVisualization.java

@@ -30,7 +30,6 @@ public ElevatorVisualization(Elevator elevator) {
         m_elevator = elevator;
         m_mechanism = new Mechanism2d(100, 100);
         MechanismRoot2d root = m_mechanism.getRoot("root", 50, 5);
-        double position = m_elevator.getPosition();
         m_base = new MechanismLigament2d(
                 "height",
                 35,

comp/src/main/java/org/team100/frc2025/RobotContainer.java

@@ -5,24 +5,19 @@
 import java.util.Optional;
 import java.util.function.BooleanSupplier;
 
+import org.team100.frc2025.Climber.Climber;
+import org.team100.frc2025.Climber.ClimberFactory;
+import org.team100.frc2025.Climber.ClimberRotate;
 import org.team100.frc2025.Elevator.Elevator;
 import org.team100.frc2025.Elevator.ElevatorDown;
 import org.team100.frc2025.Elevator.SetElevator;
 import org.team100.frc2025.Elevator.SetWrist;
 import org.team100.frc2025.Elevator.SetWristSafe;
-import org.team100.frc2025.Intake.AlgaeIntake;
-import org.team100.frc2025.Intake.RunIntake;
-import org.team100.frc2025.Intake.RunOuttake;
-import org.team100.frc2025.Climber.Climber;
-import org.team100.frc2025.Climber.ClimberFactory;
-import org.team100.frc2025.Climber.ClimberRotate;
-
 import org.team100.frc2025.Swerve.FullCycle;
 import org.team100.frc2025.Wrist.Wrist;
 import org.team100.lib.async.Async;
 import org.team100.lib.async.AsyncFactory;
 import org.team100.lib.commands.drivetrain.DriveToPoseSimple;
-import org.team100.lib.commands.drivetrain.DriveToPoseWithProfile;
 import org.team100.lib.commands.drivetrain.DriveToPoseWithTrajectory;
 import org.team100.lib.commands.drivetrain.DriveToTranslationWithFront;
 import org.team100.lib.commands.drivetrain.FullCycle2;
@@ -42,7 +37,6 @@
 import org.team100.lib.controller.simple.PIDFeedback;
 import org.team100.lib.dashboard.Glassy;
 import org.team100.lib.framework.TimedRobot100;
-import org.team100.lib.geometry.GeometryUtil;
 import org.team100.lib.hid.DriverControl;
 import org.team100.lib.hid.DriverControlProxy;
 import org.team100.lib.hid.OperatorControl;
@@ -150,7 +144,7 @@ public RobotContainer(TimedRobot100 robot) throws IOException {
                 driveLog,
                 gyro,
                 m_modules.positions(),
-                GeometryUtil.kPoseZero,
+                Pose2d.kZero,
                 Takt.get());
 
         final AprilTagFieldLayoutWithCorrectOrientation m_layout = new AprilTagFieldLayoutWithCorrectOrientation();
@@ -287,7 +281,7 @@ public RobotContainer(TimedRobot100 robot) throws IOException {
                 new Rotate(m_drive, holonomicController, swerveKinodynamics, Math.PI / 2));
 
         onTrue(driverControl::resetRotation0, new ResetPose(m_drive, new Pose2d()));
-        onTrue(driverControl::resetRotation180, new SetRotation(m_drive, GeometryUtil.kRotation180));
+        onTrue(driverControl::resetRotation180, new SetRotation(m_drive, Rotation2d.kPi));
 
         // OPERATOR BUTTONS
         whileTrue(operatorControl::elevate, new SetWristSafe(m_wrist)); //x

comp/src/main/java/org/team100/frc2025/RobotContainerParkingLot.java

@@ -17,7 +17,6 @@
 import org.team100.lib.controller.drivetrain.SwerveControllerFactory;
 import org.team100.lib.dashboard.Glassy;
 import org.team100.lib.framework.TimedRobot100;
-import org.team100.lib.geometry.GeometryUtil;
 import org.team100.lib.hid.DriverControl;
 import org.team100.lib.hid.DriverControlProxy;
 import org.team100.lib.hid.OperatorControl;
@@ -41,6 +40,7 @@
 import org.team100.lib.util.Takt;
 import org.team100.lib.visualization.TrajectoryVisualization;
 
+import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.wpilibj.RobotController;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.button.Trigger;
@@ -95,7 +95,7 @@ public class RobotContainerParkingLot implements Glassy {
                 driveLogger,
                 m_gyro,
                 m_modules.positions(),
-                GeometryUtil.kPoseZero,
+                Pose2d.kZero,
                 Takt.get());
         final AprilTagFieldLayoutWithCorrectOrientation m_layout = new AprilTagFieldLayoutWithCorrectOrientation();
         final VisionDataProvider24 visionDataProvider = new VisionDataProvider24(

comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToCoralStationLeft.java

@@ -4,16 +4,11 @@
 import java.util.List;
 
 import org.team100.frc2025.FieldConstants;
-import org.team100.frc2025.FieldConstants.FieldSector;
-import org.team100.frc2025.FieldConstants.ReefAproach;
-import org.team100.frc2025.FieldConstants.ReefDestination;
 import org.team100.frc2025.Swerve.SemiAuto.Navigator;
-import org.team100.frc2025.Swerve.SemiAuto.ReefPath;
 import org.team100.lib.controller.drivetrain.SwerveController;
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.motion.drivetrain.SwerveDriveSubsystem;
 import org.team100.lib.motion.drivetrain.kinodynamics.SwerveKinodynamics;
-import org.team100.lib.trajectory.PoseSet;
 import org.team100.lib.trajectory.Trajectory100;
 import org.team100.lib.visualization.TrajectoryVisualization;
 

comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToIJLeft.java

@@ -5,15 +5,12 @@
 
 import org.team100.frc2025.FieldConstants;
 import org.team100.frc2025.FieldConstants.FieldSector;
-import org.team100.frc2025.FieldConstants.ReefAproach;
 import org.team100.frc2025.FieldConstants.ReefDestination;
 import org.team100.frc2025.Swerve.SemiAuto.Navigator;
-import org.team100.frc2025.Swerve.SemiAuto.ReefPath;
 import org.team100.lib.controller.drivetrain.SwerveController;
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.motion.drivetrain.SwerveDriveSubsystem;
 import org.team100.lib.motion.drivetrain.kinodynamics.SwerveKinodynamics;
-import org.team100.lib.trajectory.PoseSet;
 import org.team100.lib.trajectory.Trajectory100;
 import org.team100.lib.visualization.TrajectoryVisualization;
 

comp/src/main/java/org/team100/frc2025/Swerve/Auto/GoToReefDestination.java

@@ -5,15 +5,12 @@
 
 import org.team100.frc2025.FieldConstants;
 import org.team100.frc2025.FieldConstants.FieldSector;
-import org.team100.frc2025.FieldConstants.ReefAproach;
 import org.team100.frc2025.FieldConstants.ReefDestination;
 import org.team100.frc2025.Swerve.SemiAuto.Navigator;
-import org.team100.frc2025.Swerve.SemiAuto.ReefPath;
 import org.team100.lib.controller.drivetrain.SwerveController;
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.motion.drivetrain.SwerveDriveSubsystem;
 import org.team100.lib.motion.drivetrain.kinodynamics.SwerveKinodynamics;
-import org.team100.lib.trajectory.PoseSet;
 import org.team100.lib.trajectory.Trajectory100;
 import org.team100.lib.visualization.TrajectoryVisualization;
 

comp/src/main/java/org/team100/frc2025/Swerve/FullCycle.java

@@ -5,7 +5,6 @@
 import org.team100.frc2025.FieldConstants;
 import org.team100.lib.controller.drivetrain.SwerveController;
 import org.team100.lib.dashboard.Glassy;
-import org.team100.lib.geometry.GeometryUtil;
 import org.team100.lib.logging.LoggerFactory;
 import org.team100.lib.motion.drivetrain.SwerveDriveSubsystem;
 import org.team100.lib.motion.drivetrain.kinodynamics.SwerveKinodynamics;
@@ -14,6 +13,7 @@
 import org.team100.lib.visualization.TrajectoryVisualization;
 
 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.wpilibj2.command.SequentialCommandGroup;
 
@@ -24,8 +24,8 @@
 public class FullCycle extends SequentialCommandGroup implements Glassy {
     private static final double maxVelocityM_S = 2.0;
     private static final double maxAccelM_S_S = 2;
-    private static final Pose2d waypoint0 = new Pose2d(6, 2, GeometryUtil.kRotationZero);
-    private static final Pose2d waypoint1 = new Pose2d(2, 2, GeometryUtil.kRotationZero);
+    private static final Pose2d waypoint0 = new Pose2d(6, 2, Rotation2d.kZero);
+    private static final Pose2d waypoint1 = new Pose2d(2, 2, Rotation2d.kZero);
 
     public FullCycle(
             LoggerFactory parent,

comp/src/main/java/org/team100/frc2025/Swerve/Maker.java

@@ -3,11 +3,7 @@
 import org.team100.frc2025.FieldConstants.FieldSector;
 import org.team100.frc2025.FieldConstants.ReefDestination;
 import org.team100.frc2025.Swerve.Auto.GoToCoralStationLeft;
-import org.team100.frc2025.Swerve.Auto.GoToIJLeft;
 import org.team100.frc2025.Swerve.Auto.GoToReefDestination;
-import org.team100.frc2025.Swerve.SemiAuto.Hexagon_Nav.Generate120;
-import org.team100.frc2025.Swerve.SemiAuto.Hexagon_Nav.Generate180;
-import org.team100.frc2025.Swerve.SemiAuto.Hexagon_Nav.Generate60;
 import org.team100.frc2025.Swerve.SemiAuto.Profile_Nav.Embark;
 import org.team100.lib.controller.drivetrain.SwerveController;
 import org.team100.lib.controller.drivetrain.SwerveControllerFactory;
@@ -48,10 +44,10 @@ public Maker(
 
     }
 
-    public Command test() {
-        final SwerveKinodynamics swerveKinodynamics = SwerveKinodynamicsFactory.get();
+    public Command embark() {
         final SwerveController holonomicController = SwerveControllerFactory.ridiculous(m_logger);
 
+        final SwerveKinodynamics swerveKinodynamics = SwerveKinodynamicsFactory.get();
 
         final HolonomicProfile profile = new HolonomicProfile(
                 swerveKinodynamics.getMaxDriveVelocityM_S(),
@@ -60,51 +56,50 @@ public Command test() {
                 swerveKinodynamics.getMaxAngleSpeedRad_S(),
                 swerveKinodynamics.getMaxAngleAccelRad_S2(),
                 0.1); // 5 degrees
+        return new Embark(m_drive, holonomicController, profile);
+    }
 
+    public Command test() {
 
         return new SequentialCommandGroup(
-            new GoToReefDestination(
-                m_logger,
-                m_drive,
-                SwerveControllerFactory.fieldRelativeGoodPIDF(m_logger),
-                m_viz,
-                m_kinodynamics,
-                FieldSector.IJ,
-                ReefDestination.RIGHT),
-            new GoToCoralStationLeft(
-                    m_logger,
-                    m_drive,
-                    SwerveControllerFactory.fieldRelativeGoodPIDF(m_logger),
-                    m_viz,
-                    m_kinodynamics,
-                    0.5),
-            new GoToReefDestination(
-                    m_logger,
-                    m_drive,
-                    SwerveControllerFactory.fieldRelativeGoodPIDF(m_logger),
-                    m_viz,
-                    m_kinodynamics,
-                    FieldSector.KL,
-                    ReefDestination.LEFT),
-            new GoToCoralStationLeft(
-                    m_logger,
-                    m_drive,
-                    SwerveControllerFactory.fieldRelativeGoodPIDF(m_logger),
-                    m_viz,
-                    m_kinodynamics,
-                    0),
-            new GoToReefDestination(
-                    m_logger,
-                    m_drive,
-                    SwerveControllerFactory.fieldRelativeGoodPIDF(m_logger),
-                    m_viz,
-                    m_kinodynamics,
-                    FieldSector.KL,
-                    ReefDestination.RIGHT)
-        );
-
-
-        // return new Embark(m_drive, holonomicController, profile);
+                new GoToReefDestination(
+                        m_logger,
+                        m_drive,
+                        SwerveControllerFactory.fieldRelativeGoodPIDF(m_logger),
+                        m_viz,
+                        m_kinodynamics,
+                        FieldSector.IJ,
+                        ReefDestination.RIGHT),
+                new GoToCoralStationLeft(
+                        m_logger,
+                        m_drive,
+                        SwerveControllerFactory.fieldRelativeGoodPIDF(m_logger),
+                        m_viz,
+                        m_kinodynamics,
+                        0.5),
+                new GoToReefDestination(
+                        m_logger,
+                        m_drive,
+                        SwerveControllerFactory.fieldRelativeGoodPIDF(m_logger),
+                        m_viz,
+                        m_kinodynamics,
+                        FieldSector.KL,
+                        ReefDestination.LEFT),
+                new GoToCoralStationLeft(
+                        m_logger,
+                        m_drive,
+                        SwerveControllerFactory.fieldRelativeGoodPIDF(m_logger),
+                        m_viz,
+                        m_kinodynamics,
+                        0),
+                new GoToReefDestination(
+                        m_logger,
+                        m_drive,
+                        SwerveControllerFactory.fieldRelativeGoodPIDF(m_logger),
+                        m_viz,
+                        m_kinodynamics,
+                        FieldSector.KL,
+                        ReefDestination.RIGHT));
 
     }
 

comp/src/main/java/org/team100/frc2025/Swerve/SemiAuto/Hexagon_Nav/Generate60.java

@@ -81,7 +81,7 @@ public Trajectory100 trajectory(Pose2d currentPose) {
         double dotProduct = ((pointToAnchor.getX()/pointToAnchor.getNorm()) * (vectorToAnchorPoint.getX()/vectorToAnchorPoint.getNorm()))
                 + ((pointToAnchor.getY()/pointToAnchor.getNorm()) * (vectorToAnchorPoint.getY()/vectorToAnchorPoint.getNorm()));
 
-        System.out.println(dotProduct);
+        // System.out.println(dotProduct);
 
 
         double distanceToReef = FieldConstants.getDistanceToReefCenter(currTranslation);

comp/src/main/java/org/team100/frc2025/Swerve/SemiAuto/Hexagon_Nav/LandingDestinationGroup.java

@@ -5,7 +5,6 @@
 package org.team100.frc2025.Swerve.SemiAuto.Hexagon_Nav;
 
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
 
 /** Add your docs here. */
 public record LandingDestinationGroup(Rotation2d landingSpline, Rotation2d destinationSpline) {}