diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index cd61809..517869e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/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()); } diff --git a/src/main/java/frc/robot/commands/Reverse.java b/src/main/java/frc/robot/commands/Reverse.java index cb3bb66..d2a7425 100644 --- a/src/main/java/frc/robot/commands/Reverse.java +++ b/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() { - m_subsystem = subsystem; - // Use addRequirements() here to declare subsystem dependencies. - addRequirements(subsystem); + public Reverse(IntakeSubsystem intakeSubsystem, HopperSubsystem hopperSubsystem) { + this.intakeSubsystem = intakeSubsystem; + this.hopperSubsystem = hopperSubsystem; } // 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. diff --git a/src/main/java/frc/robot/subsystems/HopperSubsystem.java b/src/main/java/frc/robot/subsystems/HopperSubsystem.java index 8097181..0bfdfb2 100644 --- a/src/main/java/frc/robot/subsystems/HopperSubsystem.java +++ b/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 kickerVelocity; - /** Creates a new HopperSubsystem. */ - public HopperSubsystem() { - spindexerMotor = new TalonFX(Constants.HopperConstants.SPINDEXER_MOTOR_ID); - spindexerMotor.getConfigurator().apply( - new TalonFXConfiguration() - .withMotorOutput( - new MotorOutputConfigs() - .withInverted(Constants.HopperConstants.SPINDEXER_MOTOR_DIRECTION)) - .withSlot0( - new Slot0Configs() - .withKP(0) - .withKI(0) - .withKD(0) - .withKS(0))); + /** + * Creates a new HopperSubsystem. + */ + public HopperSubsystem() { + spindexerMotor = new TalonFX(Constants.HopperConstants.SPINDEXER_MOTOR_ID); + spindexerMotor.getConfigurator().apply( + new TalonFXConfiguration() + .withMotorOutput( + new MotorOutputConfigs() + .withInverted(Constants.HopperConstants.SPINDEXER_MOTOR_DIRECTION)) + .withSlot0( + new Slot0Configs() + .withKP(0) + .withKI(0) + .withKD(0) + .withKS(0))); - kickerMotor = new TalonFX(Constants.HopperConstants.KICKER_MOTOR_ID); - kickerMotor.getConfigurator().apply( - new TalonFXConfiguration() - .withMotorOutput( - new MotorOutputConfigs() - .withInverted(Constants.HopperConstants.KICKER_MOTOR_DIRECTION) - ) - .withSlot0(new Slot0Configs() - .withKP(0) - .withKI(0) - .withKD(0) - .withKS(0) - )); - kickerVelocity = kickerMotor.getVelocity(); - } + kickerMotor = new TalonFX(Constants.HopperConstants.KICKER_MOTOR_ID); + kickerMotor.getConfigurator().apply( + new TalonFXConfiguration() + .withMotorOutput( + new MotorOutputConfigs() + .withInverted(Constants.HopperConstants.KICKER_MOTOR_DIRECTION) + ) + .withSlot0(new Slot0Configs() + .withKP(0) + .withKI(0) + .withKD(0) + .withKS(0) + )); + kickerVelocity = kickerMotor.getVelocity(); + } - public void feedKicker() { - //kickerMotor.setControl(kickerControl.withVelocity(1000)); - kickerMotor.set(0.75); - } + public void feedKicker() { + //kickerMotor.setControl(kickerControl.withVelocity(1000)); + kickerMotor.set(0.75); + } - public void reverseKicker() { - //kickerMotor.setControl(kickerControl.withVelocity(-500)); - kickerMotor.set(-0.5); - } + public void reverseKicker() { + //kickerMotor.setControl(kickerControl.withVelocity(-500)); + 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 periodic() { - Logger.recordOutput("HopperSubsystem-KickerSpeed(Rotations/sec)", kickerVelocity.refresh().getValue().in(Units.RotationsPerSecond)); - } + public void spindexerOff() { + spindexerMotor.stopMotor(); + } - @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(); - } + @Override + public void periodic() { + Logger.recordOutput("HopperSubsystem-KickerSpeed(Rotations/sec)", kickerVelocity.refresh().getValue().in(Units.RotationsPerSecond)); + } -} \ No newline at end of file + @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(); + } + +} diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index 7d0e930..9f7bb3c 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/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() - { - intakeMotor = new TalonFX(Constants.IntakeConstants.INTAKE_MOTOR_ID); - SetMotorDirection(intakeMotor, Constants.IntakeConstants.INTAKE_MOTOR_DIRECTION); + private TalonFX intakeMotor; + private WPI_TalonSRX deployMotor; - deployMotor = new WPI_TalonSRX(Constants.IntakeConstants.DEPLOY_MOTOR_ID); - deployMotor.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); - deployMotor.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); - deployMotor.setInverted(Constants.IntakeConstants.DEPLOY_MOTOR_INVERTED); - } + public IntakeSubsystem() { + intakeMotor = new TalonFX(Constants.IntakeConstants.INTAKE_MOTOR_ID); + SetMotorDirection(intakeMotor, Constants.IntakeConstants.INTAKE_MOTOR_DIRECTION); - 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 - ) - ), - this.runOnce(() -> deployMotor.stopMotor()) - ); - } + deployMotor = new WPI_TalonSRX(Constants.IntakeConstants.DEPLOY_MOTOR_ID); + deployMotor.configForwardLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); + deployMotor.configReverseLimitSwitchSource(LimitSwitchSource.FeedbackConnector, LimitSwitchNormal.NormallyOpen); + deployMotor.setInverted(Constants.IntakeConstants.DEPLOY_MOTOR_INVERTED); + } - public Command resetDeploy(){ - return this.runEnd(() -> deployMotor.set(-1), () -> deployMotor.stopMotor()); - } + 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 + ) + ), + this.runOnce(() -> deployMotor.stopMotor()) + ); + } - public void setSpeed(double speed) - { - intakeMotor.set(speed); - } + public Command resetDeploy() { + return this.runEnd(() -> deployMotor.set(-1), () -> deployMotor.stopMotor()); + } - public Command intake() - { - return this.runOnce(() -> {if(intakeMotor.get()==0){setSpeed(-.5);}else{intakeMotor.stopMotor();}}); - } + public void setSpeed(double speed) { + intakeMotor.set(speed); + } - public Command reverseIntake() - { - return this.runEnd(() -> setSpeed(.5), () -> { intakeMotor.stopMotor();}); - } + public Command stopIntake() { + return this.runOnce(() -> { + 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(); - } + public Command intake() { + return this.runOnce(() -> { + if (intakeMotor.get() == 0) { + 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(); + } }