diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 6144c5c..642c09e 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -32,6 +32,8 @@ import edu.wpi.first.wpilibj2.command.Commands; import edu.wpi.first.wpilibj2.command.FunctionalCommand; import edu.wpi.first.wpilibj2.command.InstantCommand; import edu.wpi.first.wpilibj2.command.PrintCommand; +import edu.wpi.first.wpilibj2.command.Subsystem; +import edu.wpi.first.wpilibj2.command.SubsystemBase; import edu.wpi.first.wpilibj2.command.button.CommandXboxController; import edu.wpi.first.wpilibj2.command.button.RobotModeTriggers; import edu.wpi.first.wpilibj2.command.button.Trigger; @@ -42,12 +44,14 @@ import frc.robot.constants.OIConstants; import frc.robot.constants.ShooterConstants; import frc.robot.constants.IntakePivotConstants.IntakePivotPosition; import frc.robot.constants.ShooterConstants.ShooterSpeeds; +import frc.robot.interfaces.IIndexer; import frc.robot.subsystems.Climber; import frc.robot.subsystems.Drivetrain; import frc.robot.subsystems.Hood; import frc.robot.subsystems.IntakePivot; import frc.robot.subsystems.IntakeRoller; import frc.robot.subsystems.PhotonVision; +import frc.robot.subsystems.RollerIndexer; import frc.robot.subsystems.Shooter; import frc.robot.subsystems.Spindexer; import frc.robot.utilities.Elastic; @@ -60,7 +64,7 @@ public class RobotContainer { private Shooter shooter; private IntakePivot intakePivot; private IntakeRoller intakeRoller; - private Spindexer spindexer; + private IIndexer indexer; //private Climber climber; private CommandXboxController driver; @@ -79,7 +83,7 @@ public class RobotContainer { shooter = new Shooter(); intakePivot = new IntakePivot(); intakeRoller = new IntakeRoller(); - spindexer = new Spindexer(); + indexer = new RollerIndexer(); //climber = new Climber(); configureNamedCommands(); @@ -184,7 +188,7 @@ public class RobotContainer { intakePivot.setDefaultCommand(intakePivot.stop()); intakeRoller.setDefaultCommand(intakeRoller.stop()); hood.setDefaultCommand(hood.stop()); - spindexer.setDefaultCommand(spindexer.stop()); + ((SubsystemBase)indexer).setDefaultCommand(indexer.stop()); //climber.setDefaultCommand(climber.stop()); // While holding POV up of the driver controller, the climber @@ -198,23 +202,23 @@ public class RobotContainer { //})); // While holding the right bumper of the driver controller, the intake rollers - // and the spindexer and feeder should move such that all motors are moving in such a way - // that it would draw balls from the floor, through the spindexer, and into the + // and the indexer and feeder should move such that all motors are moving in such a way + // that it would draw balls from the floor, through the indexer, and into the // feeder. // DO NOT INVERT MOTION WITH UNARY MINUS (-). Every motor can be inverted from the // constants file for the subsystem having the problem driver.rightBumper().whileTrue( - spindexer.spinToShooter() + indexer.spinToShooter() ); // While holding the left bumper of the driver controller, the intake rollers - // and the spindexer and feeder should move such that all motors are moving in such a way + // and the indexer and feeder should move such that all motors are moving in such a way // that it would try to eject balls through the intake. // DO NOT INVERT MOTION WITH UNARY MINUS (-). Every motor can be inverted from the // constants file for the subsystem having the problem driver.leftBumper().whileTrue( intakeRoller.runIn() - //intakeRoller.runOut().alongWith(spindexer.spinToIntake()) + //intakeRoller.runOut().alongWith(indexer.spinToIntake()) ); // While holding D-Pad up on the secondary controller, the shooter should spin @@ -281,7 +285,7 @@ public class RobotContainer { ); shooter.setDefaultCommand(shooter.stop()); intakeRoller.setDefaultCommand(intakeRoller.stop()); - spindexer.setDefaultCommand(spindexer.stop()); + ((SubsystemBase)indexer).setDefaultCommand(indexer.stop()); intakePivot.setDefaultCommand(intakePivot.manualSpeed(() -> secondary.getLeftY())); @@ -303,7 +307,7 @@ public class RobotContainer { driver.rightBumper().whileTrue(intakeRoller.runIn()); driver.rightTrigger().whileTrue( - spindexer.spinToShooter(shooter::getAverageActualSpeeds, 2000).alongWith( + indexer.spinToShooter(shooter::getAverageActualSpeeds, 2000).alongWith( intakeRoller.runIn()/* , intakePivot.jimmy(.5)*/ ) @@ -318,7 +322,7 @@ public class RobotContainer { intakePivot.manualSpeed(() -> 0.75).withTimeout(0.5) ); - driver.b().whileTrue(spindexer.spinToIntake()); + driver.b().whileTrue(indexer.spinToIntake()); /* driver.b().whileTrue( drivetrain.lockToYaw( () -> { @@ -412,10 +416,10 @@ public class RobotContainer { .withTimeout(2)); NamedCommands.registerCommand("shoot close", - spindexer.spinToShooter() + indexer.spinToShooter() .alongWith(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed)) .alongWith(hood.trackToAngle(() -> Units.degreesToRadians(10))) - .withTimeout(3).andThen(spindexer.instantaneousStop())); + .withTimeout(3).andThen(indexer.instantaneousStop())); // NamedCommands.registerCommand("Intake Start", intakeRoller.runIn()); @@ -427,7 +431,7 @@ public class RobotContainer { .onTrue( shooter.maintainSpeed(ShooterSpeeds.kHubSpeed)); - NamedCommands.registerCommand("stop spindexer", spindexer.instantaneousStop()); + NamedCommands.registerCommand("stop spindexer", indexer.instantaneousStop()); NamedCommands.registerCommand("jimmy", intakePivot.jimmy(0.2) @@ -436,11 +440,11 @@ public class RobotContainer { NamedCommands.registerCommand("shoot N jimmy", Commands.parallel( intakePivot.jimmy(0.5), - spindexer.spinToShooter() + indexer.spinToShooter() .alongWith(shooter.maintainSpeed(ShooterSpeeds.kHubSpeed), hood.trackToAngle(() -> Units.degreesToRadians(10))) - ).withTimeout(3).andThen(spindexer.instantaneousStop())); + ).withTimeout(3).andThen(indexer.instantaneousStop())); NamedCommands.registerCommand("aim", hood.trackToAnglePoseBased(drivetrain, shooter) @@ -454,7 +458,7 @@ public class RobotContainer { hood.trackToAnglePoseBased(drivetrain, shooter) .alongWith( shooter.maintainSpeed(ShooterSpeeds.kHubSpeed), - spindexer.spinToShooter(), + indexer.spinToShooter(), intakePivot.jimmy(0.5), drivetrain.lockRotationToHub(() -> 0.0, () -> 0.0, false))); } diff --git a/src/main/java/frc/robot/constants/RollerIndexerConstants.java b/src/main/java/frc/robot/constants/RollerIndexerConstants.java new file mode 100644 index 0000000..e39ba8c --- /dev/null +++ b/src/main/java/frc/robot/constants/RollerIndexerConstants.java @@ -0,0 +1,28 @@ +package frc.robot.constants; + +import com.revrobotics.spark.config.SparkMaxConfig; +import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +public class RollerIndexerConstants { + public static final int kMotor1CANID = -1; + public static final int kMotor2CANID = 4; + + public static final int kCurrentLimit = 30; + + public static final double kMotorSpeed = 1; + + public static final boolean kMotorInverted = false; + + public static final IdleMode kIdleMode = IdleMode.kBrake; + + // YOU SHOULDN'T NEED TO CHANGE ANYTHING BELOW THIS LINE UNLESS YOU'RE ADDING A CONFIGURATION ITEM + + public static final SparkMaxConfig kMotorConfig = new SparkMaxConfig(); + + static { + kMotorConfig + .inverted(kMotorInverted) + .smartCurrentLimit(kCurrentLimit) + .idleMode(kIdleMode); + } +} diff --git a/src/main/java/frc/robot/interfaces/IIndexer.java b/src/main/java/frc/robot/interfaces/IIndexer.java new file mode 100644 index 0000000..2c56c80 --- /dev/null +++ b/src/main/java/frc/robot/interfaces/IIndexer.java @@ -0,0 +1,13 @@ +package frc.robot.interfaces; + +import java.util.function.DoubleSupplier; + +import edu.wpi.first.wpilibj2.command.Command; + +public interface IIndexer { + public Command spinToShooter(); + public Command spinToShooter(DoubleSupplier shooterSpeedRPM, double cutOffRPM); + public Command spinToIntake(); + public Command stop(); + public Command instantaneousStop(); +} diff --git a/src/main/java/frc/robot/subsystems/RollerIndexer.java b/src/main/java/frc/robot/subsystems/RollerIndexer.java new file mode 100644 index 0000000..ab69468 --- /dev/null +++ b/src/main/java/frc/robot/subsystems/RollerIndexer.java @@ -0,0 +1,79 @@ +package frc.robot.subsystems; + +import java.util.function.DoubleSupplier; + +import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.SparkMax; +import com.revrobotics.spark.SparkLowLevel.MotorType; + +import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.SubsystemBase; +import frc.robot.constants.RollerIndexerConstants; +import frc.robot.constants.SpindexerConstants; +import frc.robot.interfaces.IIndexer; + +public class RollerIndexer extends SubsystemBase implements IIndexer { + private SparkMax motor1; + private SparkMax motor2; + + public RollerIndexer() { + motor1 = new SparkMax(RollerIndexerConstants.kMotor1CANID, MotorType.kBrushless); + motor2 = new SparkMax(RollerIndexerConstants.kMotor2CANID, MotorType.kBrushless); + + motor1.configure( + SpindexerConstants.kFeederConfig, + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters + ); + + motor2.configure( + SpindexerConstants.kFeederConfig, + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters + ); + } + + public Command spinToShooter() { + return run(() -> { + motor1.set(RollerIndexerConstants.kMotorSpeed); + motor2.set(-RollerIndexerConstants.kMotorSpeed); + }); + } + + public Command spinToShooter(DoubleSupplier shooterSpeedRPM, double cutOffRPM) { + return run(() -> { + if(shooterSpeedRPM.getAsDouble() < cutOffRPM) { + motor1.set(0); + motor2.set(0); + } else { + motor1.set(RollerIndexerConstants.kMotorSpeed); + motor2.set(-RollerIndexerConstants.kMotorSpeed); + } + }); + } + + public Command spinToIntake() { + return run(() -> { + motor1.set(-RollerIndexerConstants.kMotorSpeed); + motor2.set(RollerIndexerConstants.kMotorSpeed); + }); + } + + public Command stop() { + return run(() -> { + motor1.set(0); + motor2.set(0); + }); + } + + public Command instantaneousStop() { + return runOnce(() -> { + motor1.set(0); + motor2.set(0); + }); + } + +} diff --git a/src/main/java/frc/robot/subsystems/Spindexer.java b/src/main/java/frc/robot/subsystems/Spindexer.java index 0479424..eacaf65 100644 --- a/src/main/java/frc/robot/subsystems/Spindexer.java +++ b/src/main/java/frc/robot/subsystems/Spindexer.java @@ -12,8 +12,9 @@ import com.revrobotics.spark.SparkLowLevel.MotorType; import edu.wpi.first.wpilibj2.command.Command; import edu.wpi.first.wpilibj2.command.SubsystemBase; import frc.robot.constants.SpindexerConstants; +import frc.robot.interfaces.IIndexer; -public class Spindexer extends SubsystemBase { +public class Spindexer extends SubsystemBase implements IIndexer { private TalonFX spindexerMotor; private SparkMax feederMotor;