diff --git a/src/main/java/first/robot/constants/RollerIndexerConstants.java b/src/main/java/first/robot/constants/RollerIndexerConstants.java index 23da80c..22167f9 100644 --- a/src/main/java/first/robot/constants/RollerIndexerConstants.java +++ b/src/main/java/first/robot/constants/RollerIndexerConstants.java @@ -1,9 +1,14 @@ package first.robot.constants; import com.revrobotics.spark.config.SparkBaseConfig.IdleMode; + +import org.wpilib.hardware.bus.CANPort; + import com.revrobotics.spark.config.SparkMaxConfig; public class RollerIndexerConstants { + public static final CANPort kCANPort = CANPort.CAN_S0; + public static final int kMotor1CANID = 50; public static final int kMotor2CANID = 4; diff --git a/src/main/java/first/robot/constants/ShooterConstants.java b/src/main/java/first/robot/constants/ShooterConstants.java index a8e66c2..02a5680 100644 --- a/src/main/java/first/robot/constants/ShooterConstants.java +++ b/src/main/java/first/robot/constants/ShooterConstants.java @@ -1,5 +1,6 @@ package first.robot.constants; +import org.wpilib.hardware.bus.CANPort; import org.wpilib.math.util.Units; import com.revrobotics.spark.ClosedLoopSlot; @@ -35,6 +36,8 @@ public class ShooterConstants { public static final double kWheelDiameter = Units.inchesToMeters(4); // TODO Real values + public static final CANPort kCANPort = CANPort.CAN_S0; + public static final int kLeftShooterMotorCANID = 2; public static final int kRightShooterMotorCANID = 5; diff --git a/src/main/java/first/robot/constants/SpindexerConstants.java b/src/main/java/first/robot/constants/SpindexerConstants.java index d8384de..5a94e6e 100644 --- a/src/main/java/first/robot/constants/SpindexerConstants.java +++ b/src/main/java/first/robot/constants/SpindexerConstants.java @@ -1,5 +1,7 @@ package first.robot.constants; +import org.wpilib.hardware.bus.CANPort; + import com.ctre.phoenix6.configs.CurrentLimitsConfigs; import com.ctre.phoenix6.configs.MotorOutputConfigs; import com.ctre.phoenix6.signals.InvertedValue; @@ -9,6 +11,8 @@ import com.revrobotics.spark.config.SparkMaxConfig; public class SpindexerConstants { // TODO Real values + public static final CANPort kCANPort = CANPort.CAN_S0; + public static final int kSpindexerMotorCANID = 0; public static final int kFeederMotorCANID = 4; diff --git a/src/main/java/first/robot/subsystems/Drivetrain.java b/src/main/java/first/robot/subsystems/Drivetrain.java index 4b75947..6f427b9 100644 --- a/src/main/java/first/robot/subsystems/Drivetrain.java +++ b/src/main/java/first/robot/subsystems/Drivetrain.java @@ -10,10 +10,9 @@ import org.wpilib.command2.Command; import org.wpilib.command2.PrintCommand; import org.wpilib.command2.SubsystemBase; import org.wpilib.driverstation.Alliance; -import org.wpilib.driverstation.DriverStation; import org.wpilib.driverstation.MatchState; +import org.wpilib.epilogue.Logged; import org.wpilib.hardware.imu.OnboardIMU; -import org.wpilib.hardware.imu.OnboardIMU.MountOrientation; import org.wpilib.math.controller.PIDController; import org.wpilib.math.estimator.SwerveDrivePoseEstimator; import org.wpilib.math.geometry.Pose2d; @@ -36,6 +35,7 @@ import first.robot.utilities.SwerveModule; import first.robot.utilities.Utilities; import first.robot.utilities.VisualPose; +@Logged public class Drivetrain extends SubsystemBase { private SwerveModule frontLeft; private SwerveModule frontRight; @@ -146,11 +146,6 @@ public class Drivetrain extends SubsystemBase { frontRight.periodic(); rearLeft.periodic(); rearRight.periodic(); - - Logger.recordOutput("Drivetrain/Pose", getPose()); - Logger.recordOutput("Drivetrain/Gyro Angle", getGyroValue()); - Logger.recordOutput("Drivetrain/Heading", getHeadingDegrees()); - Logger.recordOutput("Drivetrain/Velocity", getCurrentChassisSpeeds()); } /** @@ -268,16 +263,11 @@ public class Drivetrain extends SubsystemBase { targetRotation = targetRotation.rotateBy(Rotation2d.k180deg); } - Logger.recordOutput("/HubAutoAlign/CurrentHeader", getHeading().getRadians()); - Logger.recordOutput("/HubAutoAlign/Setpoint", targetRotation.getRadians()); - double outputPower = -yawRotationController.calculate( getHeading().getRadians(), targetRotation.getRadians() ); - Logger.recordOutput("/HubAutoAlign/OutputPower", outputPower); - return outputPower; }, () -> true diff --git a/src/main/java/first/robot/subsystems/Hood.java b/src/main/java/first/robot/subsystems/Hood.java index dd61f77..1932c96 100644 --- a/src/main/java/first/robot/subsystems/Hood.java +++ b/src/main/java/first/robot/subsystems/Hood.java @@ -5,9 +5,9 @@ import java.util.function.DoubleSupplier; import org.wpilib.command2.Command; import org.wpilib.command2.SubsystemBase; +import org.wpilib.epilogue.Logged; import org.wpilib.math.geometry.Pose2d; import org.wpilib.math.interpolation.InterpolatingDoubleTreeMap; -import org.wpilib.math.util.MathUtil; import com.revrobotics.PersistMode; import com.revrobotics.RelativeEncoder; @@ -21,6 +21,7 @@ import first.robot.constants.HoodConstants; import first.robot.constants.ShooterConstants.ShooterSpeeds; import first.robot.utilities.Utilities; +@Logged public class Hood extends SubsystemBase { private SparkMax motor; @@ -28,12 +29,7 @@ public class Hood extends SubsystemBase { private SparkClosedLoopController controller; - //private Trigger resetTrigger; - //private Trigger timerTrigger; - - //private Timer resetTimer; - - private double currentTargetDegrees; + private double currentTargetRadians; public Hood() { motor = new SparkMax( @@ -53,47 +49,35 @@ public class Hood extends SubsystemBase { controller = motor.getClosedLoopController(); - /*resetTimer = new Timer(); - resetTimer.reset(); - - resetTrigger = new Trigger(() -> (motor.getOutputCurrent() > HoodConstants.kAmpsToTriggerPositionReset)); - resetTrigger.onTrue(new InstantCommand(resetTimer::start)); - resetTrigger.onFalse(new InstantCommand(() -> { - resetTimer.stop(); - resetTimer.reset(); - })); - - timerTrigger = new Trigger(() -> resetTimer.hasElapsed(HoodConstants.kTimeAboveThresholdToReset)); - timerTrigger.onTrue(new InstantCommand(() -> { - encoder.setPosition(0); - resetTimer.reset(); - }));*/ - - currentTargetDegrees = HoodConstants.kStartupAngle; + currentTargetRadians = HoodConstants.kStartupAngle; } - @Override - public void periodic() { - SmartDashboard.putNumber( - "HoodTargetDegrees", - Math.toDegrees(currentTargetDegrees) - ); + public double getHoodTargetDegrees() { + return Math.toDegrees(currentTargetRadians); + } - SmartDashboard.putNumber( - "HoodCurrentAngle", - Math.toDegrees(encoder.getPosition()) - ); + public double getHoodTargetRadians() { + return currentTargetRadians; + } - SmartDashboard.putBoolean( - "HoodAtSetpoint", - controller.isAtSetpoint() - ); - - Logger.recordOutput("Hood/OutputCurrent", motor.getOutputCurrent()); - Logger.recordOutput("Hood/CurrentTarget", Math.toDegrees(currentTargetDegrees)); - Logger.recordOutput("Hood/CurrentAngle", Math.toDegrees(encoder.getPosition())); - Logger.recordOutput("Hood/AtSetpoint", controller.isAtSetpoint()); - Logger.recordOutput("Hood/VoltageOut", motor.getAppliedOutput()*motor.getBusVoltage()); + public double getCurrentHoodAngleDegrees() { + return Math.toDegrees(encoder.getPosition().get()); + } + + public double getCurrentHoodAngleRadians() { + return encoder.getPosition().get(); + } + + public double getOutputVoltage() { + return motor.getAppliedOutput().get() * motor.getBusVoltage().get(); + } + + public double getOutputCurrent() { + return motor.getOutputCurrent().get(); + } + + public boolean isHoodAtSetpoint() { + return controller.isAtSetpoint().get(); } public Command trackToAnglePoseBased(Drivetrain drivetrain, Shooter shooter) { @@ -104,15 +88,13 @@ public class Hood extends SubsystemBase { double distance = drivetrainPose.getTranslation() .getDistance(hubPose.getTranslation()); - Logger.recordOutput("Hood/DistanceToHub", distance); - Optional currentSpeeds = shooter.getTargetSpeeds(); if(currentSpeeds.isPresent()) { InterpolatingDoubleTreeMap map = HoodConstants.kHoodInterpolators.get(currentSpeeds.get()); if(map != null) { - return MathUtil.clamp(map.get(distance), 0, 40); + return Math.clamp(map.get(distance), 0, 40); } else { return 0; } @@ -122,51 +104,16 @@ public class Hood extends SubsystemBase { }); } - public Command trackToAngle(DoubleSupplier degreeAngleSupplier) { + public Command trackToAngle(DoubleSupplier radianAngleSupplier) { return run(() -> { - currentTargetDegrees = degreeAngleSupplier.getAsDouble(); + currentTargetRadians = radianAngleSupplier.getAsDouble(); - controller.setSetpoint(currentTargetDegrees, ControlType.kPosition); + controller.setSetpoint(currentTargetRadians, ControlType.kPosition); }); } - /** - * An automated form of resetting the hood position sensing. - * - * Run down at the full manual speed (note that this is affected by the - * kMaxManualSpeedMultiplier constant) until the timer trigger becomes true - * (i.e. the output current has been above the threshold (kAmpsToTriggerPositionReset) - * for reset for the amount of specified by kTimeAboveThresholdToReset). Once - * that returns true, the motor is stopped until the timer trigger switches to false - * (i.e. it has reset the position automatically, because that's how it's configured, - * and resets the timer to 0, which makes the timer trigger false) - * - * @return A complete Command structure that performs the specified action - */ - /*public Command automatedRezero() { - return manualSpeed(() -> -1) - .until(timerTrigger) - .andThen( - stop().until(timerTrigger.negate()) - ); - } - - /** - * An alternate form of {@link #automatedRezero()} that doesn't rely on the triggers - * to reset the hood position to zero. Note that this method doesn't have any time limiting - * factor to it, as soon as the current goes above the threshold specified by - * kAmpsToTriggerPositionReset the encoder position will be set to zero - * - * @return A complete Command structure that performs the specified action - */ - /*public Command automatedRezeroNoTimer() { - return manualSpeed(() -> -1) - .until(() -> motor.getOutputCurrent() >= HoodConstants.kAmpsToTriggerPositionReset) - .andThen(new InstantCommand(() -> encoder.setPosition(0))); - }*/ - public Command manualSpeed(DoubleSupplier speed) { - currentTargetDegrees = 0; + currentTargetRadians = 0; return run(() -> { motor.setThrottle(speed.getAsDouble() * HoodConstants.kMaxManualSpeedMultiplier); @@ -176,8 +123,4 @@ public class Hood extends SubsystemBase { public Command stop() { return manualSpeed(() -> 0); } - - public double getTargetDegrees() { - return currentTargetDegrees; - } } diff --git a/src/main/java/first/robot/subsystems/IntakePivot.java b/src/main/java/first/robot/subsystems/IntakePivot.java index e8699b8..daa2040 100644 --- a/src/main/java/first/robot/subsystems/IntakePivot.java +++ b/src/main/java/first/robot/subsystems/IntakePivot.java @@ -6,6 +6,7 @@ import java.util.function.DoubleSupplier; import org.wpilib.command2.Command; import org.wpilib.command2.Commands; import org.wpilib.command2.SubsystemBase; +import org.wpilib.epilogue.Logged; import com.revrobotics.PersistMode; import com.revrobotics.RelativeEncoder; @@ -18,6 +19,7 @@ import com.revrobotics.spark.SparkMax; import first.robot.constants.IntakePivotConstants; import first.robot.constants.IntakePivotConstants.IntakePivotPosition; +@Logged public class IntakePivot extends SubsystemBase { private SparkMax leftMotor; private SparkMax rightMotor; @@ -58,13 +60,16 @@ public class IntakePivot extends SubsystemBase { encoder.setPosition(IntakePivotConstants.IntakePivotPosition.kUp.getPositionRadians()); } - @Override - public void periodic() { - Logger.recordOutput( - "IntakePivot/TargetPosition", - currentTargetPosition == null ? -1 : currentTargetPosition.getPositionRadians()); - Logger.recordOutput("IntakePivot/CurrentPosition", encoder.getPosition()); - Logger.recordOutput("IntakePivot/AtSetpoint", controller.isAtSetpoint()); + public double getTargetPosition() { + return currentTargetPosition == null ? -1 : currentTargetPosition.getPositionRadians(); + } + + public double getCurrentPosition() { + return encoder.getPosition().get(); + } + + public boolean isAtSetpoint() { + return controller.isAtSetpoint().get(); } public Command maintainPosition(IntakePivotPosition position) { diff --git a/src/main/java/first/robot/subsystems/IntakeRoller.java b/src/main/java/first/robot/subsystems/IntakeRoller.java index c46f24c..defa354 100644 --- a/src/main/java/first/robot/subsystems/IntakeRoller.java +++ b/src/main/java/first/robot/subsystems/IntakeRoller.java @@ -2,6 +2,7 @@ package first.robot.subsystems; import org.wpilib.command2.Command; import org.wpilib.command2.SubsystemBase; +import org.wpilib.epilogue.Logged; import com.revrobotics.PersistMode; import com.revrobotics.ResetMode; @@ -10,6 +11,7 @@ import com.revrobotics.spark.SparkMax; import first.robot.constants.IntakeRollerConstants; +@Logged public class IntakeRoller extends SubsystemBase { private SparkMax leftMotor; private SparkMax rightMotor; diff --git a/src/main/java/first/robot/subsystems/RollerIndexer.java b/src/main/java/first/robot/subsystems/RollerIndexer.java new file mode 100644 index 0000000..ef4aa0a --- /dev/null +++ b/src/main/java/first/robot/subsystems/RollerIndexer.java @@ -0,0 +1,88 @@ +package first.robot.subsystems; + +import java.util.function.DoubleSupplier; + +import org.wpilib.command2.Command; +import org.wpilib.command2.SubsystemBase; +import org.wpilib.epilogue.Logged; + +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; + +import first.robot.constants.RollerIndexerConstants; +import first.robot.interfaces.IIndexer; + +@Logged +public class RollerIndexer extends SubsystemBase implements IIndexer { + private SparkMax motor1; + private SparkMax motor2; + + public RollerIndexer() { + motor1 = new SparkMax( + RollerIndexerConstants.kCANPort, + RollerIndexerConstants.kMotor1CANID, + MotorType.kBrushless + ); + + motor2 = new SparkMax( + RollerIndexerConstants.kCANPort, + RollerIndexerConstants.kMotor2CANID, + MotorType.kBrushless + ); + + motor1.configure( + RollerIndexerConstants.kMotorConfig, + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters + ); + + motor2.configure( + RollerIndexerConstants.kMotorConfig, + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters + ); + } + + public Command spinToShooter() { + return run(() -> { + motor1.setThrottle(RollerIndexerConstants.kMotorSpeed); + motor2.setThrottle(-RollerIndexerConstants.kMotorSpeed); + }); + } + + public Command spinToShooter(DoubleSupplier shooterSpeedRPM, double cutOffRPM) { + return run(() -> { + if(shooterSpeedRPM.getAsDouble() < cutOffRPM) { + motor1.setThrottle(0); + motor2.setThrottle(0); + } else { + motor1.setThrottle(RollerIndexerConstants.kMotorSpeed); + motor2.setThrottle(-RollerIndexerConstants.kMotorSpeed); + } + }); + } + + public Command spinToIntake() { + return run(() -> { + motor1.setThrottle(-RollerIndexerConstants.kMotorSpeed); + motor2.setThrottle(RollerIndexerConstants.kMotorSpeed); + }); + } + + public Command stop() { + return run(() -> { + motor1.setThrottle(0); + motor2.setThrottle(0); + }); + } + + public Command instantaneousStop() { + return runOnce(() -> { + motor1.setThrottle(0); + motor2.setThrottle(0); + }); + } + +} diff --git a/src/main/java/first/robot/subsystems/Shooter.java b/src/main/java/first/robot/subsystems/Shooter.java new file mode 100644 index 0000000..d356723 --- /dev/null +++ b/src/main/java/first/robot/subsystems/Shooter.java @@ -0,0 +1,147 @@ +package first.robot.subsystems; + +import java.util.Optional; +import java.util.function.DoubleSupplier; + +import org.wpilib.command2.Command; +import org.wpilib.command2.SubsystemBase; +import org.wpilib.math.controller.SimpleMotorFeedforward; + +import com.revrobotics.AbsoluteEncoder; +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.ClosedLoopSlot; +import com.revrobotics.spark.SparkClosedLoopController; +import com.revrobotics.spark.SparkLowLevel.ControlType; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; + +import first.robot.constants.ShooterConstants; +import first.robot.constants.ShooterConstants.ShooterSpeeds; + +public class Shooter extends SubsystemBase { + private SparkMax leftMotor; + private SparkMax rightMotor; + + private AbsoluteEncoder leftEncoder; + private AbsoluteEncoder rightEncoder; + + private SparkClosedLoopController leftClosedLoopController; + private SparkClosedLoopController rightClosedLoopController; + + private ShooterSpeeds targetSpeeds; + private SimpleMotorFeedforward shooterFFLeft; + private SimpleMotorFeedforward shooterFFRight; + + public Shooter() { + leftMotor = new SparkMax( + ShooterConstants.kCANPort, + ShooterConstants.kLeftShooterMotorCANID, + MotorType.kBrushless + ); + + rightMotor = new SparkMax( + ShooterConstants.kCANPort, + ShooterConstants.kRightShooterMotorCANID, + MotorType.kBrushless + ); + + leftMotor.configure( + ShooterConstants.kLeftMotorConfig, + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters + ); + + rightMotor.configure( + ShooterConstants.kRightMotorConfig, + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters + ); + + leftEncoder = leftMotor.getAbsoluteEncoder(); + rightEncoder = rightMotor.getAbsoluteEncoder(); + + leftClosedLoopController = leftMotor.getClosedLoopController(); + rightClosedLoopController = rightMotor.getClosedLoopController(); + + // TODO Set this to the initial startup speed + targetSpeeds = null; + + shooterFFLeft = new SimpleMotorFeedforward(ShooterConstants.kLeftS, ShooterConstants.kLeftV, ShooterConstants.kLeftA); + shooterFFRight = new SimpleMotorFeedforward(ShooterConstants.kRightS, ShooterConstants.kRightV, ShooterConstants.kRightA); + } + + public double getTargetRPM() { + return targetSpeeds == null ? -1 : targetSpeeds.getSpeedRPM(); + } + + public double getLeftCurrentRPM() { + return leftEncoder.getVelocity().get(); + } + + public double getRightCurrentRPM() { + return rightEncoder.getVelocity().get(); + } + + public double getLeftOutputVoltage() { + return leftMotor.getAppliedOutput().get() * leftMotor.getBusVoltage().get(); + } + + public double getRightOutputVoltage() { + return rightMotor.getAppliedOutput().get() * rightMotor.getBusVoltage().get(); + } + + public boolean isLeftAtSetpoint() { + return leftClosedLoopController.isAtSetpoint().get(); + } + + public boolean isRightAtSetpoint() { + return rightClosedLoopController.isAtSetpoint().get(); + } + + public Command maintainSpeed(ShooterSpeeds speeds) { + return run(() -> { + targetSpeeds = speeds; + + if(targetSpeeds == null) { + leftMotor.disable(); + rightMotor.disable(); + } else { + leftClosedLoopController.setSetpoint( + targetSpeeds.getSpeedRPM(), + ControlType.kVelocity, + ClosedLoopSlot.kSlot0, + shooterFFLeft.calculate(targetSpeeds.getSpeedRPM()) + ); + + rightClosedLoopController.setSetpoint( + targetSpeeds.getSpeedRPM(), + ControlType.kVelocity, + ClosedLoopSlot.kSlot0, + shooterFFRight.calculate(targetSpeeds.getSpeedRPM()) + ); + } + }); + } + + public Command manualSpeed(DoubleSupplier speed) { + return run(() -> { + targetSpeeds = null; + + leftMotor.setThrottle(speed.getAsDouble() * ShooterConstants.kMaxManualSpeedMultiplier); + rightMotor.setThrottle(speed.getAsDouble() * ShooterConstants.kMaxManualSpeedMultiplier); + }); + } + + public Command stop() { + return manualSpeed(() -> 0); + } + + public double getAverageActualSpeeds() { + return (leftEncoder.getVelocity().get() + rightEncoder.getVelocity().get()) / 2; + } + + public Optional getTargetSpeeds() { + return Optional.ofNullable(targetSpeeds); + } +} diff --git a/src/main/java/first/robot/subsystems/Spindexer.java b/src/main/java/first/robot/subsystems/Spindexer.java new file mode 100644 index 0000000..2c8b4e3 --- /dev/null +++ b/src/main/java/first/robot/subsystems/Spindexer.java @@ -0,0 +1,100 @@ +package first.robot.subsystems; + +import java.util.function.DoubleSupplier; + +import org.wpilib.command2.Command; +import org.wpilib.command2.SubsystemBase; +import org.wpilib.epilogue.Logged; + +import com.ctre.phoenix6.CANBus; +import com.ctre.phoenix6.controls.DutyCycleOut; +import com.ctre.phoenix6.hardware.TalonFX; +import com.revrobotics.PersistMode; +import com.revrobotics.ResetMode; +import com.revrobotics.spark.SparkLowLevel.MotorType; +import com.revrobotics.spark.SparkMax; + +import first.robot.constants.SpindexerConstants; +import first.robot.interfaces.IIndexer; + +@Logged +public class Spindexer extends SubsystemBase implements IIndexer { + private TalonFX spindexerMotor; + + private SparkMax feederMotor; + + private DutyCycleOut spindexerMotorOutput; + + public Spindexer() { + spindexerMotor = new TalonFX( + SpindexerConstants.kSpindexerMotorCANID, + new CANBus(SpindexerConstants.kCANPort) + ); + + feederMotor = new SparkMax( + SpindexerConstants.kCANPort, + SpindexerConstants.kFeederMotorCANID, + MotorType.kBrushless + ); + + spindexerMotor.getConfigurator().apply(SpindexerConstants.kSpindexerCurrentLimitConfig); + spindexerMotor.getConfigurator().apply(SpindexerConstants.kSpindexerMotorConfig); + + feederMotor.configure( + SpindexerConstants.kFeederConfig, + ResetMode.kResetSafeParameters, + PersistMode.kPersistParameters + ); + + spindexerMotorOutput = new DutyCycleOut(0); + } + + public Command spinToShooter() { + return run(() -> { + spindexerMotor.setControl( + spindexerMotorOutput.withOutput(SpindexerConstants.kSpindexerSpeed) + ); + feederMotor.setThrottle(SpindexerConstants.kFeederSpeed); + }); + } + + public Command spinToShooter(DoubleSupplier shooterSpeedRPM, double cutOffRPM) { + return run(() -> { + if(shooterSpeedRPM.getAsDouble() < cutOffRPM) { + spindexerMotor.setControl( + spindexerMotorOutput.withOutput(0) + ); + feederMotor.setThrottle(0); + } else { + spindexerMotor.setControl( + spindexerMotorOutput.withOutput(SpindexerConstants.kSpindexerSpeed) + ); + feederMotor.setThrottle(SpindexerConstants.kFeederSpeed); + } + }); + } + + public Command spinToIntake() { + return run(() -> { + spindexerMotor.setControl( + spindexerMotorOutput.withOutput(-SpindexerConstants.kSpindexerSpeed) + ); + feederMotor.setThrottle(-SpindexerConstants.kFeederSpeed); + }); + } + + public Command stop() { + return run(() -> { + spindexerMotor.setControl(spindexerMotorOutput.withOutput(0)); + feederMotor.setThrottle(0); + }); + } + + public Command instantaneousStop() { + return runOnce(() -> { + spindexerMotor.setControl(spindexerMotorOutput.withOutput(0)); + feederMotor.setThrottle(0); + }); + } + +} \ No newline at end of file