From 9728aa2ae7663829e7bb5272f63d1d52e4c20e5c Mon Sep 17 00:00:00 2001 From: William Yuan <66759156+Wi11iamYuan@users.noreply.github.com> Date: Mon, 7 Oct 2024 23:04:21 -0700 Subject: [PATCH] note ejection code, commented out easier naming in robotcontainer (cmon guys) --- .../java/frc/team3128/RobotContainer.java | 104 ++++++++++++------ .../frc/team3128/commands/CmdManager.java | 14 +++ .../java/frc/team3128/subsystems/Amper.java | 3 +- .../java/frc/team3128/subsystems/Intake.java | 4 +- 4 files changed, 91 insertions(+), 34 deletions(-) diff --git a/src/main/java/frc/team3128/RobotContainer.java b/src/main/java/frc/team3128/RobotContainer.java index a30c373..0d425b9 100644 --- a/src/main/java/frc/team3128/RobotContainer.java +++ b/src/main/java/frc/team3128/RobotContainer.java @@ -59,6 +59,11 @@ public class RobotContainer { private Swerve swerve; + private Hopper hopper; + private Intake intake; + private Shooter shooter; + + private Leds leds; // private NAR_ButtonBoard judgePad; @@ -82,8 +87,13 @@ public RobotContainer() { controller = new NAR_XboxController(2); buttonPad = new NAR_ButtonBoard(3); + swerve = Swerve.getInstance(); + hopper = Hopper.getInstance(); + intake = Intake.getInstance(); + shooter = Shooter.getInstance(); + //uncomment line below to enable driving - CommandScheduler.getInstance().setDefaultCommand(Swerve.getInstance(), new CmdSwerveDrive(controller::getLeftX,controller::getLeftY, controller::getRightX, true)); + CommandScheduler.getInstance().setDefaultCommand(swerve, new CmdSwerveDrive(controller::getLeftX,controller::getLeftY, controller::getRightX, true)); initRobotTest(); @@ -116,55 +126,87 @@ private void configureButtonBindings() { controller.getButton(XboxButton.kLeftTrigger).onTrue(intake(Intake.Setpoint.GROUND)); controller.getButton(XboxButton.kLeftBumper).onTrue(retractIntake()); - controller.getButton(XboxButton.kRightTrigger).onTrue(Shooter.getInstance().rampUpShooter()).onFalse(Shooter.getInstance().setShooting(true)); + controller.getButton(XboxButton.kRightTrigger).onTrue(shooter.rampUpShooter()).onFalse(shooter.setShooting(true)); - controller.getButton(XboxButton.kA).onTrue(Shooter.getInstance().runShooter(0.8)); - controller.getButton(XboxButton.kY).onTrue(Shooter.getInstance().runShooter(0)); - controller.getButton(XboxButton.kB).onTrue(Shooter.getInstance().runKickMotor(KICK_SHOOTING_POWER)).onFalse(Shooter.getInstance().runKickMotor(0)); + controller.getButton(XboxButton.kA).onTrue(shooter.runShooter(0.8)); + controller.getButton(XboxButton.kY).onTrue(shooter.runShooter(0)); + controller.getButton(XboxButton.kB).onTrue(shooter.runKickMotor(KICK_SHOOTING_POWER)).onFalse(shooter.runKickMotor(0)); controller.getButton(XboxButton.kY).whileTrue(ampUp()).onFalse(ampFinAndDown()); // new Trigger(()->true).onTrue(queueNote()); - new Trigger(()-> Shooter.getInstance().getShooting()) + //Shooting + new Trigger(()-> shooter.getShooting()) .onTrue(sequence( - Shooter.getInstance().runKickMotor(KICK_POWER), - Hopper.getInstance().runManipulator(.8) + shooter.runKickMotor(KICK_POWER), + hopper.runManipulator(.8) )) .onFalse(sequence( - Shooter.getInstance().runKickMotor(0), - Hopper.getInstance().runManipulator(0), - Shooter.getInstance().stopMotors() + hopper.runManipulator(0), + shooter.stopMotors() )); - new Trigger(()-> Shooter.getInstance().noteInRollers()).negate() - .and(()->Hopper.getInstance().hasObjectPresent()).negate() - .onTrue(Shooter.getInstance().setShooting(false)); - - new Trigger(()-> Intake.getInstance().getMeasurement() > 90) - .and(()->!Hopper.getInstance().hasObjectPresent()) - .onTrue(Hopper.getInstance().runManipulator(HOPPER_INTAKE_POWER)) - .onFalse(Hopper.getInstance().runManipulator(0)); - - new Trigger(()-> Intake.getInstance().getMeasurement() < 20) - .and(()->Hopper.getInstance().hasObjectPresent()).negate() - .onTrue(Hopper.getInstance().runManipulator(0)); - - new Trigger(()-> Shooter.getInstance().noteInRollers()).negate() - .and(()->Hopper.getInstance().hasObjectPresent()) - .and(() -> !Shooter.getInstance().getShooting()) + //Stops shooting when all notes are gone + new Trigger(()-> shooter.noteInRollers()).negate() + .and(()->hopper.hasObjectPresent()).negate() + .onTrue(shooter.setShooting(false)); + + //Queues note to hopper + new Trigger(()-> intake.getMeasurement() > 90) + .and(()->!hopper.hasObjectPresent()) + .onTrue(hopper.runManipulator(HOPPER_INTAKE_POWER)) + .onFalse(hopper.runManipulator(0)); + + //Stops hopper if intake is retracted and is empty + new Trigger(()-> intake.getMeasurement() < 20) + .and(()->hopper.hasObjectPresent()).negate() + .onTrue(hopper.runManipulator(0)); + + //Queues note to shooter + new Trigger(()-> shooter.noteInRollers()).negate() + .and(()->hopper.hasObjectPresent()) + .and(() -> !shooter.getShooting()) .onTrue(sequence( - Shooter.getInstance().runKickMotor(KICK_POWER), - Hopper.getInstance().runManipulator(HOPPER_INTAKE_POWER) + shooter.runKickMotor(KICK_POWER), + hopper.runManipulator(HOPPER_INTAKE_POWER) )) .onFalse(sequence( - Shooter.getInstance().runKickMotor(-.1), + shooter.runKickMotor(-.1), waitSeconds(.1), - Shooter.getInstance().runKickMotor(0) + shooter.runKickMotor(0) )); + // new Trigger(() -> shouldEjectNote()).onTrue(ejectNote()); + + } + + private Timer ejecTimer = new Timer(); + private boolean ejectTimerStarted = false; + private boolean shouldEjectNote(){ + if(shooter.noteInRollers() && hopper.hasObjectPresent() && !ejectTimerStarted){ + ejectTimerStarted = true; + ejecTimer.start(); + } + + else if(shooter.noteInRollers() && hopper.hasObjectPresent() && ejectTimerStarted){ + if(ejecTimer.hasElapsed(2)){ + ejectTimerStarted = false; + ejecTimer.stop(); + ejecTimer.reset(); + return true; + } + } + + else{ + ejectTimerStarted = false; + ejecTimer.stop(); + ejecTimer.reset(); + } + + return false; } @SuppressWarnings("unused") diff --git a/src/main/java/frc/team3128/commands/CmdManager.java b/src/main/java/frc/team3128/commands/CmdManager.java index 93e3761..34c5f46 100644 --- a/src/main/java/frc/team3128/commands/CmdManager.java +++ b/src/main/java/frc/team3128/commands/CmdManager.java @@ -9,6 +9,7 @@ import common.hardware.input.NAR_XboxController; import edu.wpi.first.wpilibj.DriverStation.Alliance; import edu.wpi.first.wpilibj2.command.Command; +import edu.wpi.first.wpilibj2.command.CommandScheduler; import edu.wpi.first.wpilibj2.command.ScheduleCommand; import edu.wpi.first.wpilibj2.command.StartEndCommand; import frc.team3128.Robot; @@ -174,4 +175,17 @@ public static Command ampFinAndDown(){ amper.retract() ); } + + public static Command ejectNote(){ + CommandScheduler.getInstance().cancel(CommandScheduler.getInstance().requiring(hopper)); + CommandScheduler.getInstance().cancel(CommandScheduler.getInstance().requiring(intake)); + return sequence( + intake.stopRollers(), + intake.retract(), + waitSeconds(0.3), + hopper.runManipulator(HOPPER_OUTTAKE_POWER), + waitSeconds(0.2), + hopper.runManipulator(0) + ); + } } \ No newline at end of file diff --git a/src/main/java/frc/team3128/subsystems/Amper.java b/src/main/java/frc/team3128/subsystems/Amper.java index b396114..fa7d588 100644 --- a/src/main/java/frc/team3128/subsystems/Amper.java +++ b/src/main/java/frc/team3128/subsystems/Amper.java @@ -39,7 +39,8 @@ private Amper() { super(new TrapController(PIDConstants, TRAP_CONSTRAINTS), ELEV_MOTOR); //TODO: remove once done testing - this.setSafetyThresh(100); + // this.setSafetyThresh(100); + // setkG_Function(() -> getMeasurement()*Math.sin(AMPER_ANGLE)); setTolerance(POSITION_TOLERANCE); diff --git a/src/main/java/frc/team3128/subsystems/Intake.java b/src/main/java/frc/team3128/subsystems/Intake.java index 85745d3..06340e2 100644 --- a/src/main/java/frc/team3128/subsystems/Intake.java +++ b/src/main/java/frc/team3128/subsystems/Intake.java @@ -41,11 +41,11 @@ private Intake() { setTolerance(ANGLE_TOLERANCE); setConstraints(MIN_SETPOINT, MAX_SETPOINT); - setSafetyThresh(5); + // setSafetyThresh(5); // initShuffleboard(); //TODO: remove once done testing - this.setSafetyThresh(1000); + // this.setSafetyThresh(1000); }