@@ -463,7 +463,9 @@ private void configureBindings() {
463
463
/* ElEVATOR TEST */
464
464
465
465
dPadUp .onTrue (new ElevatorToTrapCommand (elevatorSubsystem ));
466
+ dPadUp .onTrue (new InstantCommand (() -> intakePivotSubsystem .enablePowerLimit (false )));
466
467
dPadDown .onTrue (new ElevatorToLimitSwitchCommand (elevatorSubsystem ));
468
+ dPadDown .onTrue (new InstantCommand (() -> intakePivotSubsystem .enablePowerLimit (true )));
467
469
468
470
// elevatorSubsystem.setManual();
469
471
@@ -510,31 +512,40 @@ private void configureBindings() {
510
512
// if the elevator is down, run the amp sequence
511
513
rightBumper .onTrue (
512
514
new ConditionalCommand (
513
- // if elevator is up
514
- new ElevatorToZeroCommand (elevatorSubsystem ).alongWith (new InstantCommand (// lower the elevator
515
- () -> intakePivotSubsystem .setPosition (0 ), intakePivotSubsystem )), // stow the pivot
516
-
517
- // if elevator is down
518
- new PrepareAmpSequence (elevatorSubsystem , intakePivotSubsystem , intakeRollerSubsystem )
519
- .until (() -> mechController .getLeftTriggerAxis () > .05
520
- || mechController .getRightTriggerAxis () > .05 ),
521
-
522
- // check if the elevator is currently targeting one of the upper positions to choose what to do
523
- () -> elevatorSubsystem .getTargetState () == ElevatorState .AMP
524
- || elevatorSubsystem .getTargetState () == ElevatorState .TRAP ));
515
+ // if elevator is up
516
+ new ElevatorToZeroCommand (elevatorSubsystem ).alongWith (
517
+ Commands .runOnce (() -> {
518
+ intakePivotSubsystem .enablePowerLimit (true );
519
+ intakePivotSubsystem .setPosition (0 );
520
+ }, intakePivotSubsystem )
521
+ ), // stow the pivot
522
+
523
+ // if elevator is down
524
+ new PrepareAmpSequence (elevatorSubsystem , intakePivotSubsystem , intakeRollerSubsystem )
525
+ .until (() -> mechController .getLeftTriggerAxis () > .05
526
+ || mechController .getRightTriggerAxis () > .05 ),
527
+
528
+ // check if the elevator is currently targeting one of the upper positions to choose what to do
529
+ () -> elevatorSubsystem .getTargetState () == ElevatorState .AMP
530
+ || elevatorSubsystem .getTargetState () == ElevatorState .TRAP
531
+ )
532
+ );
525
533
526
534
// leftBumper toggles the trap position for the elevator
527
535
leftBumper .onTrue (
528
536
new ConditionalCommand (
529
537
new ElevatorToZeroCommand (elevatorSubsystem ).alongWith (new InstantCommand (// lower the elevator
530
- () -> intakePivotSubsystem .setPosition (0 ), intakePivotSubsystem )), // stow intake
538
+ () -> {
539
+ intakePivotSubsystem .enablePowerLimit (true );
540
+ intakePivotSubsystem .setPosition (0 );
541
+ }, intakePivotSubsystem )), // stow intake
531
542
new ConditionalCommand (
532
543
new ElevatorToTrapCommand (elevatorSubsystem ).andThen (
533
- new IntakePivotSetPositionCommand (intakePivotSubsystem , .45 ).withTimeout (.1 )
544
+ new InstantCommand (() -> intakePivotSubsystem .enablePowerLimit (false )),
545
+ new IntakePivotSetPositionCommand (intakePivotSubsystem , .45 )
534
546
),
535
547
new IntakePivotSetPositionCommand (intakePivotSubsystem , 1 ).andThen (// extend pivot
536
- new IntakeRollerOuttakeCommand (intakeRollerSubsystem , .17 , .75 ) // run rollers to front sensor
537
- .until (() -> intakeRollerSubsystem .getFrontSensorReached ()),
548
+ new ConditionalWaitCommand (intakeRollerSubsystem ::getFrontSensorReached ),
538
549
new IntakePivotSetPositionCommand (intakePivotSubsystem , 0 )
539
550
),
540
551
intakeRollerSubsystem ::getAmpSensor
@@ -544,18 +555,36 @@ private void configureBindings() {
544
555
);
545
556
546
557
558
+ // roll behaviors
559
+ leftBumper .onTrue (
560
+ new ConditionalCommand (
561
+ null ,
562
+ new ConditionalCommand (
563
+ null ,
564
+ new WaitCommand (.05 ).andThen (
565
+ new ConditionalWaitCommand (intakePivotSubsystem ::atPosition ), // extend pivot
566
+ new IntakeRollerOuttakeCommand (intakeRollerSubsystem , .17 , .75 ) // run rollers to front sensor
567
+ .until (intakeRollerSubsystem ::getFrontSensorReached )
568
+ ),
569
+ intakeRollerSubsystem ::getAmpSensor
570
+ ), // raise the elevator
571
+ () -> elevatorSubsystem .getTargetState () == ElevatorState .AMP // check if targeting a high pos
572
+ || elevatorSubsystem .getTargetState () == ElevatorState .TRAP )
573
+ );
574
+
575
+
547
576
548
577
// aButton runs the intake sequence
549
578
aButton .onTrue (
550
- new InstantCommand (() -> { intakePivotSubsystem .setPosition (1 );} , intakePivotSubsystem ).alongWith (// then extend the intake
579
+ Commands . runOnce (() -> intakePivotSubsystem .setPosition (1 ), intakePivotSubsystem ).alongWith (
551
580
new IntakeRollerAmpIntakeCommand (intakeRollerSubsystem )).andThen (
552
581
new ConditionalCommand (
553
582
new IntakeRollerIntakeCommand (intakeRollerSubsystem , lightBarSubsystem ).andThen (
554
- new InstantCommand (() -> { intakePivotSubsystem .setPosition (0 );} , intakePivotSubsystem )
583
+ new InstantCommand (() -> intakePivotSubsystem .setPosition (0 ), intakePivotSubsystem )
555
584
),
556
585
557
586
new InstantCommand (
558
- () -> { intakePivotSubsystem .setPosition (0 );} , intakePivotSubsystem
587
+ () -> intakePivotSubsystem .setPosition (0 ), intakePivotSubsystem
559
588
),
560
589
() -> aButton .getAsBoolean ()
561
590
)
@@ -564,7 +593,7 @@ private void configureBindings() {
564
593
);
565
594
566
595
// bButton stops the rollers
567
- bButton .onTrue (new InstantCommand (() -> {}, intakeRollerSubsystem ));
596
+ bButton .onTrue (Commands . idle ( intakeRollerSubsystem ));
568
597
569
598
// xButton toggles the intake being stowed
570
599
xButton .onTrue (new InstantCommand (() -> {
@@ -592,14 +621,18 @@ private void configureBindings() {
592
621
new IntakePivotSetPositionCommand (intakePivotSubsystem , 1 ).andThen (
593
622
new IntakeRollerIntakeCommand (intakeRollerSubsystem , lightBarSubsystem ),
594
623
new IntakePivotSetPositionCommand (intakePivotSubsystem , intakePosition )
595
- ).unless (intakeRollerSubsystem ::getRockwellSensorValue )
624
+ ).unless (intakeRollerSubsystem ::getRockwellSensorValue ).andThen (
625
+ new IntakePivotSetPositionCommand (intakePivotSubsystem , 0 )
626
+ )
596
627
);
597
628
598
629
shooterFlywheelSubsystem .setDefaultCommand (new InstantCommand (() -> {
599
630
if (yButton .getAsBoolean ()) {
600
631
lightBarSubsystem .setLightBarStatus (LightBarStatus .SHOOTER_SPIN_UP , 2 );
601
632
// shooterFlywheelSubsystem.setShooterMotorSpeed(shooterSpeed); // for tuning
602
- shooterFlywheelSubsystem .setShooterMotorSpeed ();
633
+ if (intakePivotSubsystem .atPosition ()) {
634
+ shooterFlywheelSubsystem .setShooterMotorSpeed ();
635
+ }
603
636
shooterPivotSubsystem .setAutoAimBoolean (true );
604
637
if (shooterFlywheelSubsystem .atSpeed ()) {
605
638
mechController .setRumble (RumbleType .kBothRumble , .4 );
@@ -658,8 +691,11 @@ private void configureBindings() {
658
691
new IntakePivotSetPositionCommand (intakePivotSubsystem , intakePosition )
659
692
).unless (intakeRollerSubsystem ::getRockwellSensorValue ));
660
693
661
- dPadRight .onTrue (new ShooterFlywheelShuttleCommand (swerveSubsystem , shooterFlywheelSubsystem , fmsSubsystem , shooterPivotSubsystem , .65 , mechController ).onlyWhile (dPadRight ));
662
-
694
+ dPadRight .whileTrue (
695
+ new ConditionalWaitCommand (() -> intakePivotSubsystem .atPosition ())
696
+ .andThen (new ShooterFlywheelShuttleCommand (swerveSubsystem , shooterFlywheelSubsystem , fmsSubsystem ,
697
+ shooterPivotSubsystem , .7 , mechController ))
698
+ );
663
699
// intakePivotSubsystem.setDefaultCommand(new InstantCommand(() -> {
664
700
// intakePosition = MathUtil.clamp(intakePosition, 0, 1);
665
701
// intakePivotSubsystem.setPosition(intakePosition);
0 commit comments