Remove unused classes and dependencies

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

@@ -9,7 +9,6 @@
 import com.ctre.phoenix6.mechanisms.swerve.SwerveRequest;
 import com.pathplanner.lib.auto.AutoBuilder;
 import com.pathplanner.lib.auto.NamedCommands;
-
 import edu.wpi.first.cameraserver.CameraServer;
 import edu.wpi.first.cscore.VideoSource;
 import edu.wpi.first.wpilibj.DriverStation;
@@ -30,9 +29,10 @@
 import frc.robot.commands.SetFlywheel;
 import frc.robot.commands.SetWinch;
 import frc.robot.commands.ShootClose;
-import frc.robot.experimental.ShootWhenReadyAuton;
-import frc.robot.experimental.ShootWhenReady;
 import frc.robot.commands.ShootFromStage;
+import frc.robot.commands.ShootWhenReady;
+import frc.robot.commands.ShootWhenReady2;
+import frc.robot.commands.ShootWhenReadyAuton;
 import frc.robot.subsystems.ArmSubsystem;
 import frc.robot.subsystems.CommandSwerveDrivetrain;
 import frc.robot.subsystems.Gyro;
@@ -102,20 +102,22 @@ public class RobotContainer {
 
   // Only Sets the flywheel to target velocity, no index
   private final SetFlywheel setShooterVelocity = new SetFlywheel(shooter2, arm, shooter2.FLYWHEEL_VELOCITY);
+
   // Only Sets the flywheel to idle velocity, no index
   private final SetFlywheel setShooterIdle = new SetFlywheel(shooter2, arm, shooter2.FLYWHEEL_IDLE_VELOCITY);
 
   // An updated version of the RevAndShootCommand
   private final ShootWhenReady shootWhenReady = new ShootWhenReady(shooter2, index, notesensor);
+
   // Autonomous version of the Shoot When Ready command that addeds notesensor checks for ending the command
   private final ShootWhenReadyAuton shootWhenReadyAuton = new ShootWhenReadyAuton(shooter2, index, notesensor);
+
+  // TODO Test Shoot from stage command. If it works, add to auton
   private final ShootFromStage shootFromStage = new ShootFromStage(arm, index, intake, shooter2, notesensor);
 
 
   // ChargeIntakeCommand chargeIntake = new ChargeIntakeCommand(drivetrain, intake, driveRequest);
 
-  // private final ShootFromStage shootSafetyPose = new ShootFromStage(arm, index, intake, shooter);
-
   /* Autonomous Chooser */
   SendableChooser<Command> autoChooser;
 
@@ -128,8 +130,7 @@ public RobotContainer() {
     NamedCommands.registerCommand("ShootClose2", shootWhenReadyAuton);
     NamedCommands.registerCommand("ShootFromStage", shootFromStage);
     NamedCommands.registerCommand("Intake", new IntakeIndex(index, intake));
-
-    NamedCommands.registerCommand("AutoShootWhenReady", shootWhenReadyAuton);
+    NamedCommands.registerCommand("AutoShootWhenReady", shootWhenReadyAuton); // Autonomous
     NamedCommands.registerCommand("ShootWhenReady", shootWhenReadyAuton); // Autonomous 
     NamedCommands.registerCommand("SetFlywheelToIdle", setShooterIdle);
     // Constants.ArmConstants.ARM_MID_POSE));
@@ -144,8 +145,6 @@ public RobotContainer() {
     SmartDashboard.putData("Auto Chooser", autoChooser);
     // Shuffleboard.getTab("Auton").add("Auto Chooser", autoChooser);
 
-    // TODO removed
-    // shooter = new ShooterSubsystemVelocity();
 
     /* Configure the Button Bindings */
     configureBindings();
@@ -203,18 +202,22 @@ private void configureBindings() {
     m_driverController.rightBumper().whileTrue(new IntakeCommandGroup(index, intake));
     m_driverController.leftBumper().whileTrue(new IntakeRevCommandGroup(index, intake));
     m_driverController.rightTrigger().whileTrue(shootWhenReady);
-    // m_driverController.rightTrigger().whileTrue(setShooterVelocity); // TODO Testing purposes, remove later
-    // m_driverController.rightTrigger().whileTrue(autonShootWhenReady); // TODO Testing purposes, remove later
+
+    // TODO Test "setShooterVelocity" only, then remove
+    // m_driverController.rightTrigger().whileTrue(setShooterVelocity); 
+
+    // TODO Testing "autonShootWhenReady" Remove later
+    // m_driverController.rightTrigger().whileTrue(autonShootWhenReady); 
     m_driverController.leftTrigger().whileTrue(new SetIndex(index, -0.75));
 
-    // m_driverController.rightTrigger().whileTrue(new RevAndShootCommand(index, shooter)); /* Previous bindings */
-    // m_driverController.rightTrigger().whileFalse(new InstantCommand(() -> shooter.SetOutput(0))); /* Previous bindings */
+    /* TODO These are the rrevious event commands and bindings */
+    // m_driverController.rightTrigger().whileTrue(new RevAndShootCommand(index, shooter)); 
+    // m_driverController.rightTrigger().whileFalse(new InstantCommand(() -> shooter.SetOutput(0)));
 
     /* OPERATOR BINDINGS */
     m_operatorController.b().whileTrue(new SetArmClimb(arm, Constants.ArmConstants.ARM_MANUAL_POWER));
     m_operatorController.y().whileTrue(new SetWinch(winch, Constants.WinchConstants.WINCH_POWER));
     m_operatorController.back().whileTrue(new SetWinch(winch, Constants.WinchConstants.WINCH_POWER_BOOST));
-    // m_operatorController.start().whileTrue();
   };
 
   /* Use for Debugging and diagnostics purposes */
@@ -230,7 +233,6 @@ public void DebugMethodSingle() {
     // arm.showArmTelemetry("Driver Diagnostics");
     // Shuffleboard.getTab("Arm").add("Arm Output", arm);
 
-
     SmartDashboard.putNumber("Yaw", pidgey.getYaw());
     SmartDashboard.putNumber("Angle", pidgey.getAngle());
     SmartDashboard.putNumber("Rotation2d", pidgey.Rotation2d());

src/main/java/frc/robot/experimental/IntakeIndexTimer.java → src/main/java/frc/robot/commands/IntakeIndexTimer.java

@@ -1,8 +1,6 @@
-package frc.robot.experimental;
+package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
-import frc.robot.commands.SetIndex;
-import frc.robot.commands.SetIntake;
 import frc.robot.subsystems.IndexSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.util.Constants;

src/main/java/frc/robot/experimental/SetIntake2.java → src/main/java/frc/robot/commands/SetIntake2.java

@@ -1,4 +1,4 @@
-package frc.robot.experimental;
+package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.IntakeSubsystem;

src/main/java/frc/robot/commands/ShootFromStage.java

@@ -1,7 +1,6 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
-import frc.robot.experimental.ShootWhenReady;
 import frc.robot.subsystems.ArmSubsystem;
 import frc.robot.subsystems.IndexSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
@@ -15,8 +14,10 @@ public ShootFromStage(ArmSubsystem arm, IndexSubsystem index, IntakeSubsystem in
         addCommands(
             // Set the arm to the middle position for shooting from the closest Stage leg
             new SetArmPosition(arm, Constants.ArmConstants.ARM_MID_POSE),
+
             // Check target velocity and index the game piece forward
             new ShootWhenReady(shooter2, index, notesensor),
+
             // Set arm back to home position
             new SetArmPosition(arm, Constants.ArmConstants.ARM_HOME_POSE)
         );

src/main/java/frc/robot/experimental/ShootWhenReady.java → src/main/java/frc/robot/commands/ShootWhenReady.java

@@ -1,4 +1,4 @@
-package frc.robot.experimental;
+package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.IndexSubsystem;
@@ -22,7 +22,7 @@ public ShootWhenReady(ShooterSubsystem2 shooter2, IndexSubsystem index, NoteSens
 
     @Override
     public void initialize() {
-        // shooter2.setFlywheelToIdle();
+        shooter2.setFlywheelToIdle();
     }
 
     @Override

src/main/java/frc/robot/experimental/ShootWhenReady2.java → src/main/java/frc/robot/commands/ShootWhenReady2.java

@@ -1,13 +1,11 @@
-package frc.robot.experimental;
+package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.IndexSubsystem;
 import frc.robot.subsystems.ShooterSubsystem2;
 import frc.robot.util.Constants;
 import frc.robot.subsystems.NoteSensorSubsystem;
 
-@SuppressWarnings("unused")
-
 public class ShootWhenReady2 extends Command{
     private ShooterSubsystem2 shooter2;
     private IndexSubsystem index;

src/main/java/frc/robot/experimental/ShootWhenReadyAuton.java → src/main/java/frc/robot/commands/ShootWhenReadyAuton.java

@@ -1,4 +1,4 @@
-package frc.robot.experimental;
+package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import frc.robot.subsystems.ArmSubsystem;

src/main/java/frc/robot/subsystems/ArmIO.java.txt

@@ -0,0 +1,58 @@
+package frc.robot.subsystems;
+
+import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
+import com.ctre.phoenix6.configs.MotionMagicConfigs;
+import com.ctre.phoenix6.configs.Slot0Configs;
+import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.GravityTypeValue;
+import frc.robot.util.Constants;
+
+public class ArmIO {
+    private final TalonFX m_arm;
+
+    public ArmIO() {
+        m_arm = new TalonFX(Constants.CANIDs.ARM_ID);
+        m_arm.getConfigurator().apply(new TalonFXConfiguration());
+
+        /* Gains or configuration of arm motor for config slot 0 */
+        var armGains0 = new Slot0Configs();
+        armGains0.GravityType = GravityTypeValue.Arm_Cosine; /* .Elevator_Static | .Arm_Cosine */
+        m_arm.setInverted(true); // Set to true if you want to invert the motor direction
+        armGains0.kP = 0.50; /* Proportional Gain */
+        armGains0.kI = 0.00; /* Integral Gain */
+        armGains0.kD = 0.00; /* Derivative Gain */
+        armGains0.kV = 0.00; /* Velocity Feed Forward Gain */
+        armGains0.kS = 0.00; /* Static Feed Forward Gain */
+        armGains0.kA = 0.00; /* Acceleration Feedforward */
+        armGains0.kG = 0.00; /* Gravity Feedfoward */
+
+        // set Motion Magic settings
+        var armMotionMagic0 = new MotionMagicConfigs();
+        armMotionMagic0.MotionMagicCruiseVelocity = Constants.ArmConstants.ARM_MAX_VEL; 
+        armMotionMagic0.MotionMagicAcceleration = Constants.ArmConstants.ARM_MAX_ACCEL; 
+        armMotionMagic0.MotionMagicJerk = Constants.ArmConstants.ARM_JERK;
+
+        var armSoftLimit0 = new SoftwareLimitSwitchConfigs();
+        armSoftLimit0.ForwardSoftLimitEnable = true;
+        armSoftLimit0.ForwardSoftLimitThreshold = Constants.ArmConstants.ARM_FWD_LIMIT;
+        armSoftLimit0.ReverseSoftLimitEnable = true;
+        armSoftLimit0.ReverseSoftLimitThreshold = Constants.ArmConstants.ARM_REV_LIMIT;
+
+        var armCurrent0 = new CurrentLimitsConfigs();
+        armCurrent0.StatorCurrentLimitEnable = true;
+        armCurrent0.StatorCurrentLimit = Constants.ArmConstants.ARM_STATOR_LIMIT;
+        armCurrent0.SupplyCurrentLimitEnable = true;
+        armCurrent0.SupplyCurrentLimit = Constants.ArmConstants.ARM_SUPPLY_LIMIT;
+
+        /*
+         * Long form (better for my learning): Applies gains with an optional 50 ms
+         * timeout (I think)
+         */
+        m_arm.getConfigurator().apply(armGains0, 0.050);
+        m_arm.getConfigurator().apply(armMotionMagic0, 0.050);
+        m_arm.getConfigurator().apply(armSoftLimit0, 0.050);
+        m_arm.getConfigurator().apply(armCurrent0, 0.050);
+    }        
+} // end of class ArmIO

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

@@ -4,7 +4,6 @@
 import com.ctre.phoenix6.configs.CurrentLimitsConfigs;
 import com.ctre.phoenix6.configs.MotionMagicConfigs;
 import com.ctre.phoenix6.configs.Slot0Configs;
-import com.ctre.phoenix6.configs.Slot1Configs;
 import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.MotionMagicVoltage;
@@ -53,29 +52,11 @@ public ArmSubsystem() {
         armGains0.kA = 0.00; /* Acceleration Feedforward */
         armGains0.kG = 0.00; /* Gravity Feedfoward */
 
-        /* Gains or configuration of arm motor for config slot 1 */
-        var armGains1 = new Slot1Configs();
-        armGains1.GravityType = GravityTypeValue.Arm_Cosine; /* .Elevator_Static | .Arm_Cosine */
-        m_arm.setInverted(true); // Set to true if you want to invert the motor direction
-        armGains1.kP = 0.50; /* Proportional Gain */
-        armGains1.kI = 0.00; /* Integral Gain */
-        armGains1.kD = 0.00; /* Derivative Gain */
-        armGains1.kV = 0.00; /* Velocity Feed Forward Gain */
-        armGains1.kS = 0.00; /*
-                              * Static Feed Forward Gain // Approximately 0.25V to get the mechanism moving
-                              */
-        armGains1.kA = 0.00; /* Acceleration Feedforward */
-        armGains1.kG = 0.00; /* Gravity Feedfoward */
-
         // set Motion Magic settings
         var armMotionMagic0 = new MotionMagicConfigs();
-        armMotionMagic0.MotionMagicCruiseVelocity = Constants.ArmConstants.ARM_MAX_VEL; // 80 rps cruise velocity //
-                                                                                        // FIMXE changed for safety
-                                                                                        // testing
-        armMotionMagic0.MotionMagicAcceleration = Constants.ArmConstants.ARM_MAX_ACCEL; // 160 rps/s acceleration (0.5
-                                                                                        // seconds) // FIMXE changed for
-                                                                                        // safety testing
-        armMotionMagic0.MotionMagicJerk = Constants.ArmConstants.ARM_JERK; // 1600 rps/s^2 jerk (0.1 seconds)
+        armMotionMagic0.MotionMagicCruiseVelocity = Constants.ArmConstants.ARM_MAX_VEL; 
+        armMotionMagic0.MotionMagicAcceleration = Constants.ArmConstants.ARM_MAX_ACCEL; 
+        armMotionMagic0.MotionMagicJerk = Constants.ArmConstants.ARM_JERK;
 
         var armSoftLimit0 = new SoftwareLimitSwitchConfigs();
         armSoftLimit0.ForwardSoftLimitEnable = true;
@@ -131,29 +112,6 @@ public void setArmForClimb(double power) {
 
 
     }
-
-    /* I think this is all unused code */
-    /*
-    double targetPosition = 10;
-
-    public void moveArmToPositionAndStop(double targetPosition) {
-        // Move the arm to the target position
-        m_arm.setControl(new PositionDutyCycle(targetPosition));
-
-        // Wait until the arm reaches the target position
-        while (Math.abs(m_arm.getPosition().getValue() - targetPosition) > 1) {
-            // You might want to add a delay here to prevent the loop from running too fast
-    }
-
-    // Stop the arm
-    m_arm.setControl(new PositionDutyCycle(0));
-    }
-    */
-
-    /*
-     * Currently only being called in subsystem-command;
-     * inspite of my efforts it only appears once it's triggered at least once
-     */
     /* 
     public void showArmTelemetry() {