diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 8d48b3b..9ec85fd 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -10,8 +10,8 @@ import edu.wpi.first.wpilibj2.command.button.Trigger; import frc.robot.Constants.SimulationConstants; import frc.robot.commands.drive.DriveCommand; -import frc.robot.commands.drive.Eject; -import frc.robot.commands.drive.Intake; +import frc.robot.commands.intake.Eject; +import frc.robot.commands.intake.Intake; import frc.robot.extras.simulation.field.SimulatedField; import frc.robot.extras.simulation.mechanismSim.IntakeSimulation.IntakeSide; import frc.robot.extras.simulation.mechanismSim.swerve.GyroSimulation; diff --git a/src/main/java/frc/robot/commands/intake/Eject.java b/src/main/java/frc/robot/commands/intake/Eject.java index 2d0157a..7dfddc6 100644 --- a/src/main/java/frc/robot/commands/intake/Eject.java +++ b/src/main/java/frc/robot/commands/intake/Eject.java @@ -13,14 +13,17 @@ public Eject(IntakeSubsystem intakeSubsystem){ addRequirements(this.intakeSubsystem); } +@Override public void execute(){ intakeSubsystem.setIntakeSpeed(IntakeConstants.ejectSpeed); } - public void end(){ +@Override + public void end(boolean interrupted){ intakeSubsystem.setIntakeSpeed(0); } +@Override public boolean isFinished(){ return false; } diff --git a/src/main/java/frc/robot/commands/intake/Intake.java b/src/main/java/frc/robot/commands/intake/Intake.java index 7d7d7bb..d92c24e 100644 --- a/src/main/java/frc/robot/commands/intake/Intake.java +++ b/src/main/java/frc/robot/commands/intake/Intake.java @@ -12,14 +12,17 @@ public Intake(IntakeSubsystem intakeSubsystem){ addRequirements(this.intakeSubsystem); } + @Override public void execute(){ intakeSubsystem.setIntakeSpeed(IntakeConstants.intakeSpeed); } - public void end(){ + @Override + public void end(boolean interrupted){ intakeSubsystem.setIntakeSpeed(0); } + @Override public boolean isFinished(){ return false; } diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java index 86284e7..11fa87b 100644 --- a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java +++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java @@ -5,4 +5,11 @@ public final class IntakeConstants { public static final int intakeSpeed = 0 - 9; public static final int ejectSpeed = 0 - 9; + + //the max amount of stator and supply current allowed in the motor, respectively + public static final double IntakeStatorLimit = 0.0; + public static final double IntakeSupplyLimit = 0.0; + //Enables stator and supply current limits, respectively + public static final boolean IntakeStatorLimitEnable = true; + public static final boolean IntakeSupplyLimitEnable = true; } diff --git a/src/main/java/frc/robot/subsystems/intake/PhysicalIntake.java b/src/main/java/frc/robot/subsystems/intake/PhysicalIntake.java index af8aa6c..e5244f7 100644 --- a/src/main/java/frc/robot/subsystems/intake/PhysicalIntake.java +++ b/src/main/java/frc/robot/subsystems/intake/PhysicalIntake.java @@ -19,10 +19,10 @@ public PhysicalIntake(){ intakeConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake; intakeConfig.MotorOutput.DutyCycleNeutralDeadband = HardwareConstants.MIN_FALCON_DEADBAND; - intakeConfig.CurrentLimits.StatorCurrentLimit = 0.0; - intakeConfig.CurrentLimits.StatorCurrentLimitEnable = true; - intakeConfig.CurrentLimits.SupplyCurrentLimit = 0.0; - intakeConfig.CurrentLimits.StatorCurrentLimitEnable = true; + intakeConfig.CurrentLimits.StatorCurrentLimit = IntakeConstants.IntakeStatorLimit; + intakeConfig.CurrentLimits.StatorCurrentLimitEnable = IntakeConstants.IntakeStatorLimitEnable; + intakeConfig.CurrentLimits.SupplyCurrentLimit = IntakeConstants.IntakeSupplyLimit; + intakeConfig.CurrentLimits.StatorCurrentLimitEnable = IntakeConstants.IntakeSupplyLimitEnable; motor.getConfigurator().apply(intakeConfig); }