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,94 +1,100 @@
 // 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. */
+    /**
-  public HopperSubsystem() {
+     * Creates a new HopperSubsystem.
-    spindexerMotor = new TalonFX(Constants.HopperConstants.SPINDEXER_MOTOR_ID);
+     */
-    spindexerMotor.getConfigurator().apply(
+    public HopperSubsystem() {
-        new TalonFXConfiguration()
+        spindexerMotor = new TalonFX(Constants.HopperConstants.SPINDEXER_MOTOR_ID);
-            .withMotorOutput(
+        spindexerMotor.getConfigurator().apply(
-                new MotorOutputConfigs()
+                new TalonFXConfiguration()
-                    .withInverted(Constants.HopperConstants.SPINDEXER_MOTOR_DIRECTION))
+                        .withMotorOutput(
-            .withSlot0(
+                                new MotorOutputConfigs()
-                new Slot0Configs()
+                                        .withInverted(Constants.HopperConstants.SPINDEXER_MOTOR_DIRECTION))
-                    .withKP(0)
+                        .withSlot0(
-                    .withKI(0)
+                                new Slot0Configs()
-                    .withKD(0)
+                                        .withKP(0)
-                    .withKS(0)));
+                                        .withKI(0)
+                                        .withKD(0)
+                                        .withKS(0)));
 
-    kickerMotor = new TalonFX(Constants.HopperConstants.KICKER_MOTOR_ID);
+        kickerMotor = new TalonFX(Constants.HopperConstants.KICKER_MOTOR_ID);
-    kickerMotor.getConfigurator().apply(
+        kickerMotor.getConfigurator().apply(
-        new TalonFXConfiguration()
+                new TalonFXConfiguration()
-            .withMotorOutput(
+                        .withMotorOutput(
-                new MotorOutputConfigs()
+                                new MotorOutputConfigs()
-                    .withInverted(Constants.HopperConstants.KICKER_MOTOR_DIRECTION)
+                                        .withInverted(Constants.HopperConstants.KICKER_MOTOR_DIRECTION)
-            )
+                        )
-            .withSlot0(new Slot0Configs()
+                        .withSlot0(new Slot0Configs()
-              .withKP(0)
+                                .withKP(0)
-              .withKI(0)
+                                .withKI(0)
-              .withKD(0)
+                                .withKD(0)
-              .withKS(0)
+                                .withKS(0)
-            ));
+                        ));
-    kickerVelocity = kickerMotor.getVelocity();
+        kickerVelocity = kickerMotor.getVelocity();
-  } 
+    }
 
-  public void feedKicker() {
+    public void feedKicker() {
-    //kickerMotor.setControl(kickerControl.withVelocity(1000));
+        //kickerMotor.setControl(kickerControl.withVelocity(1000));
-    kickerMotor.set(0.75);
+        kickerMotor.set(0.75);
-  }
+    }
 
-  public void reverseKicker() {
+    public void reverseKicker() {
-    //kickerMotor.setControl(kickerControl.withVelocity(-500));
+        //kickerMotor.setControl(kickerControl.withVelocity(-500));
-    kickerMotor.set(-0.5);
+        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);
+    }
 
-  @Override
+    public void spindexerOff() {
-  public void periodic() {
+        spindexerMotor.stopMotor();
-      Logger.recordOutput("HopperSubsystem-KickerSpeed(Rotations/sec)", kickerVelocity.refresh().getValue().in(Units.RotationsPerSecond));
+    }
-  }
 
-  @PostTest(enabled = true)
+    @Override
-  public void hopperSystem_CANDevicesConnected(){
+    public void periodic() {
-    assertThat(kickerMotor.isConnected()).as("Kicker motor not connected!").isTrue();
+        Logger.recordOutput("HopperSubsystem-KickerSpeed(Rotations/sec)", kickerVelocity.refresh().getValue().in(Units.RotationsPerSecond));
-    assertThat(spindexerMotor.isConnected()).as("Spindexer motor not connected!").isTrue();
+    }
-  }
+
+    @PostTest(enabled = true)
+    public void hopperSystem_CANDevicesConnected() {
+        assertThat(kickerMotor.isConnected()).as("Kicker motor not connected!").isTrue();
+        assertThat(spindexerMotor.isConnected()).as("Spindexer motor not connected!").isTrue();
+    }
 
 }

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

@@ -1,76 +1,88 @@
 // 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. */
 
-  private TalonFX intakeMotor;
+    /**
-  private WPI_TalonSRX deployMotor;
+     * Creates a new ExampleSubsystem.
+     */
 
-  public IntakeSubsystem() 
+    private TalonFX intakeMotor;
-  {
+    private WPI_TalonSRX deployMotor;
-    intakeMotor = new TalonFX(Constants.IntakeConstants.INTAKE_MOTOR_ID);
-    SetMotorDirection(intakeMotor, Constants.IntakeConstants.INTAKE_MOTOR_DIRECTION);
 
-    deployMotor = new WPI_TalonSRX(Constants.IntakeConstants.DEPLOY_MOTOR_ID);
+    public IntakeSubsystem() {
-    deployMotor.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
+        intakeMotor = new TalonFX(Constants.IntakeConstants.INTAKE_MOTOR_ID);
-    deployMotor.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
+        SetMotorDirection(intakeMotor, Constants.IntakeConstants.INTAKE_MOTOR_DIRECTION);
-    deployMotor.setInverted(Constants.IntakeConstants.DEPLOY_MOTOR_INVERTED);
-  }
 
-  public Command deploy()
+        deployMotor = new WPI_TalonSRX(Constants.IntakeConstants.DEPLOY_MOTOR_ID);
-  {
+        deployMotor.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
-    return Commands.sequence(
+        deployMotor.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen);
-      Commands.deadline(
+        deployMotor.setInverted(Constants.IntakeConstants.DEPLOY_MOTOR_INVERTED);
-        Commands.waitSeconds(2),
+    }
-        this.runOnce(
-          () -> {deployMotor.set(1);} // Bump this speed up if intake doesn't deploy
-        )
-      ),
-      this.runOnce(() -> deployMotor.stopMotor())
-    );
-  }
 
-  public Command resetDeploy(){
+    public Command deploy() {
-    return this.runEnd(() -> deployMotor.set(-1), () -> deployMotor.stopMotor());
+        return Commands.sequence(
-  }
+                Commands.deadline(
+                        Commands.waitSeconds(2),
+                        this.runOnce(
+                                () -> {
+                                    deployMotor.set(1);
+                                } // Bump this speed up if intake doesn't deploy
+                        )
+                ),
+                this.runOnce(() -> deployMotor.stopMotor())
+        );
+    }
 
-  public void setSpeed(double speed)
+    public Command resetDeploy() {
-  {
+        return this.runEnd(() -> deployMotor.set(-1), () -> deployMotor.stopMotor());
-    intakeMotor.set(speed);
+    }
-  }
 
-  public Command intake()
+    public void setSpeed(double speed) {
-  {
+        intakeMotor.set(speed);
-    return this.runOnce(() -> {if(intakeMotor.get()==0){setSpeed(-.5);}else{intakeMotor.stopMotor();}});
+    }
-  }
 
-  public Command reverseIntake()
+    public Command stopIntake() {
-  {
+        return this.runOnce(() -> {
-    return this.runEnd(() -> setSpeed(.5), () -> { intakeMotor.stopMotor();});
+            intakeMotor.stopMotor();
-  }
+        });
+    }
 
-  @PostTest(enabled = true)
+    public Command intake() {
-  public void IntakeSubsystem_CANDevicesConnected(){
+        return this.runOnce(() -> {
-    assertThat(intakeMotor.isConnected()).as("Intake motor not connected!").isTrue();
+            if (intakeMotor.get() == 0) {
-    assertThat(deployMotor.isAlive()).as("Deploy controller not connected!").isTrue();
+                setSpeed(-.5);
-  }
+            } else {
+                intakeMotor.stopMotor();
+            }
+        });
+    }
+
+    public Command reverseIntake() {
+        return this.runEnd(() -> setSpeed(.5), () -> {
+            intakeMotor.stopMotor();
+        });
+    }
+
+    @PostTest(enabled = true)
+    public void IntakeSubsystem_CANDevicesConnected() {
+        assertThat(intakeMotor.isConnected()).as("Intake motor not connected!").isTrue();
+        assertThat(deployMotor.isAlive()).as("Deploy controller not connected!").isTrue();
+    }
 }