diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java index 3f7792b..9302645 100644 --- a/src/main/java/frc/robot/Constants.java +++ b/src/main/java/frc/robot/Constants.java @@ -207,7 +207,7 @@ public static final class Setpoints { public static final ElevatorState GROUND_INTAKE_CONE = new ElevatorState(MIN_EXTENSION_INCHES, 75); public static final ElevatorState GROUND_INTAKE_CUBE = - new ElevatorState(MIN_EXTENSION_INCHES, 81.4); + new ElevatorState(MIN_EXTENSION_INCHES, 85); public static final ElevatorState SCORE_HIGH_CONE = new ElevatorState(43, 52.5); public static final ElevatorState SCORE_HIGH_CUBE = new ElevatorState(53.8, 74.5); public static final ElevatorState SCORE_MID_CONE = new ElevatorState(22.5, 35.5); @@ -255,6 +255,9 @@ public static final class Intake { public static final double HOLD_CONE_PERCENT = 0.1; public static final double HOLD_CUBE_PERCENT = -0.1; + public static final double CONE_STATOR_LIMIT = 75; + public static final double CUBE_STATOR_LIMIT = 65; + public static final class Ports { public static final int INTAKE_MOTOR_PORT = CAN.at(18, "intake motor"); public static final int CONE_TOF_PORT = 0; diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java index 405aa3b..757d269 100644 --- a/src/main/java/frc/robot/RobotContainer.java +++ b/src/main/java/frc/robot/RobotContainer.java @@ -219,6 +219,8 @@ private void configureButtonBindings() { anthony.y().onTrue(new HaltDriveCommandsCommand(drivebaseSubsystem)); + anthony.leftStick().onTrue(new HaltDriveCommandsCommand(drivebaseSubsystem)); + jacob.leftStick().onTrue(new InstantCommand(() -> {}, elevatorSubsystem)); jacob.start().onTrue(new SetZeroModeCommand(elevatorSubsystem)); diff --git a/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java b/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java index 5261718..5cff800 100644 --- a/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java +++ b/src/main/java/frc/robot/commands/GroundIntakeElevatorCommand.java @@ -4,6 +4,7 @@ import frc.robot.Constants; import frc.robot.Constants.Elevator; import frc.robot.subsystems.ElevatorSubsystem; +import frc.robot.subsystems.ElevatorSubsystem.Modes; import frc.util.Util; import java.util.function.BooleanSupplier; @@ -20,6 +21,11 @@ public GroundIntakeElevatorCommand(ElevatorSubsystem elevatorSubsystem, BooleanS addRequirements(this.elevatorSubsystem); } + @Override + public void initialize() { + elevatorSubsystem.setMode(Modes.POSITION_CONTROL); + } + @Override public void execute() { if (isCube.getAsBoolean()) { @@ -28,16 +34,4 @@ public void execute() { elevatorSubsystem.setTargetState(Constants.Elevator.Setpoints.GROUND_INTAKE_CONE); } } - - @Override - public boolean isFinished() { - return Util.epsilonEquals( - elevatorSubsystem.getCurrentAngleDegrees(), - elevatorSubsystem.getTargetAngle(), - Elevator.ANGLE_EPSILON) - && Util.epsilonEquals( - elevatorSubsystem.getCurrentExtensionInches(), - elevatorSubsystem.getTargetExtension(), - Elevator.EXTENSION_EPSILON); - } } diff --git a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java index ad7e19b..0e2e68d 100644 --- a/src/main/java/frc/robot/subsystems/IntakeSubsystem.java +++ b/src/main/java/frc/robot/subsystems/IntakeSubsystem.java @@ -20,7 +20,6 @@ public class IntakeSubsystem extends SubsystemBase { private Modes currentIntakeMode; private LinearFilter filter; private double filterOutput; - private double statorCurrentLimit; private boolean isCube; // private final TimeOfFlight coneToF, cubeToF; @@ -40,8 +39,6 @@ public IntakeSubsystem() { filterOutput = 0.0; - statorCurrentLimit = 75; - isCube = false; // coneToF = new TimeOfFlight(Constants.Intake.Ports.CONE_TOF_PORT); @@ -122,7 +119,9 @@ private double getFilterCalculatedValue() { @Override public void periodic() { filterOutput = filter.calculate(intakeMotor.getStatorCurrent()); - if (filterOutput >= statorCurrentLimit) { + if(isCube && filterOutput >= Intake.CUBE_STATOR_LIMIT) { + currentIntakeMode = Modes.HOLD; + } else if(filterOutput >= Intake.CUBE_STATOR_LIMIT) { currentIntakeMode = Modes.HOLD; }