note ejection code, commented out

easier naming in robotcontainer (cmon guys)

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")

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)
+        );
+    }
 }

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);

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);
 
     }