refactoring and cleanup before going to main

src/main/java/frc/robot/RobotContainer.java

@@ -7,12 +7,10 @@
 import frc.robot.auto.AutoFactory;
 import frc.robot.commands.climb.ClimberRetractCommand;
 import frc.robot.commands.climb.ClimberSlowRetractCommand;
-import frc.robot.commands.GyroOffsetCommand;
-import frc.robot.commands.auto.drive.AimToSpeakerCommand;
-import frc.robot.commands.auto.drive.BasicAuto;
-import frc.robot.commands.auto.shoot.ShootAutoLowCommand;
-import frc.robot.commands.auto.shoot.ShootCommandAuto;
-import frc.robot.commands.auto.shoot.ShootSubCommandAuto;
+import frc.robot.commands.auto.commands.drive.AimToSpeakerCommand;
+import frc.robot.commands.auto.commands.shoot.ShootAutoLowCommand;
+import frc.robot.commands.auto.commands.shoot.ShootCommandAuto;
+import frc.robot.commands.auto.commands.shoot.ShootSubCommandAuto;
 import frc.robot.commands.climb.ClimberExtendCommand;
 import frc.robot.commands.drive.DriveCommand;
 import frc.robot.commands.drive.DriveWhileAimingCommand;
@@ -37,26 +35,17 @@
 import frc.robot.subsystems.AprilTagSubsystem;
 import frc.robot.subsystems.ClimberSubsystem;
 import frc.robot.subsystems.AdvantageScopeSubsystem;
-//import frc.robot.subsystems.ClimberSubsystem;
 import frc.robot.subsystems.DrivetrainSubsystem;
 import frc.robot.subsystems.IndexerSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.LedSubsystem;
 import frc.robot.subsystems.ShamperSubsystem;
 import frc.robot.subsystems.TrapArmSubsystem;
-// import frc.robot.subsystems.Superstructure;
-//import frc.robot.subsystems.TrapArmSubsystem;
 import frc.robot.subsystems.VisionSubsystem;
 import frc.robot.subsystems.ShamperSubsystem.ShamperSpeed;
-// import frc.robot.subsystems.Superstructure.SuperstructureState;
-//import frc.robot.util.RobotStatusCommunicator;
 import frc.robot.util.io.Dashboard;
 import com.pathplanner.lib.auto.NamedCommands;
-import com.pathplanner.lib.commands.PathPlannerAuto;
-import edu.wpi.first.networktables.GenericEntry;
 import edu.wpi.first.wpilibj.Joystick;
-import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
-import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.InstantCommand;
 import edu.wpi.first.wpilibj2.command.button.JoystickButton;
@@ -76,8 +65,6 @@ public class RobotContainer {
   private final AdvantageScopeSubsystem advantageScope;
   private final TrapArmSubsystem trapArm;
 
-  //private final Superstructure superstructure;
-
   private final AutoFactory autoFactory;
 
   private final Joystick translationJoystick;
@@ -136,13 +123,8 @@ public RobotContainer() {
     NamedCommands.registerCommand("Manual Shoot", new ShamperManualShootCommand(shamper, ShamperSpeed.SPEAKER_IDLE));
     NamedCommands.registerCommand("Manual Index", new IndexerIndexCommand(indexer));
     NamedCommands.registerCommand("ShootAutoLow", new ShootAutoLowCommand(shamper, indexer));
-    // NamedCommands.registerCommand("Gyro Offset", new GyroOffsetCommand());
 
     NamedCommands.registerCommand("Intake", new IntakeCommand(intake, indexer, shamper));
-    // NamedCommands.registerCommand("Intake", new IndexerBackupCommand(intake, indexer, shamper));
-
-
-    //advantageScope.startRecording();
 
     configureButtonBindings();
   }
@@ -245,37 +227,6 @@ private void configureButtonBindings() {
     JoystickButton trapReleaseButton = new JoystickButton(controlPanel, 3);
 
     trapReleaseButton.whileTrue(new TrapReleaseCommand(trapArm));
-
-    /*
-     *  Superstructure Position Button Bindings
-     */
-
-    // JoystickButton shamperShootButton = new JoystickButton(rotationJoystick, 1);
-    // JoystickButton shamperAmpIdleButton = new JoystickButton(rotationJoystick, 4);
-    // JoystickButton superstructureIntakeButton = new JoystickButton(rotationJoystick, 2);
-    // // JoystickButton shamperSpeakerIdleAimButon = new JoystickButton(rotationJoystick, 3);
-    // JoystickButton shamperPodiumIdleButton = new JoystickButton(rotationJoystick, 3);
-    // JoystickButton shamperDefaultButton = new JoystickButton(rotationJoystick, 5);
-
-    // shamperAmpIdleButton.onTrue(new InstantCommand(() -> superstructure.setState(SuperstructureState.AMP_IDLE)));
-    // // shamperSpeakerIdleAimButon.onTrue(new InstantCommand(() -> superstructure.setState(SuperstructureState.SPEAKER_IDLE)));
-    // superstructureIntakeButton.onTrue(new InstantCommand(() -> superstructure.setState(SuperstructureState.INTAKE)));
-    // shamperPodiumIdleButton.onTrue(new InstantCommand(() -> superstructure.setState(SuperstructureState.PODIUM_IDLE)));
-    // shamperDefaultButton.onTrue(new InstantCommand(() -> superstructure.setState(SuperstructureState.DEFAULT)));
-
-    // shamperShootButton.onTrue(new InstantCommand(() -> {
-    //   if(superstructure.getState() == SuperstructureState.AMP_IDLE) {
-    //     superstructure.setState(SuperstructureState.AMP_SCORE);
-    //   // } else if (superstructure.getState() == SuperstructureState.SPEAKER_IDLE) {
-    //   //   superstructure.setState(SuperstructureState.SPEAKER_SCORE);
-    //   } else if (superstructure.getState() == SuperstructureState.PODIUM_IDLE) {
-    //     superstructure.setState(SuperstructureState.PODIUM_SCORE);
-    //   } else {
-    //     System.out.println("WASN'T IDLING*********");
-    //   }
-    // })).onFalse(new InstantCommand(() -> superstructure.setState(SuperstructureState.DEFAULT)));
-
-    // shamperDefaultButton.onTrue(new InstantCommand(() -> superstructure.setState(SuperstructureState.DEFAULT)));
 
     /*
      * Music Player Toggle
@@ -303,8 +254,6 @@ public void precompileAuto() {
   }
   public Command getAutonomousCommand() {
     return autoFactory.getCompiledAuto();
-    //return new PathPlannerAuto("Amp 3");
-    // return new BasicAuto(drivetrain, shamper, indexer);
   }
 
   public void resetGyro(){

src/main/java/frc/robot/RobotState.java

@@ -13,7 +13,6 @@
 import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
-// import frc.robot.subsystems.Superstructure.SuperstructureState;
 import frc.robot.util.AimingCalculator;
 import frc.robot.util.io.Dashboard;
 import frc.robot.util.states.DrivetrainState;
@@ -35,8 +34,6 @@ public class RobotState {
     private boolean shamperAtGoalAngle;
     private double autoOffset;
 
-    // private SuperstructureState superstructureState;
-
     public static RobotState getInstance() {
         if (INSTANCE == null) {
             INSTANCE = new RobotState();

src/main/java/frc/robot/auto/AutoFactory.java

@@ -9,7 +9,7 @@
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.Constants;
-import frc.robot.commands.auto.drive.BasicAuto;
+import frc.robot.commands.auto.backupAuto.BasicAuto;
 import frc.robot.subsystems.DrivetrainSubsystem;
 import frc.robot.subsystems.ShamperSubsystem;
 import frc.robot.util.io.Dashboard;

src/main/java/frc/robot/commands/auto/drive/BasicAuto.java → src/main/java/frc/robot/commands/auto/backupAuto/BasicAuto.java

@@ -1,46 +1,27 @@
-package frc.robot.commands.auto.drive;
+package frc.robot.commands.auto.backupAuto;
 
-import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import edu.wpi.first.wpilibj2.command.WaitCommand;
 import frc.robot.Constants;
-import frc.robot.commands.GyroOffsetCommand;
+import frc.robot.commands.auto.commands.GyroOffsetCommand;
+import frc.robot.commands.auto.commands.drive.AutoDriveCommand;
 import frc.robot.commands.indexer.IndexerIndexCommand;
 import frc.robot.commands.shamper.ShamperAngleCommand;
 import frc.robot.commands.shamper.ShamperManualShootCommand;
 import frc.robot.subsystems.DrivetrainSubsystem;
 import frc.robot.subsystems.IndexerSubsystem;
 import frc.robot.subsystems.ShamperSubsystem;
 import frc.robot.subsystems.ShamperSubsystem.ShamperSpeed;
-import frc.robot.util.calculator.ShootingAngleCalculator;
 
 
 public class BasicAuto extends SequentialCommandGroup {
-    private String pointOnPodium;
-    private DrivetrainSubsystem drivetrain;
-    private ShamperSubsystem shamper;
-    private IndexerSubsystem indexer;
-
-
     public BasicAuto (DrivetrainSubsystem drivetrain, ShamperSubsystem shamper, IndexerSubsystem indexer) {
-        this.pointOnPodium = pointOnPodium;
-        this.drivetrain = drivetrain;
-        this.shamper = shamper;
-        this.indexer = indexer;
         addCommands(new ParallelDeadlineGroup(new WaitCommand(1.5), new ShamperAngleCommand(shamper, Constants.Shamper.Angle.SUB)),
         new ParallelDeadlineGroup(new WaitCommand(2.5), new ShamperManualShootCommand(shamper, ShamperSpeed.SPEAKER_IDLE)),
         new ParallelDeadlineGroup(new WaitCommand(1), new IndexerIndexCommand(indexer)),
         new AutoDriveCommand(drivetrain, 1.5, -0.25, 0));
         new GyroOffsetCommand(180);
 
     }
-
-
-
-
-
-
-
-
 }

src/main/java/frc/robot/commands/GyroOffsetCommand.java → src/main/java/frc/robot/commands/auto/commands/GyroOffsetCommand.java

@@ -2,14 +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 frc.robot.commands;
+package frc.robot.commands.auto.commands;
 
-import edu.wpi.first.math.geometry.Pose2d;
-import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.util.Units;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.RobotState;
-import frc.robot.util.RobotStateEstimator;
 
 public class GyroOffsetCommand extends Command {
   private double offset;
@@ -27,7 +23,6 @@ public void initialize() {}
   @Override
   public void execute() {
     RobotState.getInstance().addNavXOffset(offset);
-    //RobotStateEstimator.getInstance().resetOdometry(new Pose2d(1.33, 7.026501927468906, new Rotation2d(Units.degreesToRadians(-126.78843142290575))));
   }
 
   // Called once the command ends or is interrupted.

src/main/java/frc/robot/commands/auto/drive/AimToSpeakerCommand.java → src/main/java/frc/robot/commands/auto/commands/drive/AimToSpeakerCommand.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 frc.robot.commands.auto.drive;
+package frc.robot.commands.auto.commands.drive;
 
 import org.littletonrobotics.junction.Logger;
 import edu.wpi.first.wpilibj2.command.Command;

src/main/java/frc/robot/commands/auto/drive/AutoDriveCommand.java → src/main/java/frc/robot/commands/auto/commands/drive/AutoDriveCommand.java

@@ -1,4 +1,4 @@
-package frc.robot.commands.auto.drive;
+package frc.robot.commands.auto.commands.drive;
 
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;

src/main/java/frc/robot/commands/auto/shoot/ShootAutoLowCommand.java → src/main/java/frc/robot/commands/auto/commands/shoot/ShootAutoLowCommand.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 frc.robot.commands.auto.shoot;
+package frc.robot.commands.auto.commands.shoot;
 
 import frc.robot.subsystems.IndexerSubsystem;
 import frc.robot.subsystems.ShamperSubsystem;

src/main/java/frc/robot/commands/auto/shoot/ShootCommandAuto.java → src/main/java/frc/robot/commands/auto/commands/shoot/ShootCommandAuto.java

@@ -2,16 +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 frc.robot.commands.auto.shoot;
+package frc.robot.commands.auto.commands.shoot;
 
 import org.littletonrobotics.junction.Logger;
 
-import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import frc.robot.Constants.Shamper;
 import frc.robot.RobotState;
 import frc.robot.subsystems.IndexerSubsystem;
 import frc.robot.subsystems.ShamperSubsystem;

src/main/java/frc/robot/commands/auto/shoot/ShootSubCommandAuto.java → src/main/java/frc/robot/commands/auto/commands/shoot/ShootSubCommandAuto.java

@@ -2,24 +2,14 @@
 // 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 frc.robot.commands.auto.shoot;
+package frc.robot.commands.auto.commands.shoot;
 
-import org.littletonrobotics.junction.Logger;
-
-import edu.wpi.first.wpilibj.DriverStation;
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj2.command.Command;
-import edu.wpi.first.wpilibj2.command.ParallelDeadlineGroup;
-import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import frc.robot.Constants.Shamper;
 import frc.robot.Constants;
-import frc.robot.RobotState;
 import frc.robot.subsystems.IndexerSubsystem;
 import frc.robot.subsystems.ShamperSubsystem;
 import frc.robot.subsystems.ShamperSubsystem.ShamperSpeed;
-import frc.robot.util.AimingCalculator;
-import frc.robot.util.calculator.ShootAngleConfig;
-import frc.robot.util.calculator.ShootingAngleCalculator;
 
 public class ShootSubCommandAuto extends Command {
     private final ShamperSubsystem shamper;

src/main/java/frc/robot/commands/drive/DriveWhileAimingCommand.java

@@ -10,7 +10,6 @@
 import org.littletonrobotics.junction.Logger;
 
 import edu.wpi.first.math.filter.SlewRateLimiter;
-import edu.wpi.first.math.geometry.Rotation2d;
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.RobotState;
 import frc.robot.subsystems.DrivetrainSubsystem;

src/main/java/frc/robot/commands/intake/IntakeThenBackupCommand.java

@@ -12,20 +12,9 @@
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.ShamperSubsystem;
 
-// NOTE:  Consider using this command inline, rather than writing a subclass.  For more
-// information, see:
-// https://docs.wpilib.org/en/stable/docs/software/commandbased/convenience-features.html
 public class IntakeThenBackupCommand extends SequentialCommandGroup {
-  private IndexerSubsystem indexer;
-  private IntakeSubsystem intake;
-  private ShamperSubsystem shamper;
   /** Creates a new IntakeThenBackupCommand. */
   public IntakeThenBackupCommand(IntakeSubsystem intake, IndexerSubsystem indexer, ShamperSubsystem shamper) {
-    this.indexer = indexer;
-    this.intake = intake;
-    this.shamper = shamper;
-
-
     addCommands(
       new IntakeCommand(intake, indexer, shamper),
       new ParallelDeadlineGroup(

src/main/java/frc/robot/commands/shamper/ShamperManualShootCommand.java

@@ -1,11 +1,8 @@
 package frc.robot.commands.shamper;
 
 import edu.wpi.first.wpilibj2.command.Command;
-import frc.robot.Constants;
-import frc.robot.RobotState;
 import frc.robot.subsystems.ShamperSubsystem;
 import frc.robot.subsystems.ShamperSubsystem.ShamperSpeed;
-import frc.robot.util.calculator.ShootingAngleCalculator;
 
 public class ShamperManualShootCommand extends Command{
     private final ShamperSubsystem shamper;
@@ -35,13 +32,10 @@ public void initialize() {
 
     @Override
     public void execute(){
-
-        //shamper.setAngle(ShootingAngleCalculator.getInstance().getShooterConfig(RobotState.getInstance().getRobotPose()).getAngleDegrees());
     }
 
     @Override
     public void end(boolean interrupted) {
         shamper.stopShooter();
-        //shamper.setAngle(Constants.Shamper.Angle.DEFAULT);
     }
 }

src/main/java/frc/robot/subsystems/ClimberSubsystem.java

@@ -6,7 +6,6 @@
 import com.revrobotics.CANSparkBase.IdleMode;
 import com.revrobotics.CANSparkLowLevel.MotorType;
 
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 import frc.robot.RobotState;

src/main/java/frc/robot/subsystems/LedSubsystem.java

@@ -1,13 +1,9 @@
 package frc.robot.subsystems;
 
-import java.sql.Driver;
-
 import edu.wpi.first.wpilibj.DigitalOutput;
 import edu.wpi.first.wpilibj.DriverStation;
-import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
-import frc.robot.Robot;
 import frc.robot.RobotState;
 import frc.robot.auto.AutoFactory.Auto;
 import frc.robot.util.io.Dashboard;

src/main/java/frc/robot/subsystems/ShamperSubsystem.java

@@ -8,10 +8,8 @@
 
 import com.ctre.phoenix6.configs.ClosedLoopRampsConfigs;
 import com.ctre.phoenix6.configs.Slot0Configs;
-import com.ctre.phoenix6.configs.Slot1Configs;
 import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
-import com.ctre.phoenix6.signals.GravityTypeValue;
 import com.ctre.phoenix6.signals.NeutralModeValue;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkBase.IdleMode;
@@ -25,7 +23,6 @@
 import edu.wpi.first.wpilibj2.command.SubsystemBase;
 import frc.robot.Constants;
 import frc.robot.RobotState;
-import frc.robot.util.AimingCalculator;
 
 public class ShamperSubsystem extends SubsystemBase {
   private static TalonFX lowerMotor;