Merge pull request #40 from Team4388/auto-placement

Auto placement --> master

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

@@ -7,11 +7,14 @@
 
 package frc4388.robot;
 
-import edu.wpi.first.math.controller.PIDController;
-import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Transform3d;
+import edu.wpi.first.math.geometry.Translation3d;
 import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.math.util.Units;
 import frc4388.utility.Gains;
 import frc4388.utility.LEDPatterns;
+import frc4388.utility.RobotUnits;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
@@ -168,5 +171,8 @@ public final class Constants {
     
     // public static final double MID_TARGET_HEIGHT = 34.0;
     public static final double MID_TAPE_HEIGHT = 24.0;
+
+    public static final double APRIL_HEIGHT = -1.0; // TODO: find actual value
+
   }
 }

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

@@ -7,20 +7,6 @@
 
 package frc4388.robot;
 
-import java.lang.System;
-import java.lang.reflect.Array;
-import java.util.Arrays;
-
-import com.ctre.phoenix.motorcontrol.ControlMode;
-import com.ctre.phoenix.motorcontrol.DemandType;
-import com.ctre.phoenix.motorcontrol.can.MotControllerJNI;
-
-import java.io.File;
-import java.io.IOException;
-import java.io.PrintWriter;
-
-import edu.wpi.first.math.geometry.Translation2d;
-import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -28,10 +14,6 @@ import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import frc4388.utility.DeferredBlock;
 import frc4388.utility.RobotTime;
 
-import frc4388.robot.subsystems.Location;
-import frc4388.robot.subsystems.Apriltags.Tag;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-
 /**
  * The VM is configured to automatically run this class, and to call the
  * functions corresponding to each mode, as described in the TimedRobot

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

@@ -7,26 +7,34 @@
 
 package frc4388.robot;
 
+import java.util.function.Consumer;
+
+import edu.wpi.first.math.geometry.Translation2d;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.REVLibError;
 import com.revrobotics.CANSparkMax.FaultID;
-
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.ConditionalCommand;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.RunCommand;
+import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
+import edu.wpi.first.wpilibj2.command.button.POVButton;
 import frc4388.robot.Constants.*;
-import frc4388.robot.commands.AutoBalance;
-import frc4388.robot.commands.JoystickPlayback;
 import frc4388.robot.subsystems.Arm;
 import frc4388.robot.subsystems.Claw;
 import frc4388.robot.subsystems.Limelight;
 import frc4388.robot.subsystems.SwerveDrive;
-import frc4388.robot.commands.JoystickRecorder;
-import frc4388.robot.commands.LimeAlign;
-import frc4388.robot.commands.PlaybackChooser;
+import frc4388.robot.commands.BooleanCommand;
+import frc4388.robot.commands.Arm.PivotCommand;
+import frc4388.robot.commands.Arm.TeleCommand;
+import frc4388.robot.commands.Autos.AutoBalance;
+import frc4388.robot.commands.Autos.PlaybackChooser;
+import frc4388.robot.commands.Placement.AprilRotAlign;
+import frc4388.robot.commands.Placement.LimeAlign;
+import frc4388.robot.commands.Swerve.RotateToAngle;
 import frc4388.utility.controller.DeadbandedXboxController;
 import frc4388.utility.controller.XboxController; 
 
@@ -52,7 +60,7 @@ public class RobotContainer {
 
     public final Claw m_robotClaw = new Claw(m_robotMap.leftClaw, m_robotMap.rightClaw, m_robotMap.spinnyspin);
 
-    public final Limelight m_limeLight = new Limelight();
+    public final Limelight m_robotLimeLight = new Limelight();
 
     /* Controllers */
     private final DeadbandedXboxController m_driverXbox = new DeadbandedXboxController(OIConstants.XBOX_DRIVER_ID);
@@ -60,9 +68,7 @@ public class RobotContainer {
 
     /* Autos */
     public SendableChooser<Command> chooser = new SendableChooser<>();
-    
-    private Command noAuto = new InstantCommand();
-    
+
     // private Command balance = new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive);
     
     // private Command blue1Path = new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt");
@@ -74,6 +80,92 @@ public class RobotContainer {
     // private Command taxi = new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt");
 
     private PlaybackChooser playbackChooser;
+
+    /* Commands */
+    private Command emptyCommand = new InstantCommand();
+    private Command interruptCommand = new InstantCommand(() -> {}, m_robotArm, m_robotSwerveDrive, m_robotClaw, m_robotLimeLight);
+
+    private SequentialCommandGroup armToHome = new SequentialCommandGroup(new TeleCommand(m_robotArm, 0), new PivotCommand(m_robotArm, 135));
+
+    private Command toggleClaw = new InstantCommand(() -> m_robotClaw.toggle(), m_robotClaw);
+
+    private boolean readyForPlacement = false;
+    private Boolean isPole = null;
+
+    private SequentialCommandGroup alignToPole =
+        new SequentialCommandGroup(
+            new RotateToAngle(m_robotSwerveDrive, 0.0),
+            new InstantCommand(() -> m_robotLimeLight.setToLimePipeline(), m_robotLimeLight),
+            new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getLowestTape().getYaw(), 0.04),
+            new RotateToAngle(m_robotSwerveDrive, 0.0),
+            new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive)
+            // new DriveToLimeDistance(m_robotSwerveDrive, m_robotLimeLight, 37, () -> m_robotLimeLight.getDistanceToTape())
+        ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = true));
+
+    // private SequentialCommandGroup alignToShelf = 
+    //     new SequentialCommandGroup(
+    //         new RotateToAngle(m_robotSwerveDrive, 0.0),
+    //         new InstantCommand(() -> m_robotLimeLight.setToAprilPipeline(), m_robotLimeLight),
+    //         new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getAprilPoint().getYaw(), 0.04),
+    //         new RotateToAngle(m_robotSwerveDrive, 0.0),
+    //         new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive)
+    //     ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false));
+    
+    private SequentialCommandGroup alignToShelf = 
+        new SequentialCommandGroup(
+            new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight),
+            new InstantCommand(() -> m_robotLimeLight.setToAprilPipeline(), m_robotLimeLight),
+            new LimeAlign(m_robotSwerveDrive, m_robotLimeLight, () -> m_robotLimeLight.getAprilPoint().getYaw(), 0.04),
+            new AprilRotAlign(m_robotSwerveDrive, m_robotLimeLight),
+            new RunCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0.0, -0.4), new Translation2d(0.0, 0.0), true), m_robotSwerveDrive)
+        ).andThen(new InstantCommand(() -> readyForPlacement = true), new InstantCommand(() -> isPole = false));
+
+    public SequentialCommandGroup place = null;
+
+    private Consumer<SequentialCommandGroup> queuePlacement = (scg) -> {
+        place = scg.andThen(new InstantCommand(() -> readyForPlacement = false), new InstantCommand(() -> isPole = null), new InstantCommand(() -> place = null));
+    };
+
+    // TODO: find actual values
+    private SequentialCommandGroup placeCubeHigh =
+        new SequentialCommandGroup(
+            // new InstantCommand(() -> System.out.println("Placing cone high")),
+            new PivotCommand(m_robotArm, 64 + 135),
+            new InstantCommand(() -> m_robotArm.setRotVel(0)),
+            new TeleCommand(m_robotArm, 95642),
+            toggleClaw.asProxy(),
+            armToHome.asProxy()
+        );
+
+    private SequentialCommandGroup placeCubeMid = new SequentialCommandGroup(
+        new PivotCommand(m_robotArm, 70 + 135),
+        new TeleCommand(m_robotArm, 32866),
+        toggleClaw.asProxy(),
+        armToHome.asProxy()
+    );
+
+    private SequentialCommandGroup placeConeHigh = new SequentialCommandGroup(
+        new PivotCommand(m_robotArm, 0),
+        new TeleCommand(m_robotArm, 0),
+        toggleClaw.asProxy(),
+        armToHome.asProxy()
+    );
+
+    private SequentialCommandGroup placeConeMid = new SequentialCommandGroup(
+        new PivotCommand(m_robotArm, 0),
+        new TeleCommand(m_robotArm, 0),
+        toggleClaw.asProxy(),
+        armToHome.asProxy()
+    );
+
+    private SequentialCommandGroup placeLow = new SequentialCommandGroup(
+        new PivotCommand(m_robotArm, 0),
+        new TeleCommand(m_robotArm, 0),
+        toggleClaw.asProxy(),
+        armToHome.asProxy()
+    );
+
+
     /**
      * The container for the robot. Contains subsystems, OI devices, and commands.
      */
@@ -95,21 +187,32 @@ public class RobotContainer {
         .withName("Arm DefaultCommand"));
         
         // * Auto Commands
-        // chooser.setDefaultOption("NoAuto", noAuto);
+        chooser.setDefaultOption("NoAuto", emptyCommand);
+        chooser.addOption("alignToPole", alignToPole);
+        chooser.addOption("alignToShelf", alignToShelf);
+
+        chooser.addOption("placeConeHigh", placeConeHigh);
+        chooser.addOption("placeConeMid", placeConeMid);
+        chooser.addOption("placeCubeHigh", placeCubeHigh);
+        chooser.addOption("placeCubeMid", placeCubeMid);
+        chooser.addOption("placeLow", placeLow);
 
         // chooser.addOption("Blue1Path", blue1Path);
         // chooser.addOption("Blue1PathWithBalance", blue1PathWithBalance);
 
         // chooser.addOption("Red1Path", red1Path);
         // chooser.addOption("Red1PathWithBalance", red1PathWithBalance);
-        
-        // chooser.addOption("Taxi", taxi);
 
         playbackChooser = new PlaybackChooser(m_robotSwerveDrive)
             .addOption("Balance", new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive))
             .buildDisplay();
     }
 
+    // here be dragons - enter if you dare
+    private static class TapState {
+        public boolean gearTapped = true;
+        public long    gearTime   = 0;
+    }
 
     /**
      * Use this method to define your button->command mappings. Buttons can be
@@ -120,23 +223,38 @@ public class RobotContainer {
     private void configureButtonBindings() {
         // * Driver Buttons
         new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
-            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive));
+            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyro(), m_robotSwerveDrive)); // final
         
-        new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
-            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToTurbo()))
+        // because closure reasons - ask Daniel Thomas
+        // final TapState tap = new TapState();
+        new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
+            .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.setToTurbo()))
             .onFalse(new InstantCommand(() -> m_robotSwerveDrive.setToFast()));
-
-        // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
-        //     .whileTrue(new InstantCommand(() -> m_robotSwerveDrive.setToTurbo()));
+            // .onTrue(new InstantCommand(() -> {
+            //     tap.gearTapped = true;
+            //     tap.gearTime   = System.currentTimeMillis();
+            // }))
+            // .whileTrue(new RunCommand(() -> {
+            //     if (tap.gearTapped && System.currentTimeMillis() - tap.gearTime > 200) {
+            //         m_robotSwerveDrive.setToTurbo();
+            //         tap.gearTapped = false;
+            //     }
+            // }))
+            // .onFalse(new InstantCommand(() -> {
+            //     if (tap.gearTapped)
+            //         m_robotSwerveDrive.setToFast();
+            //     else
+            //         m_robotSwerveDrive.setToSlow();
+            // }));
         
-        new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
+        new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.setToSlow()));
 
-        //     // .onFalse()
-
         new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
-            .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive));
+            .onTrue(new AutoBalance(m_robotMap.gyro, m_robotSwerveDrive)); // final
 
+        new JoystickButton(getDeadbandedDriverController(), XboxController.X_BUTTON)
+            .onTrue(interruptCommand.asProxy()); // final
         // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
         //     .whileTrue(new JoystickRecorder(m_robotSwerveDrive,
         //                                     () -> getDeadbandedDriverController().getLeftX(),
@@ -148,25 +266,26 @@ public class RobotContainer {
 
         // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
         //     .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Blue1Path.txt"))
-        //     .onFalse(new InstantCommand());
+        //     .onFalse(new InstantCommand()); 
 
         // * Operator Buttons
-        // new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
-        //     // .onTrue(new InstantCommand(() -> System.out.println("Claw Button")));
-        //     .onTrue(new InstantCommand(() -> m_robotClaw.toggle()));
+        
+        // align (pole)
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON) // final
+            .onTrue(alignToPole)
+            .onFalse(interruptCommand.asProxy());
+        
+        // align (shelf)
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON) // final
+            .onTrue(alignToShelf)
+            .onFalse(interruptCommand.asProxy());
 
+        // toggle claw
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON) // final
+            .onTrue(toggleClaw.asProxy());
         
-
-        new JoystickButton(getDeadbandedOperatorController(), XboxController.B_BUTTON)
-            .onTrue(new InstantCommand(() -> m_limeLight.toggleLEDs(), m_limeLight));
-        
-        new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
-            .whileTrue(new LimeAlign(m_robotSwerveDrive, m_limeLight));
-        
-        new JoystickButton(getDeadbandedOperatorController(), XboxController.X_BUTTON)
-            .onTrue(new InstantCommand(() -> m_robotClaw.toggle()));
-        
-        new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON)
+        // kill soft limits
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.Y_BUTTON) // final
             .onTrue(new InstantCommand(() -> m_robotArm.killSoftLimits()));
 
         // SmartDashboard.putBoolean("isn't SparkMAX", );
@@ -175,11 +294,39 @@ public class RobotContainer {
             .onTrue  (new InstantCommand(() -> m_robotClaw.yesspinnyspin()))
             .onFalse (new InstantCommand(() -> m_robotClaw.nospinnyspin()));
         
-        // new JoystickButton(getDeadbandedOperatorController(), XboxController.A_BUTTON)
-        //     .onTrue(new InstantCommand(() -> m_robotArm.resetTeleSoftLimit(), m_robotArm));
+        //Arm to Home
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON) // final
+            .onTrue(armToHome.asProxy());
+
+        //Interupt Button
+        new JoystickButton(getDeadbandedOperatorController(), XboxController.RIGHT_BUMPER_BUTTON) // final
+            .onTrue(interruptCommand.asProxy());
+
+
+        // place high
+        new POVButton(getDeadbandedOperatorController(), 0)
+            .onTrue(new ConditionalCommand(
+                new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeConeHigh)), new InstantCommand(() -> queuePlacement.accept(placeCubeHigh)), () -> isPole == true),
+                emptyCommand.asProxy(),
+                () -> readyForPlacement == true)
+            );
+
+        // place mid
+        new POVButton(getDeadbandedOperatorController(), 270)
+            .onTrue(new ConditionalCommand(
+                new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeConeMid)), new InstantCommand(() -> queuePlacement.accept(placeCubeMid)), () -> isPole == true),
+                emptyCommand.asProxy(),
+                () -> readyForPlacement == true)
+            );
+
+        // place low
+        new POVButton(getDeadbandedOperatorController(), 180)
+            .onTrue(new ConditionalCommand(new InstantCommand(() -> queuePlacement.accept(placeLow)), emptyCommand.asProxy(), () -> readyForPlacement == true));
+
+        // confirm
+        new POVButton(getDeadbandedOperatorController(), 90)
+            .onTrue(new BooleanCommand(() -> place, () -> emptyCommand.asProxy(), () -> place != null));
 
-        // new JoystickButton(getDeadbandedOperatorController(), XboxController.LEFT_BUMPER_BUTTON)
-        //     .onTrue(new InstantCommand(() -> {}, m_robotArm, m_robotSwerveDrive, m_robotClaw));
     }
 
     /**

src/main/java/frc4388/robot/commands/Arm/PivotCommand.java

@@ -2,9 +2,10 @@
 // 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;
+package frc4388.robot.commands.Arm;
 
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc4388.robot.commands.PelvicInflammatoryDisease;
 import frc4388.robot.subsystems.Arm;
 
 public class PivotCommand extends PelvicInflammatoryDisease {

src/main/java/frc4388/robot/commands/Arm/TeleCommand.java

@@ -0,0 +1,39 @@
+// 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.Arm;
+
+import edu.wpi.first.wpilibj2.command.CommandBase;
+import frc4388.robot.Constants.ArmConstants;
+import frc4388.robot.commands.PelvicInflammatoryDisease;
+import frc4388.robot.subsystems.Arm;
+
+public class TeleCommand extends CommandBase {
+  private final Arm     arm;
+  private final double  target;
+  private       boolean goIn;
+  
+  /** Creates a new ArmCommand. */
+  public TeleCommand(Arm arm, double target) {
+    this.arm    = arm;
+    this.target = target;
+    addRequirements(arm);
+  }
+
+  @Override
+  public void initialize() {
+    this.goIn = target < arm.getArmLength();
+  }
+
+  @Override
+  public void execute() {
+    arm.setTeleVel(goIn ? 1 : -1);
+  }
+
+  @Override
+  public boolean isFinished() {
+    if (goIn) return arm.getArmLength() < target;
+    else      return arm.getArmLength() > target;
+  }
+}

src/main/java/frc4388/robot/commands/Autos/AutoBalance.java

@@ -2,11 +2,12 @@
 // 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;
+package frc4388.robot.commands.Autos;
 
 import edu.wpi.first.math.MathUtil;
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import frc4388.robot.commands.PelvicInflammatoryDisease;
 import frc4388.robot.subsystems.SwerveDrive;
 import frc4388.utility.RobotGyro;
 

src/main/java/frc4388/robot/commands/Autos/PlaybackChooser.java

@@ -1,4 +1,4 @@
-package frc4388.robot.commands;
+package frc4388.robot.commands.Autos;
 
 import java.io.File;
 import java.util.ArrayList;
@@ -10,6 +10,7 @@ import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
 import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
+import frc4388.robot.commands.Swerve.JoystickPlayback;
 import frc4388.robot.subsystems.SwerveDrive;
 
 public class PlaybackChooser {

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

@@ -0,0 +1,67 @@
+// 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;
+
+import java.util.function.Supplier;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.CommandBase;
+
+public class BooleanCommand extends CommandBase {
+
+  private Supplier<Command> onTrue;
+  private Supplier<Command> onFalse;
+
+  private Supplier<Boolean> condition;
+
+  private Supplier<Command> selected;
+
+
+  /** Creates a new BooleanCommand. */
+  public BooleanCommand(Supplier<Command> onTrue, Supplier<Command> onFalse, Supplier<Boolean> condition) {
+    this.onTrue = onTrue;
+    this.onFalse = onFalse;
+    this.condition = condition;
+  }
+
+  // Called when the command is initially scheduled.
+  @Override
+  public void initialize() {
+    if (condition.get()) {
+      selected = onTrue;
+    } else {
+      selected = onFalse;
+    }
+    if (selected.get() != null) {
+      selected.get().initialize();
+    }
+  }
+
+  // Called every time the scheduler runs while the command is scheduled.
+  @Override
+  public void execute() {
+    if (selected.get() != null) {
+      selected.get().execute();
+    }
+  }
+
+  // Called once the command ends or is interrupted.
+  @Override
+  public void end(boolean interrupted) {
+    if (selected.get() != null) {
+      selected.get().end(interrupted);
+    }
+  }
+
+  // Returns true when the command should end.
+  @Override
+  public boolean isFinished() {
+    if (selected.get() != null) {
+      return selected.get().isFinished();
+    } else {
+      return true;
+    }
+  }
+}

src/main/java/frc4388/robot/commands/Placement/AprilRotAlign.java

@@ -0,0 +1,42 @@
+// 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.Placement;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import frc4388.robot.commands.PelvicInflammatoryDisease;
+import frc4388.robot.subsystems.Limelight;
+import frc4388.robot.subsystems.SwerveDrive;
+
+public class AprilRotAlign extends PelvicInflammatoryDisease {
+  
+  SwerveDrive drive;
+  Limelight lime;
+  
+  /** Creates a new AprilRotAlign. */
+  public AprilRotAlign(SwerveDrive drive, Limelight lime) {
+    super(0.1, 0.2, 0.0, 0.0, 0.0);
+  
+    this.drive = drive;
+    this.lime = lime;
+
+    addRequirements(drive, lime);
+  }
+
+  @Override
+  public double getError() {
+    double err = 0.0;
+
+    try {
+      err = lime.getAprilSkew();
+    } catch (NullPointerException ex) {}
+    
+    return err;
+  }
+
+  @Override
+  public void runWithOutput(double output) {
+    drive.driveWithInput(new Translation2d(0.0, 0.0), new Translation2d(-output, 0.0), true);
+  }
+}

src/main/java/frc4388/robot/commands/Placement/DriveToLimeDistance.java

@@ -0,0 +1,44 @@
+// 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.Placement;
+
+import java.util.function.DoubleSupplier;
+
+import edu.wpi.first.math.geometry.Translation2d;
+import frc4388.robot.commands.PelvicInflammatoryDisease;
+import frc4388.robot.subsystems.Limelight;
+import frc4388.robot.subsystems.SwerveDrive;
+
+public class DriveToLimeDistance extends PelvicInflammatoryDisease {
+
+  SwerveDrive drive;
+  Limelight lime;
+
+  double targetDistance;
+  DoubleSupplier ds;
+
+  /** Creates a new DriveToLimeDistance. */
+  public DriveToLimeDistance(SwerveDrive drive, Limelight lime, double targetDistance, DoubleSupplier ds) {
+    super(0.5, 0.0, 0.0, 0.0, 1);
+
+    this.drive = drive;
+    this.lime = lime;
+    this.targetDistance = targetDistance;
+    this.ds = ds;
+
+    addRequirements(drive, lime);
+  }
+
+  @Override
+  public double getError() {
+    return targetDistance - ds.getAsDouble();
+  }
+
+  @Override
+  public void runWithOutput(double output) {
+    System.out.println(output / Math.abs(getError()));
+    drive.driveWithInput(new Translation2d(0.0, output / Math.abs(getError())), new Translation2d(0.0, 0.0), true);    
+  }
+}

src/main/java/frc4388/robot/commands/Placement/LimeAlign.java

@@ -2,12 +2,13 @@
 // 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;
+package frc4388.robot.commands.Placement;
 
-import org.photonvision.targeting.PhotonTrackedTarget;
+import java.util.function.DoubleSupplier;
 
 import edu.wpi.first.math.geometry.Translation2d;
 import frc4388.robot.Constants.VisionConstants;
+import frc4388.robot.commands.PelvicInflammatoryDisease;
 import frc4388.robot.subsystems.Limelight;
 import frc4388.robot.subsystems.SwerveDrive;
 
@@ -16,11 +17,14 @@ public class LimeAlign extends PelvicInflammatoryDisease {
   SwerveDrive drive;
   Limelight lime;
 
-  public LimeAlign(SwerveDrive drive, Limelight lime) {
-    super(0.7, 0.1, 0.0, 0.0, 0);
+  DoubleSupplier ds;
+
+  public LimeAlign(SwerveDrive drive, Limelight lime, DoubleSupplier ds, double tolerance) {
+    super(0.4, 0.4, 0.0, 0.0, tolerance);
 
     this.drive = drive;
     this.lime = lime;
+    this.ds = ds;
     
     addRequirements(drive, lime);
   }
@@ -30,7 +34,8 @@ public class LimeAlign extends PelvicInflammatoryDisease {
     double err = 0.0;
 
     try {
-      err = lime.getFirstTargetPoint().getYaw() / (VisionConstants.H_FOV / 2); 
+      System.out.println(ds.getAsDouble());
+      err = ds.getAsDouble() / (VisionConstants.H_FOV / 2);
     } catch (NullPointerException ex) {}
     
     return err;
@@ -38,13 +43,6 @@ public class LimeAlign extends PelvicInflammatoryDisease {
 
   @Override
   public void runWithOutput(double output) {
-
-    if (output > 0) {
-      output += 0.6;
-    } else if (output < 0) {
-      output -= 0.6;
-    }
-
     drive.driveWithInput(new Translation2d(output, 0.0), new Translation2d(0.0, 0.0), true);
   }
 }

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

@@ -2,7 +2,7 @@
 // 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;
+package frc4388.robot.commands.Swerve;
 
 import java.io.File;
 import java.io.FileNotFoundException;

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

@@ -2,7 +2,7 @@
 // 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;
+package frc4388.robot.commands.Swerve;
 
 import java.io.File;
 import java.io.IOException;

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

@@ -0,0 +1,35 @@
+// 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.PelvicInflammatoryDisease;
+import frc4388.robot.subsystems.SwerveDrive;
+
+public class RotateToAngle extends PelvicInflammatoryDisease {
+
+  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/TeleCommand.java

@@ -1,32 +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;
-
-import frc4388.robot.Constants.ArmConstants;
-import frc4388.robot.subsystems.Arm;
-
-public class TeleCommand extends PelvicInflammatoryDisease {
-  private final Arm    arm;
-  private final double target;
-  
-  /** Creates a new ArmCommand. */
-  public TeleCommand(Arm arm, double target) {
-    super(0.6, 0, 0, 0, 0);
-    this.arm    = arm;
-    this.target = target;
-    addRequirements(arm);
-  }
-
-  @Override
-  public double getError() {
-    return (arm.getArmLength() - target) /
-           (ArmConstants.TELE_FORWARD_SOFT_LIMIT - ArmConstants.TELE_REVERSE_SOFT_LIMIT);
-  }
-
-  @Override
-  public void runWithOutput(double output) {
-    arm.setTeleVel(output);
-  }
-}

src/main/java/frc4388/robot/subsystems/Arm.java

@@ -1,28 +1,20 @@
 package frc4388.robot.subsystems;
 
 import com.ctre.phoenix.motorcontrol.ControlMode;
-import com.ctre.phoenix.motorcontrol.FeedbackDevice;
 import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
 import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
-import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
-import com.ctre.phoenix.motorcontrol.TalonFXFeedbackDevice;
-import com.ctre.phoenix.motorcontrol.can.TalonFXConfiguration;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonFX;
 import com.ctre.phoenix.sensors.CANCoder;
 import com.ctre.phoenix.sensors.CANCoderConfiguration;
 import frc4388.robot.Constants.ArmConstants;
-import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.utility.DeferredBlock;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.RunCommand;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Arm extends SubsystemBase {
     private WPI_TalonFX m_tele;
     public WPI_TalonFX  m_pivot;
     private CANCoder    m_pivotEncoder;
-    private boolean     m_debug;
 
     // Moves arm to distance [dist] then returns new ang
     public Arm(WPI_TalonFX pivot, WPI_TalonFX tele, CANCoder encoder, boolean debug) {
@@ -48,6 +40,8 @@ public class Arm extends SubsystemBase {
     }
 
     public void setRotVel(double vel) {
+        if (vel > 1) vel = 1;
+
         var degrees = Math.abs(getArmRotation()) - 135;
         SmartDashboard.putNumber("arm degrees", degrees);
         SmartDashboard.putNumber("arm rot vel", vel);
@@ -56,6 +50,8 @@ public class Arm extends SubsystemBase {
             m_pivot.set(ControlMode.PercentOutput, 0);
         } else if (degrees > 90 && vel > 0) {
             m_pivot.set(ControlMode.PercentOutput, .15 * vel);
+        } else if (degrees < 25 && vel < 0) {
+            m_pivot.set(ControlMode.PercentOutput, .15 * vel);
         } else {
             m_pivot.set(ControlMode.PercentOutput, .3 * vel);
         }
@@ -86,7 +82,7 @@ public class Arm extends SubsystemBase {
     }
 
     public double getArmLength() {
-        return m_tele.getSelectedSensorPosition();
+        return m_tele.getSelectedSensorPosition() - tele_soft;
     }
 
     public double getArmRotation() {
@@ -120,9 +116,10 @@ public class Arm extends SubsystemBase {
     }
 
     boolean tele_softLimit = false;
+    double  tele_soft = 0;
     public void resetTeleSoftLimit() {
         if (!tele_softLimit) {
-            var tele_soft = m_tele.getSelectedSensorPosition();
+            tele_soft = m_tele.getSelectedSensorPosition();
             m_tele.configForwardSoftLimitThreshold(91000 - tele_soft);
             m_tele.configReverseSoftLimitThreshold(tele_soft);
             m_tele.configForwardSoftLimitEnable(true);
@@ -140,7 +137,6 @@ public class Arm extends SubsystemBase {
 
     @Override
     public void periodic() {
-        double degrees = Math.abs(m_pivotEncoder.getAbsolutePosition() - 135);
 
         if (m_tele.isFwdLimitSwitchClosed() == 1 && tele_reset) {
             var tele_soft = m_tele.getSelectedSensorPosition();
@@ -154,17 +150,10 @@ public class Arm extends SubsystemBase {
         }
 
         // double x = Math.cos(Math.toRadians(degrees));
+        SmartDashboard.putNumber("arm length", getArmLength());
     }
 
-    boolean soft_limits = true;
     public void killSoftLimits() {
         resetTeleSoftLimit();
-        var pivot_soft = m_pivot.getSelectedSensorPosition();
-        var tele_soft  = m_tele.getSelectedSensorPosition();
-        
-        m_pivot.configForwardSoftLimitEnable(!soft_limits);
-        m_pivot.configReverseSoftLimitEnable(!soft_limits);
-
-        soft_limits = !soft_limits;
     }
-}
+}

src/main/java/frc4388/robot/subsystems/Claw.java

@@ -1,4 +1,5 @@
 package frc4388.robot.subsystems;
+
 import java.util.Timer;
 import java.util.TimerTask;
 
@@ -8,6 +9,7 @@ import edu.wpi.first.wpilibj.PWM;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 
 public class Claw extends SubsystemBase {
+
 	private final PWM         m_leftMotor;
 	private final PWM         m_rightMotor;
 	private final CANSparkMax m_spinnyspin;

src/main/java/frc4388/robot/subsystems/Limelight.java

@@ -4,23 +4,33 @@
 
 package frc4388.robot.subsystems;
 
+import java.io.IOException;
 import java.util.ArrayList;
 import java.util.List;
+import java.util.Optional;
 
-import org.opencv.core.Point;
+import org.photonvision.EstimatedRobotPose;
 import org.photonvision.PhotonCamera;
+import org.photonvision.PhotonPoseEstimator;
+import org.photonvision.PhotonPoseEstimator.PoseStrategy;
 import org.photonvision.common.hardware.VisionLEDMode;
 import org.photonvision.targeting.PhotonPipelineResult;
 import org.photonvision.targeting.PhotonTrackedTarget;
 import org.photonvision.targeting.TargetCorner;
 
+import edu.wpi.first.apriltag.AprilTag;
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.VisionConstants;
-import frc4388.utility.AbhiIsADumbass;
 
 public class Limelight extends SubsystemBase {
+  
   private PhotonCamera cam;
+  private PhotonPoseEstimator photonPoseEstimator;
 
   private boolean lightOn;
 
@@ -28,8 +38,6 @@ public class Limelight extends SubsystemBase {
   public Limelight() {
     cam = new PhotonCamera(VisionConstants.NAME);
     cam.setDriverMode(false);
-    
-    setToLimePipeline();
   }
 
   public void setLEDs(boolean on) {
@@ -48,44 +56,15 @@ public class Limelight extends SubsystemBase {
 
   public void setToLimePipeline() {
     cam.setPipelineIndex(1);
+    setLEDs(true);
   }
 
   public void setToAprilPipeline() {
     cam.setPipelineIndex(0);
+    setLEDs(false);
   }
 
-  public ArrayList<Point> getTargetPoints() {
-    if (!cam.isConnected()) return null;
-
-    PhotonPipelineResult result = cam.getLatestResult();
-
-    if (!result.hasTargets()) return null;
-
-    ArrayList<Point> points = new ArrayList<>(2);
-
-    for(PhotonTrackedTarget target : result.getTargets()) {
-      List<TargetCorner> corners = target.getDetectedCorners();
-  
-      double sumX = 0.0;
-      double sumY = 0.0;
-      double mx = 0.0;
-      double my = 0.0;
-  
-      for (TargetCorner c : corners) {
-        sumX += c.x;
-        sumY += c.y;
-      }
-  
-      mx = sumX / 4.0;
-      my = sumY / 4.0;
-
-      points.add(new Point(mx, my));
-    }
-
-    return points;
-  }
-
-  public PhotonTrackedTarget getFirstTargetPoint() {
+  public PhotonTrackedTarget getAprilPoint() {
     if (!cam.isConnected()) return null;
 
     PhotonPipelineResult result = cam.getLatestResult();
@@ -95,40 +74,92 @@ public class Limelight extends SubsystemBase {
     return result.getBestTarget();
   }
 
-  private double getPointAngle(Point point) {
-    return (VisionConstants.LIME_VIXELS - point.y) * (VisionConstants.V_FOV / VisionConstants.LIME_VIXELS);
+  private List<TargetCorner> getAprilCorners() {
+    if (!cam.isConnected()) return null;
+
+    PhotonPipelineResult result = cam.getLatestResult();
+
+    if (!result.hasTargets()) return null;
+
+    return result.getBestTarget().getDetectedCorners();
   }
 
-  public double getHorizontalDistanceToTarget(boolean high) {
-    ArrayList<Point> targetPoints = getTargetPoints();
-    if (targetPoints == null) return -1;
+  public double getAprilSkew() {
+    List<TargetCorner> corners = getAprilCorners();
+    ArrayList<TargetCorner> bottomSide = getAprilBottomSide(corners);
 
-    // Point highPoint = targetPoints.get(0).y <= targetPoints.get(1).y ? targetPoints.get(0) : targetPoints.get(1);
-    // Point midPoint = targetPoints.get(0).y >= targetPoints.get(1).y ? targetPoints.get(0) : targetPoints.get(1);
+    if (bottomSide == null) return 0;
 
-    PhotonTrackedTarget tapePoint = getFirstTargetPoint();//high ? highPoint : midPoint;
-    double tapeHeight = VisionConstants.MID_TAPE_HEIGHT;//high ? VisionConstants.HIGH_TAPE_HEIGHT : VisionConstants.MID_TAPE_HEIGHT;
+    TargetCorner bottomRight = bottomSide.get(0).x > bottomSide.get(1).x ? bottomSide.get(0) : bottomSide.get(1);
+    TargetCorner bottomLeft = bottomRight.x == bottomSide.get(0).x ? bottomSide.get(1) : bottomSide.get(0);
 
-    double theta = 35.0 + tapePoint.getPitch();
-
-    double effectiveTapeHeight = tapeHeight - VisionConstants.LIME_HEIGHT;
-
-    double horizontalDistanceToTarget = effectiveTapeHeight / Math.tan(Math.toRadians(theta));
-
-    return horizontalDistanceToTarget;
+    return bottomLeft.y - bottomRight.y;
   }
 
-  int ctr = 0;
+  private ArrayList<TargetCorner> getAprilBottomSide(List<TargetCorner> box) {
+    if (box == null) return null;
 
-  @Override
-  public void periodic() {
-    // This method will be called once per scheduler run
+    ArrayList<TargetCorner> bottomSide = new ArrayList<>();
+
+    TargetCorner l1 = new TargetCorner(-1, -1);
+    TargetCorner l2 = new TargetCorner(-1, -1);
     
-    if (ctr % 50 == 0) {
-      SmartDashboard.putNumber("Horizontal Distance", getHorizontalDistanceToTarget(false));
+    for (TargetCorner c : box) {
+      if (c.y > l1.y) l1 = c;
     }
 
-    ctr++;
+    for (TargetCorner c : box) {
+      if (c.y == l1.y) continue;
+      if (c.y > l2.y) l2 = c;
+    }
 
+    bottomSide.add(l1);
+    bottomSide.add(l2);
+
+    return bottomSide;
   }
+
+  public double getDistanceToApril() {    
+    PhotonTrackedTarget aprilPoint = getAprilPoint();
+    if (aprilPoint == null) return -1;
+
+    double aprilHeight = VisionConstants.APRIL_HEIGHT - VisionConstants.LIME_HEIGHT;
+    double theta = 35.0 + aprilPoint.getPitch();
+
+    double distanceToApril = aprilHeight / Math.tan(Math.toRadians(theta));
+    return distanceToApril;
+  }
+
+  public PhotonTrackedTarget getLowestTape() {
+    if (!cam.isConnected()) return null;
+
+    PhotonPipelineResult result = cam.getLatestResult();
+
+    if (!result.hasTargets()) return null;
+
+    ArrayList<PhotonTrackedTarget> points = (ArrayList<PhotonTrackedTarget>) result.getTargets();
+
+    PhotonTrackedTarget lowest = points.get(0);
+    for (PhotonTrackedTarget point : points) {
+      if (point.getPitch() < lowest.getPitch()) {
+        lowest = point;
+      }
+    }
+
+    return lowest;
+  }
+
+  public double getDistanceToTape() {    
+    PhotonTrackedTarget tapePoint = getLowestTape();
+    if (tapePoint == null) return -1;
+
+    double tapeHeight = VisionConstants.MID_TAPE_HEIGHT - VisionConstants.LIME_HEIGHT;
+    double theta = 35.0 + tapePoint.getPitch();
+
+    double distanceToTape = tapeHeight / Math.tan(Math.toRadians(theta));
+    return distanceToTape;
+  }
+
+  @Override
+  public void periodic() {}
 }

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

@@ -4,16 +4,12 @@
 
 package frc4388.robot.subsystems;
 
-import edu.wpi.first.math.estimator.SwerveDrivePoseEstimator;
-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.math.kinematics.SwerveDriveKinematics;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
-import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.utility.RobotGyro;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
@@ -77,10 +73,10 @@ public class SwerveDrive extends SubsystemBase {
 
       // Use the left joystick to set speed. Apply a cubic curve and the set max speed.
       Translation2d speed = leftStick.times(leftStick.getNorm() * speedAdjust);
-      Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
+      // Translation2d cubedSpeed = new Translation2d(Math.pow(speed.getX(), 3.00), Math.pow(speed.getY(), 3.00));
 
       // Convert field-relative speeds to robot-relative speeds.
-      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * cubedSpeed.getX(), cubedSpeed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
+      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), speed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d().times(-1));
     } else {
       // Create robot-relative speeds.
       chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), leftStick.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
@@ -101,6 +97,19 @@ public class SwerveDrive extends SubsystemBase {
     }
   }
 
+  public boolean rotateToTarget(double angle) {
+    double currentAngle = getGyroAngle();
+    double error = angle - currentAngle;
+
+    driveWithInput(new Translation2d(0, 0), new Translation2d(error / Math.abs(error) * 0.3, 0), true);
+
+    if (Math.abs(angle - getGyroAngle()) < 5.0) {
+      return true;
+    }
+
+    return false;
+  }
+
   public double getGyroAngle() {
     return gyro.getAngle();
   }
@@ -122,7 +131,8 @@ public class SwerveDrive extends SubsystemBase {
 
   @Override
   public void periodic() {
-    // This method will be called once per scheduler run
+    // This method will be called once per scheduler run\
+    SmartDashboard.putNumber("Gyro", getGyroAngle());
   }
 
   public void shiftDown() {

src/main/java/frc4388/robot/subsystems/Vision.java

@@ -3,7 +3,6 @@ package frc4388.robot.subsystems;
 import edu.wpi.first.apriltag.AprilTag;
 import edu.wpi.first.math.geometry.Pose3d;
 import edu.wpi.first.math.geometry.Rotation3d;
-import edu.wpi.first.networktables.NetworkTable;
 import edu.wpi.first.networktables.NetworkTableEntry;
 import edu.wpi.first.networktables.NetworkTableInstance;
 

src/main/java/frc4388/utility/controller/DeadbandedXboxController.java

@@ -4,7 +4,6 @@ import static frc4388.robot.Constants.OIConstants.LEFT_AXIS_DEADBAND;
 
 import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.XboxController;
-import frc4388.robot.Constants.OIConstants;
 
 public class DeadbandedXboxController extends XboxController {
   public DeadbandedXboxController(int port) { super(port); }
@@ -21,11 +20,6 @@ public class DeadbandedXboxController extends XboxController {
     Translation2d translation2d = new Translation2d(x, y);
     double magnitude = translation2d.getNorm();
 
-    // if (OIConstants.SKEW_STICKS && magnitude >= 1) {
-    //   System.out.println("if statement running");
-    //   return translation2d.div(magnitude);
-    // }
-
     if (magnitude < LEFT_AXIS_DEADBAND) return new Translation2d(0,0);
 
     return translation2d;

src/main/java/frc4388/utility/controller/XboxTriggerButton.java

@@ -1,7 +1,6 @@
 package frc4388.utility.controller;
 
 import edu.wpi.first.wpilibj2.command.button.Button;
-import frc4388.utility.controller.XboxController;
 
 /**
  * Mapping for the Xbox controller triggers to allow triggers to be defined as