Reverse command :3

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

@@ -13,6 +13,7 @@ import frc.robot.Constants.OperatorConstants;
 import frc.robot.commands.AutoShootCommandChassisTurret;
 import frc.robot.commands.AutoShootCommandChassisTurretPathPlanner;
 import frc.robot.commands.AutoTurretCommand;
+import frc.robot.commands.Reverse;
 import frc.robot.subsystems.DriveSubsystem;
 import frc.robot.subsystems.HopperSubsystem;
 import frc.robot.subsystems.IntakeSubsystem;
@@ -82,7 +83,7 @@ public class RobotContainer {
         //m_driverController.y().whileTrue(new RelayCommand(hopperSubsystem, turretSubsystem, intakeSubsystem));
         m_driverController.start().whileTrue(intakeSubsystem.resetDeploy());
         m_driverController.x().whileTrue(new AutoShootCommandChassisTurret(driveSubsystem, hopperSubsystem, turretSubsystem));
-        m_driverController.b().onTrue(Commands.parallel(intakeSubsystem.reverseIntake(), null));
+        m_driverController.b().onTrue(new Reverse(intakeSubsystem, hopperSubsystem));
         m_driverController.y().onTrue(intakeSubsystem.deploy());
     }
 

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

@@ -12,17 +12,17 @@ import frc.robot.subsystems.IntakeSubsystem;
  */
 public class Reverse extends Command {
 
-    private int m_subsystem;
+    private IntakeSubsystem intakeSubsystem;
+    private HopperSubsystem hopperSubsystem;
 
     /**
      * Creates a new ExampleCommand.
      *
      * @param subsystem The subsystem used by this command.
      */
-    public Reverse() {
+    public Reverse(IntakeSubsystem intakeSubsystem, HopperSubsystem hopperSubsystem) {
-        m_subsystem = subsystem;
+        this.intakeSubsystem = intakeSubsystem;
-        // Use addRequirements() here to declare subsystem dependencies.
+        this.hopperSubsystem = hopperSubsystem;
-        addRequirements(subsystem);
     }
 
     // Called when the command is initially scheduled.
@@ -33,6 +33,9 @@ public class Reverse extends Command {
     // Called every time the scheduler runs while the command is scheduled.
     @Override
     public void execute() {
+        intakeSubsystem.reverseIntake();
+        hopperSubsystem.spindexerReverse();
+        hopperSubsystem.reverseKicker();
     }
 
     // Called once the command ends or is interrupted.
@@ -40,6 +43,7 @@ public class Reverse extends Command {
     public void end(boolean interrupted) {
         hopperSubsystem.stopKicker();
         hopperSubsystem.spindexerOff();
+        intakeSubsystem.stopIntake();
     }
 
     // Returns true when the command should end.

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

@@ -1,39 +1,35 @@
 // Copyright (c) FIRST and other WPILib contributors.
 // 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.subsystems;
 
 import org.fairportrobotics.frc.posty.TestableSubsystem;
+import static org.fairportrobotics.frc.posty.assertions.Assertions.assertThat;
 import org.fairportrobotics.frc.posty.test.PostTest;
 import org.littletonrobotics.junction.Logger;
 
 import com.ctre.phoenix6.StatusSignal;
-import com.ctre.phoenix6.configs.ClosedLoopGeneralConfigs;
 import com.ctre.phoenix6.configs.MotorOutputConfigs;
 import com.ctre.phoenix6.configs.Slot0Configs;
 import com.ctre.phoenix6.configs.TalonFXConfiguration;
 import com.ctre.phoenix6.controls.VelocityVoltage;
 import com.ctre.phoenix6.hardware.TalonFX;
 
-import edu.wpi.first.units.AngularVelocityUnit;
 import edu.wpi.first.units.Units;
 import edu.wpi.first.units.measure.AngularVelocity;
-import edu.wpi.first.wpilibj2.command.Command;
-
-import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
 import frc.robot.Constants;
 
 public class HopperSubsystem extends TestableSubsystem {
 
     private TalonFX spindexerMotor;
 
-
     private TalonFX kickerMotor;
     private VelocityVoltage kickerControl = new VelocityVoltage(0);
     private StatusSignal<AngularVelocity> kickerVelocity;
 
-  /** Creates a new HopperSubsystem. */
+    /**
+     * Creates a new HopperSubsystem.
+     */
     public HopperSubsystem() {
         spindexerMotor = new TalonFX(Constants.HopperConstants.SPINDEXER_MOTOR_ID);
         spindexerMotor.getConfigurator().apply(
@@ -74,11 +70,21 @@ public class HopperSubsystem extends TestableSubsystem {
         kickerMotor.set(-0.5);
     }
 
- public void stopKicker() {kickerMotor.stopMotor();}
+    public void stopKicker() {
+        kickerMotor.stopMotor();
+    }
 
-  public void spindexerOn() { spindexerMotor.set(0.75);}
+    public void spindexerOn() {
+        spindexerMotor.set(0.75);
+    }
 
-  public void spindexerOff() { spindexerMotor.stopMotor();}
+    public void spindexerReverse() {
+        spindexerMotor.set(-0.75);
+    }
+
+    public void spindexerOff() {
+        spindexerMotor.stopMotor();
+    }
 
     @Override
     public void periodic() {

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

@@ -1,32 +1,32 @@
 // Copyright (c) FIRST and other WPILib contributors.
 // 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.subsystems;
 
 import org.fairportrobotics.frc.posty.TestableSubsystem;
+import static org.fairportrobotics.frc.posty.assertions.Assertions.assertThat;
 import org.fairportrobotics.frc.posty.test.PostTest;
+import static org.fairportrobotics.frc.robolib.motors.Utils.SetMotorDirection;
 
 import com.ctre.phoenix.motorcontrol.LimitSwitchNormal;
 import com.ctre.phoenix.motorcontrol.LimitSwitchSource;
 import com.ctre.phoenix.motorcontrol.can.WPI_TalonSRX;
 import com.ctre.phoenix6.hardware.TalonFX;
-import frc.robot.Constants;
-
-import static org.fairportrobotics.frc.posty.assertions.Assertions.*;
-import static org.fairportrobotics.frc.robolib.motors.Utils.*;
 
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.Constants;
 
 public class IntakeSubsystem extends TestableSubsystem {
-  /** Creates a new ExampleSubsystem. */
+
+    /**
+     * Creates a new ExampleSubsystem.
+     */
 
     private TalonFX intakeMotor;
     private WPI_TalonSRX deployMotor;
 
-  public IntakeSubsystem() 
+    public IntakeSubsystem() {
-  {
         intakeMotor = new TalonFX(Constants.IntakeConstants.INTAKE_MOTOR_ID);
         SetMotorDirection(intakeMotor, Constants.IntakeConstants.INTAKE_MOTOR_DIRECTION);
 
@@ -36,13 +36,14 @@ public class IntakeSubsystem extends TestableSubsystem {
         deployMotor.setInverted(Constants.IntakeConstants.DEPLOY_MOTOR_INVERTED);
     }
 
-  public Command deploy()
+    public Command deploy() {
-  {
         return Commands.sequence(
                 Commands.deadline(
                         Commands.waitSeconds(2),
                         this.runOnce(
-          () -> {deployMotor.set(1);} // Bump this speed up if intake doesn't deploy
+                                () -> {
+                                    deployMotor.set(1);
+                                } // Bump this speed up if intake doesn't deploy
                         )
                 ),
                 this.runOnce(() -> deployMotor.stopMotor())
@@ -53,19 +54,30 @@ public class IntakeSubsystem extends TestableSubsystem {
         return this.runEnd(() -> deployMotor.set(-1), () -> deployMotor.stopMotor());
     }
 
-  public void setSpeed(double speed)
+    public void setSpeed(double speed) {
-  {
         intakeMotor.set(speed);
     }
 
-  public Command intake()
+    public Command stopIntake() {
-  {
+        return this.runOnce(() -> {
-    return this.runOnce(() -> {if(intakeMotor.get()==0){setSpeed(-.5);}else{intakeMotor.stopMotor();}});
+            intakeMotor.stopMotor();
+        });
     }
 
-  public Command reverseIntake()
+    public Command intake() {
-  {
+        return this.runOnce(() -> {
-    return this.runEnd(() -> setSpeed(.5), () -> { intakeMotor.stopMotor();});
+            if (intakeMotor.get() == 0) {
+                setSpeed(-.5);
+            } else {
+                intakeMotor.stopMotor();
+            }
+        });
+    }
+
+    public Command reverseIntake() {
+        return this.runEnd(() -> setSpeed(.5), () -> {
+            intakeMotor.stopMotor();
+        });
     }
 
     @PostTest(enabled = true)