diff --git a/Robot2024/src/main/java/com/team2813/Constants.java b/Robot2024/src/main/java/com/team2813/Constants.java index a97f1d3..128886d 100644 --- a/Robot2024/src/main/java/com/team2813/Constants.java +++ b/Robot2024/src/main/java/com/team2813/Constants.java @@ -40,16 +40,17 @@ public static class OperatorConstants { //square = cross, cross = circle, circle = square - public static final Trigger climbUpButton = operatorController.povUp(); + public static final Trigger climbUpButton = operatorController.povUp().and(() -> false); public static final Trigger climbDownButton = operatorController.povDown(); public static final Trigger altOuttakeButton = new Trigger(() -> operatorXboxController.getLeftTriggerAxis() >= 0.5).and(() -> false); + public static final Trigger sourceIntake = operatorController.povUp(); } public static class DriverConstants { public static final int driverControllerPort = 0; public static final CommandPS4Controller DRIVER_CONTROLLER = new CommandPS4Controller(driverControllerPort); public static final Trigger slowmodeButton = DRIVER_CONTROLLER.L1(); - public static final Trigger orientButton = DRIVER_CONTROLLER.R2(); + public static final Trigger orientButton = DRIVER_CONTROLLER.R2(); // options public static final Trigger spoolAutoAimButton = DRIVER_CONTROLLER.square(); // actually maps to cross public static final Trigger ampIntakeButton = DRIVER_CONTROLLER.triangle(); //R2 public static final Trigger autoAimButton = DRIVER_CONTROLLER.R1(); diff --git a/Robot2024/src/main/java/com/team2813/RobotContainer.java b/Robot2024/src/main/java/com/team2813/RobotContainer.java index b8cf054..42e2cd0 100644 --- a/Robot2024/src/main/java/com/team2813/RobotContainer.java +++ b/Robot2024/src/main/java/com/team2813/RobotContainer.java @@ -25,6 +25,7 @@ import static com.team2813.Constants.OperatorConstants.shootPodium; import static com.team2813.Constants.OperatorConstants.shootWooferFront; import static com.team2813.Constants.OperatorConstants.shootWooferSide; +import static com.team2813.Constants.OperatorConstants.sourceIntake; import com.pathplanner.lib.auto.AutoBuilder; import com.pathplanner.lib.auto.NamedCommands; @@ -44,6 +45,7 @@ import com.team2813.subsystems.Magazine; import com.team2813.subsystems.Shooter; import com.team2813.subsystems.ShooterPivot; +import com.team2813.subsystems.ShooterPivot.Position; import edu.wpi.first.net.PortForwarder; import edu.wpi.first.wpilibj.XboxController; @@ -206,6 +208,32 @@ private void configureBindings(AutoCommands autoCommands) { shootPodium.onTrue(autoCommands.shootPodium()); farSpeaker.onTrue(autoCommands.farSpeaker()); autoAimButton.onTrue(new AutoAimCommand(shooter, shooterPivot, mag, drive)); + + sourceIntake.onTrue( + new SequentialCommandGroup( + new LockFunctionCommand(shooterPivot::atPosition, () -> shooterPivot.setSetpoint(Position.SOURCE_INTAKE), shooterPivot), + new InstantCommand(shooter::reverse, shooter) + ) + ); + sourceIntake.onFalse( + new SequentialCommandGroup( + new ParallelCommandGroup( + new InstantCommand(mag::reverseMag, mag), + new InstantCommand(intake::outtakeNote, intake) + ), + new WaitCommand(0.1), + new ParallelCommandGroup( + new InstantCommand(shooter::stop, shooter), + new InstantCommand(intake::intake, intake), + new InstantCommand(mag::runOnlyMag, mag) + ), + new WaitCommand(0.1), + new ParallelCommandGroup( + new InstantCommand(intake::stopIntakeMotor, intake), + new InstantCommand(mag::stop, mag) + ) + ) + ); } public Command getAutonomousCommand() { diff --git a/Robot2024/src/main/java/com/team2813/commands/DefaultShooterCommand.java b/Robot2024/src/main/java/com/team2813/commands/DefaultShooterCommand.java index 1bcb01f..a0d3fd6 100644 --- a/Robot2024/src/main/java/com/team2813/commands/DefaultShooterCommand.java +++ b/Robot2024/src/main/java/com/team2813/commands/DefaultShooterCommand.java @@ -48,7 +48,7 @@ public void execute() { double val = control.getAsDouble(); if (Math.abs(val) > DEADZONE) { shooterPivot.set(ControlMode.DUTY_CYCLE, val * MULTIPLIER); - } else { + } else if (!shooterPivot.isEnabled()) { shooterPivot.set(ControlMode.DUTY_CYCLE, 0); } } diff --git a/Robot2024/src/main/java/com/team2813/subsystems/Drive.java b/Robot2024/src/main/java/com/team2813/subsystems/Drive.java index debe4fb..3610862 100644 --- a/Robot2024/src/main/java/com/team2813/subsystems/Drive.java +++ b/Robot2024/src/main/java/com/team2813/subsystems/Drive.java @@ -163,13 +163,6 @@ public Drive() { Shuffleboard.getTab("swerve").add("rotation PID", facingRequest.HeadingController); } - private void setLimits(int module) { - drivetrain.getModule(0).getDriveMotor() - .getConfigurator().apply(new CurrentLimitsConfigs() - .withSupplyCurrentLimit(40) - .withSupplyCurrentLimitEnable(true)); - } - final SwerveRequest.FieldCentricFacingAngle facingRequest; public void turnToFace(Rotation2d rotation) { diff --git a/Robot2024/src/main/java/com/team2813/subsystems/Magazine.java b/Robot2024/src/main/java/com/team2813/subsystems/Magazine.java index 9e46c10..d297a02 100644 --- a/Robot2024/src/main/java/com/team2813/subsystems/Magazine.java +++ b/Robot2024/src/main/java/com/team2813/subsystems/Magazine.java @@ -48,5 +48,6 @@ public void runOnlyMag() { public void reverseMag() { magMotor.set(ControlMode.DUTY_CYCLE, -0.6); + kickerMotor.set(ControlMode.DUTY_CYCLE, -0.6); } } \ No newline at end of file diff --git a/Robot2024/src/main/java/com/team2813/subsystems/Shooter.java b/Robot2024/src/main/java/com/team2813/subsystems/Shooter.java index f8c953e..e384312 100644 --- a/Robot2024/src/main/java/com/team2813/subsystems/Shooter.java +++ b/Robot2024/src/main/java/com/team2813/subsystems/Shooter.java @@ -58,6 +58,11 @@ public boolean atVelocity() { return shooterMotor.getVelocity() - targetVelocity > -1; } + public void reverse() { + targetVelocity = 0; + shooterMotor.set(ControlMode.DUTY_CYCLE, -0.2); + } + public void stop() { targetVelocity = 0; shooterMotor.set(ControlMode.VELOCITY, 0); diff --git a/Robot2024/src/main/java/com/team2813/subsystems/ShooterPivot.java b/Robot2024/src/main/java/com/team2813/subsystems/ShooterPivot.java index 7693b9f..a990f3c 100644 --- a/Robot2024/src/main/java/com/team2813/subsystems/ShooterPivot.java +++ b/Robot2024/src/main/java/com/team2813/subsystems/ShooterPivot.java @@ -70,6 +70,7 @@ public static enum Position implements MotorSubsystem.Position { PODIUM(0.076660), TEST(0.067871), FAR_SPEAKER(0.088135), + SOURCE_INTAKE(0.048096), BOTTOM_HARD_STOP(0.099854); private final double pos;