Merge pull request #33 from IgniteRobotics/feature/intake-improvements

Feature/intake improvements

Author: Orcasphynx

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

@@ -23,7 +23,6 @@
 import frc.robot.subsystems.indexer.IndexerSubsystem;
 import frc.robot.subsystems.intake.IntakeSubsystem;
 import frc.robot.subsystems.shooter.ShooterSubsystem;
-import frc.robot.subsystems.shooter.SimpleShooterSubsystem;
 
 @Logged
 public class RobotContainer {
@@ -46,7 +45,6 @@ public class RobotContainer {
   @Logged(name = "Shooter")
   public final ShooterSubsystem shooter = new ShooterSubsystem();
 
-  public final SimpleShooterSubsystem simpleShooter = new SimpleShooterSubsystem();
 
   private final SendableChooser<Command> autoChooser;
 
@@ -105,10 +103,10 @@ public void configureTestBindings() {
     driverJoystick.rightBumper().onTrue(Commands.runOnce(SignalLogger::start));
     driverJoystick.leftBumper().onTrue(Commands.runOnce(SignalLogger::stop));
 
-    operatorJoystick.y().whileTrue(simpleShooter.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
-    operatorJoystick.a().whileTrue(simpleShooter.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
-    operatorJoystick.b().whileTrue(simpleShooter.sysIdDynamic(SysIdRoutine.Direction.kForward));
-    operatorJoystick.x().whileTrue(simpleShooter.sysIdDynamic(SysIdRoutine.Direction.kReverse));
+    operatorJoystick.y().whileTrue(shooter.sysIdQuasistatic(SysIdRoutine.Direction.kForward));
+    operatorJoystick.a().whileTrue(shooter.sysIdQuasistatic(SysIdRoutine.Direction.kReverse));
+    operatorJoystick.b().whileTrue(shooter.sysIdDynamic(SysIdRoutine.Direction.kForward));
+    operatorJoystick.x().whileTrue(shooter.sysIdDynamic(SysIdRoutine.Direction.kReverse));
 
     // Reset the field-centric heading on left bumper press.
     driverJoystick.start().onTrue(drivetrain.runOnce(drivetrain::seedFieldCentric));
@@ -140,7 +138,7 @@ public void configureTeleopBindings() {
     driverJoystick
         .leftTrigger()
         .whileTrue(shooter.launchLemonsCommand())
-        .onFalse(simpleShooter.stopLaunchLemonsNoPIDCommand());
+        .onFalse(shooter.stopLaunchLemonsNoPIDCommand());
 
     driverJoystick
         .rightTrigger()
@@ -149,8 +147,8 @@ public void configureTeleopBindings() {
 
     operatorJoystick
         .leftTrigger()
-        .whileTrue(simpleShooter.launchLemonsCommandNoPID())
-        .onFalse(simpleShooter.stopLaunchLemonsNoPIDCommand());
+        .whileTrue(shooter.launchLemonsCommandNoPID())
+        .onFalse(shooter.stopLaunchLemonsNoPIDCommand());
 
     operatorJoystick
         .rightTrigger()

src/main/java/frc/robot/subsystems/intake/IntakeConstants.java

@@ -1,7 +1,9 @@
 package frc.robot.subsystems.intake;
 
+import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.configs.SoftwareLimitSwitchConfigs;
+import com.ctre.phoenix6.signals.NeutralModeValue;
 
 public class IntakeConstants {
 
@@ -53,6 +55,13 @@ public static SoftwareLimitSwitchConfigs createExtensionSoftwareLimitSwitchConfi
     return configs;
   }
 
+  public static MotorOutputConfigs createExtensionMotorOutputConfigs() {
+    MotorOutputConfigs newConfigs = new MotorOutputConfigs();
+    // newConfigs.Inverted = InvertedValue.Clockwise_Positive;
+    newConfigs.NeutralMode = NeutralModeValue.Coast;
+    return newConfigs;
+  }
+
   public static final double SAFE_HOMING_EFFORT = -0.2;
   public static final double SAFE_STATOR_LIMIT = 0.8;
 }

src/main/java/frc/robot/subsystems/intake/IntakePreferences.java

@@ -18,4 +18,7 @@ private IntakePreferences() {}
       new DoublePreference("Intake/Retract Percent (for without PID)", -0.1); // in percent
   public static DoublePreference rollerIntakePercent =
       new DoublePreference("Intake/Roller Intake Percent (for without PID)", 0.1); // in percent
+
+  public static DoublePreference dislodgePosition =
+      new DoublePreference("Intake/Extension Dislodge Position", 1.5);
 }

src/main/java/frc/robot/subsystems/intake/IntakeSubsystem.java

@@ -54,15 +54,19 @@ public IntakeSubsystem() {
     extensionMotor
         .getConfigurator()
         .apply(IntakeConstants.createExtensionSoftwareLimitSwitchConfigs());
+    extensionMotor.getConfigurator().apply(IntakeConstants.createExtensionMotorOutputConfigs());
     extensionMotor.setPosition(0);
     extensionTarget = Rotations.of(0);
     extensionControl = new PositionTorqueCurrentFOC(0);
   }
 
   @Override
   public void periodic() {
-    // rollerMotor.setControl(rollerControl.withVelocity(rollerVelocityTarget.in(RotationsPerSecond)));
-    // extensionMotor.setControl(extensionControl.withPosition(extensionTarget.in(Rotations)));
+    rollerMotor.setControl(rollerControl.withVelocity(rollerVelocityTarget.in(RotationsPerSecond)));
+    extensionMotor.setControl(
+        extensionControl
+            .withPosition(extensionTarget.in(Rotations))
+            .withOverrideCoastDurNeutral(true));
   }
 
   public Command spinRollerCommand() {
@@ -83,12 +87,12 @@ private void setRollerVoltage(double magnitude) {
   }
 
   public Command startRollerNoPID() {
-    return runOnce(() -> rollerMotor.set(IntakePreferences.rollerIntakePercent.getValue()))
+    return run(() -> rollerMotor.set(IntakePreferences.rollerIntakePercent.getValue()))
         .withName("Set Roller Percent");
   }
 
   public Command stopRollerNoPID() {
-    return runOnce(() -> rollerMotor.set(0)).withName("Stop Roller No PID");
+    return run(() -> rollerMotor.set(0)).withName("Stop Roller No PID");
   }
 
   @Logged(name = "Extension Setpoint", importance = Importance.CRITICAL)
@@ -124,11 +128,23 @@ public Command collectCommand() {
   }
 
   public Command stowCommand() {
-    return stopRollerCommand()
-        .andThen(setIntakeExtensionCommand(Rotations.of(0)))
+    return setIntakeExtensionCommand(Rotations.of(0))
+        .andThen(stopRollerCommand())
+        .andThen(setIntakeExtensionCommand(Rotations.of(0)).repeatedly())
         .withName("Stow Intake");
   }
 
+  public Command dislodgeCommand() {
+    return spinRollerCommand()
+        .andThen(
+            setIntakeExtensionCommand(Rotations.of(IntakePreferences.dislodgePosition.getValue()))
+                .andThen(
+                    setIntakeExtensionCommand(
+                        Rotations.of(IntakePreferences.intakeCollectPosition.getValue()))))
+        .repeatedly()
+        .withName("Dislodge Intake");
+  }
+
   public Command homeIntakeCommand() {
     return runEnd(
             () -> extensionMotor.set(IntakeConstants.SAFE_HOMING_EFFORT),