Merge branch 'Prep-For-Denver' into DeBounce

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

@@ -7,12 +7,10 @@
 
 package frc4388.robot;
 
-import edu.wpi.first.math.trajectory.TrapezoidProfile;
-
 import edu.wpi.first.math.geometry.Translation2d;
-
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
-import frc4388.utility.LEDPatterns;
 import frc4388.utility.Gains;
+import frc4388.utility.LEDPatterns;
 
 /**
  * The Constants class provides a convenient place for teams to hold robot-wide numerical or boolean
@@ -40,10 +38,10 @@ public final class Constants {
         public static final double TURBO_SPEED = 4.0;
     
         public static final class DefaultSwerveRotOffsets {
-          public static final double FRONT_LEFT_ROT_OFFSET = 216.869140625;
+          public static final double FRONT_LEFT_ROT_OFFSET = 220;
-          public static final double FRONT_RIGHT_ROT_OFFSET = 229.4826875;//-212.695 + 90; //77.783 + 45 + 90 ;//-202.588;
+          public static final double FRONT_RIGHT_ROT_OFFSET = 136.365;//-212.695 + 90; //77.783 + 45 + 90 ;//-202.588;
-          public static final double BACK_LEFT_ROT_OFFSET = -277.646484375;
+          public static final double BACK_LEFT_ROT_OFFSET = -279.08349884;
-          public static final double BACK_RIGHT_ROT_OFFSET = 140.009765625;
+          public static final double BACK_RIGHT_ROT_OFFSET = 140.8887656;
         }
 
         public static final class IDs {
@@ -191,8 +189,8 @@ public final class Constants {
 
     public static final class ClimbConstants {
         public static final int CLIMB_MOTOR_ID = 19;
-        public static final double CLIMB_OUT_SPEED = 0.3;
+        public static final double CLIMB_IN_SPEED = -1.0;
-        public static final double CLIMB_IN_SPEED = -0.3;
+        public static final double CLIMB_OUT_SPEED = 0.87;
     }
 
     public static final class OIConstants {

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

@@ -87,9 +87,9 @@ public class RobotContainer {
 
     private ParallelCommandGroup intakeToShoot = new ParallelCommandGroup(
         new InstantCommand(() -> m_robotIntake.talonPIDIn()),
-        new InstantCommand(() -> m_robotShooter.idle()),
+        new InstantCommand(() -> m_robotShooter.idle())
-        new InstantCommand(() -> m_driverXbox.setRumble(null, 1.0)).andThen(new WaitCommand(1)).andThen(new InstantCommand(() -> m_driverXbox.setRumble(null, 0.0)))
+        // new InstantCommand(() -> m_driverXbox.setRumble(RumbleType.kRightRumble, 1.0)).andThen(new WaitCommand(0.2)).andThen(new InstantCommand(() -> m_driverXbox.setRumble(RumbleType.kRightRumble, 0.0))),
-        //new InstantCommand(() -> m_robotShooter.spin())
+        // new InstantCommand(() -> m_robotShooter.spin())
     );
 
     // private SequentialCommandGroup outtakeToShootFull = new SequentialCommandGroup(
@@ -133,8 +133,8 @@ public class RobotContainer {
     );
 
     private SequentialCommandGroup turnOffShoot = new SequentialCommandGroup(
-        new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter),
+        new InstantCommand(() -> m_robotShooter.stop(), m_robotShooter)
-        new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
+        // new InstantCommand(() -> m_robotIntake.talonStopIntakeMotors(), m_robotIntake)
     );
 
     private SequentialCommandGroup emergencyRetract = new SequentialCommandGroup(
@@ -327,34 +327,46 @@ public class RobotContainer {
         new JoystickButton(getDeadbandedDriverController(), XboxController.A_BUTTON)
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroFlip(), m_robotSwerveDrive));
 
-        new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.RIGHT_TRIGGER_AXIS) > 0.5)
-            .onTrue(new InstantCommand(() -> m_robotClimber.climbOut()))
-            .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb()));
-
-        new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_TRIGGER_AXIS) > 0.5)
-            .onTrue(new InstantCommand(() -> m_robotClimber.climbIn()))
-            .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb()));
-
         new Trigger(() -> getDeadbandedDriverController().getPOV() == 0)
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightBlue()));
 
         new Trigger(() -> getDeadbandedDriverController().getPOV() == 180)
             .onTrue(new InstantCommand(() -> m_robotSwerveDrive.resetGyroRightAmp()));
 
+       // *  /* D-Pad Stuff */
+       new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > 0.9)
+            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1),
+                                                                               new Translation2d(0, 0),
+                                                                               true)))
+            .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), 
+                                                                                new Translation2d(0, 0), 
+                                                                                true)));
+
+       new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.TOP_BOTTOM_DPAD_AXIS) > -0.9)
+            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, -1),
+                                                                               new Translation2d(0, 0),
+                                                                               true)))
+            .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), 
+                                                                                new Translation2d(0, 0), 
+                                                                                true)));
+
+       new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > 0.9)
+            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(1, 0),
+                                                                               new Translation2d(0, 0),
+                                                                               true)))
+            .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), 
+                                                                                new Translation2d(0, 0), 
+                                                                                true)));
+
+       new Trigger(() -> getDeadbandedDriverController().getRawAxis(XboxController.LEFT_RIGHT_DPAD_AXIS) > -0.9)
+            .onTrue(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(-1, 0),
+                                                                               new Translation2d(0, 0),
+                                                                               true)))
+            .onFalse(new InstantCommand(() -> m_robotSwerveDrive.driveWithInput(new Translation2d(0, 0), 
+                                                                                new Translation2d(0, 0), 
+                                                                                true)));
+        
         // ! /* Auto Recording */
-        // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
-        //    .whileTrue(new JoystickRecorder(m_robotSwerveDrive,
-        //                                    () -> getDeadbandedDriverController().getLeftX(),
-        //                                    () -> getDeadbandedDriverController().getLeftY(),
-        //                                    () -> getDeadbandedDriverController().getRightX(),
-        //                                    () -> getDeadbandedDriverController().getRightY(),
-        //                                    "Taxi.txt"))
-        //    .onFalse(new InstantCommand());
-
-        // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
-        //    .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"))
-        //    .onFalse(new InstantCommand()); 
-
         new JoystickButton(m_autoRecorderXbox, XboxController.LEFT_BUMPER_BUTTON)
            .whileTrue(new neoJoystickRecorder(m_robotSwerveDrive,
                         new DeadbandedXboxController[]{getDeadbandedDriverController(), getDeadbandedOperatorController()},
@@ -368,6 +380,18 @@ public class RobotContainer {
            true, false))
            .onFalse(new InstantCommand());
 
+        // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON)
+                //    .whileTrue(new JoystickRecorder(m_robotSwerveDrive,
+                //                                    () -> getDeadbandedDriverController().getLeftX(),
+                //                                    () -> getDeadbandedDriverController().getLeftY(),
+                //                                    () -> getDeadbandedDriverController().getRightX(),
+                //                                    () -> getDeadbandedDriverController().getRightY(),
+                //                                    "Taxi.txt"))
+                //    .onFalse(new InstantCommand());
+
+                // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON)
+                //    .onTrue(new JoystickPlayback(m_robotSwerveDrive, "Taxi.txt"))
+                //    .onFalse(new InstantCommand()); 
         // ! /* Speed */
         // new JoystickButton(getDeadbandedDriverController(), XboxController.RIGHT_BUMPER_BUTTON) // final
         //     .onTrue(new InstantCommand(()  -> m_robotSwerveDrive.shiftUp()));
@@ -376,11 +400,11 @@ public class RobotContainer {
         // new JoystickButton(getDeadbandedDriverController(), XboxController.LEFT_BUMPER_BUTTON) // final
         //     .onTrue(new InstantCommand(() -> m_robotSwerveDrive.shiftDown()));
         
-        // new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
+        new JoystickButton(getDeadbandedDriverController(), XboxController.Y_BUTTON)
-        //     .whileTrue(new InstantCommand(() -> 
+            .whileTrue(new InstantCommand(() -> 
-        //     m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1),
+            m_robotSwerveDrive.driveWithInput(new Translation2d(0, 1),
-        //                                       new Translation2d(0, 0),
+                                              new Translation2d(0, 0),
-        //                         true), m_robotSwerveDrive));
+                                true), m_robotSwerveDrive));
 
         
        //?  /* Operator Buttons */
@@ -430,8 +454,12 @@ public class RobotContainer {
             .onTrue(emergencyRetract);
 
         new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.RIGHT_TRIGGER_AXIS) > 0.5)
-            .onTrue(new InstantCommand(() -> m_robotShooter.idle()))
+            .onTrue(new InstantCommand(() -> m_robotClimber.climbOut()))
-            .onFalse(new InstantCommand(() -> m_robotShooter.stop()));
+            .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb()));
+
+        new Trigger(() -> getDeadbandedOperatorController().getRawAxis(XboxController.LEFT_TRIGGER_AXIS) > 0.5)
+            .onTrue(new InstantCommand(() -> m_robotClimber.climbIn()))
+            .onFalse(new InstantCommand(() -> m_robotClimber.stopClimb()));
 
         new Trigger(() -> getDeadbandedOperatorController().getPOV() == 0)
             .onTrue(new InstantCommand(() -> m_robotClimber.climbOut()))

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

@@ -0,0 +1,107 @@
+// package frc4388.robot.commands.Autos;
+
+// import java.io.File;
+// import java.util.ArrayList;
+// import java.util.HashMap;
+
+// import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
+// import edu.wpi.first.wpilibj.shuffleboard.ComplexWidget;
+// 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.commands.Swerve.neoJoystickPlayback;
+// import frc4388.robot.subsystems.SwerveDrive;
+// import frc4388.utility.controller.VirtualController;
+
+// public class neoPlaybackChooser {
+//     private final SendableChooser<String> m_teamChosser = new SendableChooser<String>();    
+//     private final SendableChooser<String> m_possitionChosser = new SendableChooser<String>();
+//     private final SendableChooser<String> m_autoNameChosser = new SendableChooser<String>();
+
+//     private final SwerveDrive m_swerve;
+//     private final VirtualController[] m_controllers;
+//     // private final ArrayList<SendableChooser<Command>> m_choosers    = new ArrayList<>();
+//     // private       SendableChooser<Command>            m_playback    = null;
+//     private final ArrayList<ComplexWidget>            m_widgets     = new ArrayList<>();
+//     // private final HashMap<String, Command>            m_commandPool = new HashMap<>();
+    
+//     // private final File        m_dir    = new File("/home/lvuser/autos/");
+//     // private       int         m_cmdNum = 0;
+
+//     // commands
+//     private Command m_noAuto = new InstantCommand();
+    
+//     public neoPlaybackChooser(SwerveDrive swerve, VirtualController[] controllers) {
+//         m_teamChosser.addOption("Red", "red");
+//         m_teamChosser.setDefaultOption("Blue", "blue");
+//         m_teamChosser.addOption("Nuetral", "nuetral");
+//         m_possitionChosser.addOption("AMP", "amp");
+//         m_possitionChosser.setDefaultOption("Center", "center");
+//         m_possitionChosser.addOption("Source", "source");
+//         m_swerve = swerve;
+//         m_controllers = controllers;
+//     }
+
+//     public neoPlaybackChooser addOption(String name, String option) {
+//         m_autoNameChosser.addOption(name, option);
+//         return this;
+//     }
+
+//     // public PlaybackChooser buildDisplay() {
+//     //     for (int i = 0; i < 10; i++) {
+//     //         appendCommand();
+//     //     }
+//     //     m_playback = m_choosers.get(0);
+//     //     nextChooser();
+
+//     //     // ! This does not work, why?
+//     //     Shuffleboard.getTab("Auto Chooser")
+//     //         .add("Add Sequence", new InstantCommand(() -> nextChooser()))
+//     //         .withPosition(4, 0);
+//     //     return this;
+//     // }
+
+//     // This will be bound to a button for the time being
+//     public void render() {
+//         // var chooser = new SendableChooser<Command>();
+//         // // chooser.setDefaultOption("No Auto", m_noAuto);
+
+//         // m_choosers.add(chooser);
+//         ComplexWidget widget = Shuffleboard.getTab("Neo Auto Chooser")
+//             .add("Command: " + m_choosers.size(), chooser)
+//             .withSize(4, 1)
+//             .withPosition(0, m_choosers.size() - 1)
+//             .withWidget(BuiltInWidgets.kSplitButtonChooser)
+//             .withWidget(BuiltInWidgets.kComboBoxChooser);
+
+
+//         m_widgets.add(widget);
+//     }
+
+//     // public void nextChooser() {
+//     //     SendableChooser<Command> chooser = m_choosers.get(m_cmdNum++);
+
+//     //     String[] dirs = m_dir.list();
+
+//     //     if(dirs != null){ // Fix funny error
+//     //         for (String auto : dirs) {
+//     //             chooser.addOption(auto, new JoystickPlayback(m_swerve, auto));
+//     //         }
+//     //     }
+
+        
+//     //     for (var cmd_name : m_commandPool.keySet()) {
+//     //         chooser.addOption(cmd_name, m_commandPool.get(cmd_name));
+//     //     }
+//     // }
+    
+//     public String autoName() {
+//         return m_teamChosser.getSelected() + "_" + m_possitionChosser.getSelected() + "_" + m_autoNameChosser.getSelected() + ".auto";
+//     }
+
+//     public Command getCommand() {
+//         return new neoJoystickPlayback(m_swerve, autoName(), m_controllers, true, true);
+//     }
+// }

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

@@ -12,6 +12,11 @@ import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
 import frc4388.utility.UtilityStructs.AutoRecordingFrame;
 import frc4388.utility.controller.VirtualController;
 
+
+/**
+ * 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;
@@ -29,6 +34,14 @@ public class neoJoystickPlayback extends Command {
     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;
@@ -38,18 +51,44 @@ public class neoJoystickPlayback extends Command {
         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)) {
@@ -101,6 +140,9 @@ public class neoJoystickPlayback extends Command {
         }
     }
     
+    /**
+     * Unloads the auto.
+     */
     public void unloadAuto() {
         System.out.println("AUTOPLAYBACK: Auto unloaded");
         frames.clear();

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

@@ -13,6 +13,10 @@ import frc4388.utility.UtilityStructs.AutoRecordingControllerFrame;
 import frc4388.utility.UtilityStructs.AutoRecordingFrame;
 import frc4388.utility.controller.DeadbandedXboxController;
 
+/**
+ * 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;
@@ -21,6 +25,12 @@ public class neoJoystickRecorder extends Command {
     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;
@@ -30,6 +40,12 @@ public class neoJoystickRecorder extends Command {
         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);
     }

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

@@ -7,10 +7,12 @@ package frc4388.robot.subsystems;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.controls.PositionVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants;
 import frc4388.robot.Constants.ClimbConstants;
+import frc4388.robot.Constants.IntakeConstants;
 
 //! 6.5C Scoring Criteria for Stage
 
@@ -21,6 +23,7 @@ public class Climber extends SubsystemBase {
   public Climber(TalonFX climbMotor) {
     this.climbMotor = climbMotor;
     this.climbMotor.setInverted(true);
+    
     var slot0Configs = new Slot0Configs();
     slot0Configs.kP = 0.7; // An error of 0.5 rotations results in 12 V output
     slot0Configs.kI = 0.0; // no output for integrated error
@@ -30,15 +33,17 @@ public class Climber extends SubsystemBase {
   }
 
   public void climbOut() {
-    PositionVoltage request = new PositionVoltage(0);
+    //PositionVoltage request = new PositionVoltage(0);
-    climbMotor.setControl(request.withPosition(-520)); 
+    //climbMotor.setControl(request.withPosition(-520)); 
-    //climbMotor.set(Constants.ClimbConstants.CLIMB_IN_SPEED);
+    
+    climbMotor.set(Constants.ClimbConstants.CLIMB_OUT_SPEED);
   }
 
   public void climbIn() {
-    PositionVoltage request = new PositionVoltage(-520);
+    //PositionVoltage request = new PositionVoltage(-520);
-    climbMotor.setControl(request.withPosition(0)); 
+    //climbMotor.setControl(request.withPosition(0)); 
-    // climbMotor.set(Constants.ClimbConstants.CLIMB_IN_SPEED);
+    climbMotor.set(Constants.ClimbConstants.CLIMB_IN_SPEED);
+    ;
   }
 
   public void stopClimb() {
@@ -48,5 +53,6 @@ public class Climber extends SubsystemBase {
   @Override
   public void periodic() {
     // This method will be called once per scheduler run
+    SmartDashboard.putNumber("climber pos", climbMotor.getPosition().getValue());
   }
 }

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

@@ -333,7 +333,7 @@ public class Intake extends SubsystemBase {
 
     resetArmPosition();
 
-    SmartDashboard.putNumber("Pivot Position", getArmPos());
+    //SmartDashboard.putNumber("Pivot Position", getArmPos());
 
     smartDashboardOuttakeValue = SmartDashboard.getNumber("Outtake Speed", IntakeConstants.INTAKE_OUT_SPEED_UNPRESSED);
   }

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

@@ -55,8 +55,8 @@ public class Limelight extends SubsystemBase {
     
     isNearSpeaker = distance <= VisionConstants.SpeakerBubbleDistance;
 
-    SmartDashboard.putBoolean("nearSpeaker", isNearSpeaker);
+    //SmartDashboard.putBoolean("nearSpeaker", isNearSpeaker);
-    SmartDashboard.putNumber("speakerDistance", distance);
+    //SmartDashboard.putNumber("speakerDistance", distance);
   }
 
   public Pose2d getPose() {

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

@@ -99,7 +99,7 @@ public class Shooter extends SubsystemBase {
     // This method will be called once per scheduler run
   //  SmartDashboard.putNumber("Left Shooter RPM", leftShooter.getRotorVelocity().getValue());
     //SmartDashboard.putNumber("Right Shooter RPM", rightShooter.getRotorVelocity().getValue());
-    smartDashboardShooterSpeed = SmartDashboard.getNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED);
+    //smartDashboardShooterSpeed = SmartDashboard.getNumber("Shooter Speed", ShooterConstants.SHOOTER_SPEED);
 
     // If the robot is near the speaker, and is stopped, or idled, set to limelight idle speed.
     // Else if the robot is not near the speaker, then set the speed back to idle.

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

@@ -81,7 +81,7 @@ public class  SwerveDrive extends SubsystemBase {
       // 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 * speed.getX(), -1 * speed.getY(), rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED, gyro.getRotation2d());//.times(-1));
+      chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(-1 * speed.getX(), -1 * speed.getY(), rightStick.getX(), gyro.getRotation2d());//.times(-1));
     } else {      // Create robot-relative speeds.
       chassisSpeeds = new ChassisSpeeds(-1 * leftStick.getX(), -1 * leftStick.getY(), -1 * rightStick.getX() * SwerveDriveConstants.ROTATION_SPEED);
     }
@@ -218,7 +218,7 @@ public class  SwerveDrive extends SubsystemBase {
   @Override
   public void periodic() {
     // This method will be called once per scheduler run\
-    SmartDashboard.putNumber("Gyro", getGyroAngle());
+    // SmartDashboard.putNumber("Gyro", getGyroAngle());
   }
 
   public void shiftDown() {

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

@@ -4,26 +4,34 @@
 
 package frc4388.robot.subsystems;
 
+import javax.swing.text.StyleContext.SmallAttributeSet;
+
+import com.ctre.phoenix.ErrorCode;
 import com.ctre.phoenix.motorcontrol.FeedbackDevice;
 import com.ctre.phoenix.motorcontrol.RemoteSensorSource;
 import com.ctre.phoenix.motorcontrol.TalonFXControlMode;
 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.SensorInitializationStrategy;
 
 import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc4388.robot.Constants.SwerveDriveConstants;
 import frc4388.utility.Gains;
+import frc4388.utility.configurable.ConfigurableDouble;
 
 public class SwerveModule extends SubsystemBase {
     private WPI_TalonFX driveMotor;
     private WPI_TalonFX angleMotor;
     private CANCoder encoder;
-
+    private int selfid;
+    private ConfigurableDouble offsetGetter;
+    private static int swerveId = 0;
     public static Gains swerveGains = SwerveDriveConstants.PIDConstants.SWERVE_GAINS;
   
     /** Creates a new SwerveModule. */
@@ -31,7 +39,9 @@ public class SwerveModule extends SubsystemBase {
         this.driveMotor = driveMotor;
         this.angleMotor = angleMotor;
         this.encoder = encoder;
-
+        this.offsetGetter = new ConfigurableDouble("Swerve id " + swerveId, offset);
+        this.selfid = swerveId;
+         swerveId++;
         TalonFXConfiguration angleConfig = new TalonFXConfiguration();
         angleConfig.slot0.kP = swerveGains.kP;
         angleConfig.slot0.kI = swerveGains.kI;
@@ -43,12 +53,23 @@ public class SwerveModule extends SubsystemBase {
         angleConfig.primaryPID.selectedFeedbackSensor = FeedbackDevice.RemoteSensor0;
         angleMotor.configAllSettings(angleConfig);
         
+        //encoder.configSensorInitializationStrategy(SensorInitializationStrategy.BootToAbsolutePosition);
+        reset(0);
         encoder.configMagnetOffset(offset);
         
         driveMotor.setSelectedSensorPosition(0);
         driveMotor.config_kP(0, 0.2);
     }
     
+    @Override
+    public void periodic() {
+        //encoder.configMagnetOffset(offsetGetter.get());
+        //SmartDashboard.putString("Error Code: " + selfid, getstuff());
+        // SmartDashboard.putNumber("Angular Position: " + selfid, getAngle().getDegrees());
+        // SmartDashboard.putNumber("Angular Velocity: " + selfid, getAngularVel());
+        // SmartDashboard.putNumber("Drive Position: " + selfid, getDrivePos());
+        // SmartDashboard.putNumber("Drive Velocity: " + selfid, getDriveVel());
+    }
     /**
      * Get the drive motor of the SwerveModule
      * @return the drive motor of the SwerveModule
@@ -158,4 +179,9 @@ public class SwerveModule extends SubsystemBase {
     public double getVoltage() {
         return (Math.abs(angleMotor.getMotorOutputVoltage()) + Math.abs(driveMotor.getMotorOutputVoltage()));
     }
+
+    // public String getstuff() {
+    //     encoder.getPosition();
+    //     return "" + encoder.getLastError().value;
+    // }
 }

src/main/java/frc4388/utility/Configurable/ConfigurableDouble.java

@@ -1,23 +0,0 @@
-package frc4388.utility.configurable;
-
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-
-public class ConfigurableDouble {
-    private double defualtValue;
-    private String name;
-
-    /**
-     * Creates an new ConfigurableDouble through Smart Dashboard.
-     * @param name the name of the Smart Dashboard key.
-     * @param defualtValue the initilization value
-     */
-    public ConfigurableDouble(String name, double defualtValue) {
-        this.name = name;
-        this.defualtValue = defualtValue;
-        SmartDashboard.putNumber(name, defualtValue);
-    }
-
-    public double get() {
-        return SmartDashboard.getNumber(name, defualtValue);
-    }
-}

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

@@ -2,22 +2,39 @@ package frc4388.utility.controller;
 
 import edu.wpi.first.wpilibj.GenericHID;
 
+/**
+ * A virtual controller that can be bound like an standard controller.
+ * @author Zachary Wilke
+ */
 public class VirtualController extends GenericHID {
     private short m_buttonStates = 0; 
     private short m_buttonStatesLastFrame = 0; 
     private double[] m_axes = new double[6];
     private short[] m_pov = new short[1];
 
+    /**
+     * Create an virtual controller
+     * @param port virtual port (merely a formality).
+     */
     public VirtualController(int port) {
         super(port);
     }
 
+    /**
+     * Set the curent inputs to the new frames.
+     * @param axes joystick axes, (i.e. joysticks and triggers).
+     * @param buttonFlags the bit packed button states.
+     * @param pov the array of dpads.
+     */
     public void setFrame(double[] axes, short buttonFlags, short[] pov) {
         m_axes = axes;
         setOutputs(buttonFlags);
         m_pov = pov;
     }
 
+    /**
+     * Zero outs the controls.
+     */
     public void zeroControls() {
         m_axes = new double[6];
         m_buttonStates = 0;
@@ -25,6 +42,12 @@ public class VirtualController extends GenericHID {
         m_pov = new short[1];
     }
 
+    /**
+     * Gets the value of a bitflag from an int
+     * @param value int to search
+     * @param index index of bit
+     * @return if the bit is set
+     */
     public static boolean getFlag(int value, int index) {
         return ((value & 1 << index) != 0);
     }
@@ -90,6 +113,10 @@ public class VirtualController extends GenericHID {
                   Hopefully this isn't a problem            */
     }
 
+    /** 
+     * Use {@link VirtualController#setFrame} or {@link VirtualController#setOutputs}.
+     * this is an no-op overide.
+     */
     @Override
     public void setOutput(int outputNumber, boolean value) {
         // do not use
@@ -97,12 +124,20 @@ public class VirtualController extends GenericHID {
         //m_buttonStates[outputNumber - 1] = value;
     }
 
+    /**
+     * Set buttons from a packed int, if you want to set joysticks and dpad use {@link VirtualController#SetFrame} 
+     */    
     @Override
     public void setOutputs(int value) {
         m_buttonStatesLastFrame = m_buttonStates;
         m_buttonStates = (short) value;
     }
 
+    /**
+     * Why are you Setting rumble on an virtual controller?
+     * @param type the rumble type (even though it won't do anything)
+     * @param value the rumble strength (always multiplyed by 0.0)
+     */
     @Override
     public void setRumble(RumbleType type, double value) {
         System.out.println("Why are you Setting rumble on an virtual controller?");