Intake subsystem/command edits

src/main/java/frc/robot/Constants.java

@@ -242,11 +242,9 @@ public static final class Ports {
     }
 
     public static final class Modes {
-      public static final IntakePowers INTAKE = new IntakePowers(.95, .75);
-      public static final IntakePowers HOLD = new IntakePowers(0, 0d);
-      public static final IntakePowers REVERSE = new IntakePowers(-.5, -.5);
-      public static final IntakePowers SHOOT_SPEAKER = new IntakePowers(0, 0.7);
-      public static final IntakePowers SHOOT_AMP = new IntakePowers(0, -1);
+      public static final IntakePowers INTAKE = new IntakePowers(.95);
+      public static final IntakePowers HOLD = new IntakePowers(0);
+      public static final IntakePowers REVERSE = new IntakePowers(-.5);
     }
 
     public static final boolean IS_BEAMBREAK = true;

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

@@ -33,12 +33,11 @@
 import frc.robot.autonomous.HeadingAngle;
 import frc.robot.autonomous.HeadingTargetLock;
 import frc.robot.commands.AccelNoteCommand;
-import frc.robot.commands.AdvancedIntakeCommand;
+import frc.robot.commands.IntakeCommand;
 import frc.robot.commands.DefaultDriveCommand;
 import frc.robot.commands.DefenseModeCommand;
 import frc.robot.commands.HaltDriveCommandsCommand;
 import frc.robot.commands.HeightCommand;
-import frc.robot.commands.IntakeCommand;
 import frc.robot.commands.MaintainShooterCommand;
 import frc.robot.commands.OuttakeCommand;
 import frc.robot.commands.PivotAngleCommand;
@@ -131,7 +130,7 @@ public RobotContainer() {
 
     // reigster commands for pathplanner
     NamedCommands.registerCommand(
-        "IntakeCommand", new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem));
+        "IntakeCommand", new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem));
     NamedCommands.registerCommand("ShootCommand", new ShootCommand(shooterSubsystem));
     NamedCommands.registerCommand(
         "ShooterRampUpCommand",
@@ -347,11 +346,11 @@ private void configureButtonBindings() {
     // INTAKE
     anthony
         .leftBumper()
-        .onTrue(new AdvancedIntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem));
+        .onTrue(new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem));
 
     jacob
         .leftBumper()
-        .onTrue(new AdvancedIntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem));
+        .onTrue(new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem));
 
     // SHOOT
     anthony
@@ -360,7 +359,7 @@ private void configureButtonBindings() {
             new AccelNoteCommand(shooterSubsystem)
                 .andThen(new ShootCommand(shooterSubsystem))
                 .andThen(
-                    new AdvancedIntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem)));
+                    new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
 
     // SHOOT OVERRIDE
     jacob
@@ -419,7 +418,7 @@ private void configureButtonBindings() {
                         ? -Setpoints.SOURCE_DEGREES
                         : Setpoints.SOURCE_DEGREES)
                 .alongWith(
-                    new AdvancedIntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem)));
+                    new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
 
     // NOTE TO SHOOTER OR SERIALIZER
     anthony

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

@@ -5,6 +5,7 @@
 package frc.robot.commands;
 
 import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.ElevatorSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
 import frc.robot.subsystems.IntakeSubsystem.IntakeMode;
 import frc.robot.subsystems.PivotSubsystem;
@@ -16,16 +17,19 @@ public class IntakeCommand extends Command {
   IntakeSubsystem intakeSubsystem;
   ShooterSubsystem shooterSubsystem;
   PivotSubsystem pivotSubsystem;
+  ElevatorSubsystem elevatorSubsystem;
 
   /** Creates a new IntakeCommand. */
   public IntakeCommand(
       IntakeSubsystem intakeSubsystem,
       ShooterSubsystem shooterSubsystem,
-      PivotSubsystem pivotSubsystem) {
+      PivotSubsystem pivotSubsystem,
+      ElevatorSubsystem elevatorSubsystem) {
     this.intakeSubsystem = intakeSubsystem;
     this.shooterSubsystem = shooterSubsystem;
     this.pivotSubsystem = pivotSubsystem;
-    addRequirements(intakeSubsystem, shooterSubsystem, pivotSubsystem);
+    this.elevatorSubsystem = elevatorSubsystem;
+    addRequirements(intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem);
     // Use addRequirements() here to declare subsystem dependencies.
     // addRequirements(intakeSubsystem, shooterSubsystem, pivotSubsystem);
   }
@@ -36,6 +40,7 @@ public void initialize() {
     pivotSubsystem.setTargetDegrees(20);
     intakeSubsystem.setIntakeMode(IntakeMode.INTAKE);
     shooterSubsystem.setShooterMode(ShooterMode.INTAKE);
+    elevatorSubsystem.setTargetHeight(0);
   }
 
   // Called every time the scheduler runs while the command is scheduled.
@@ -47,7 +52,6 @@ public void execute() {}
   public void end(boolean interrupted) {
     intakeSubsystem.setIntakeMode(IntakeSubsystem.IntakeMode.HOLD);
     shooterSubsystem.setShooterMode(ShooterMode.IDLE);
-    shooterSubsystem.haltAccelerator();
   }
 
   // Returns true when the command should end.

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

@@ -18,7 +18,7 @@ public TransferNoteCommand(
     } else if (shooterSubsystem.isShooterBeamBreakSensorTriggered()) {
       addCommands(
           new UnloadShooterCommand(shooterSubsystem, pivotSubsystem, elevatorSubsystem)
-              .andThen(new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem)));
+              .andThen(new IntakeCommand(intakeSubsystem, shooterSubsystem, pivotSubsystem, elevatorSubsystem)));
     }
   }
 }

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

@@ -17,28 +17,22 @@ public class IntakeSubsystem extends SubsystemBase {
   private final TalonFX intakeMotor;
   private final ShuffleboardTab tab = Shuffleboard.getTab("Intake");
   private IntakeMode intakeMode;
-  private IntakeMode pastMode;
-  private double timeSincePenaltyHazard;
-  private boolean pastPenalty;
 
   public enum IntakeMode {
     INTAKE(Intake.Modes.INTAKE),
     HOLD(Intake.Modes.HOLD),
-    REVERSE(Intake.Modes.REVERSE),
-    SHOOT_SPEAKER(Intake.Modes.SHOOT_SPEAKER),
-    SHOOT_AMP(Intake.Modes.SHOOT_AMP);
+    REVERSE(Intake.Modes.REVERSE);
 
-    public final IntakePowers modePowers;
+    public final IntakePowers modePowers; // rename to modePower
 
     private IntakeMode(IntakePowers modePowers) {
       this.modePowers = modePowers;
     }
   }
 
-  public record IntakePowers(double intakeSpeed, double serializerSpeed) {
-    public IntakePowers(double intakeSpeed, double serializerSpeed) {
+  public record IntakePowers(double intakeSpeed) {
+    public IntakePowers(double intakeSpeed) {
       this.intakeSpeed = intakeSpeed;
-      this.serializerSpeed = serializerSpeed;
     }
   }
 
@@ -53,8 +47,6 @@ public IntakeSubsystem() {
 
     intakeMode = IntakeMode.HOLD;
 
-    timeSincePenaltyHazard = 7;
-
     if (Config.SHOW_SHUFFLEBOARD_DEBUG_DATA) {
       tab.addDouble("intake voltage", () -> intakeMotor.getMotorVoltage().getValueAsDouble());
       tab.addString("Current Mode", () -> intakeMode.toString());