12
12
13
13
import com .choreo .lib .ChoreoTrajectory ;
14
14
import com .fasterxml .jackson .databind .util .Named ;
15
- import com .pathplanner .lib .auto .AutoBuilder ;
16
- import com .pathplanner .lib .auto .NamedCommands ;
17
- import com .pathplanner .lib .commands .PathPlannerAuto ;
18
- import com .pathplanner .lib .path .PathPlannerPath ;
19
- import com .pathplanner .lib .util .PathPlannerLogging ;
15
+ // import com.pathplanner.lib.auto.AutoBuilder;
16
+ // import com.pathplanner.lib.auto.NamedCommands;
17
+ // import com.pathplanner.lib.commands.PathPlannerAuto;
18
+ // import com.pathplanner.lib.path.PathPlannerPath;
19
+ // import com.pathplanner.lib.util.PathPlannerLogging;
20
20
21
21
import edu .wpi .first .cscore .MjpegServer ;
22
22
import edu .wpi .first .cscore .UsbCamera ;
@@ -101,15 +101,15 @@ public class RobotContainer {
101
101
private final BaseDriveController driveController ;
102
102
private final SwerveSubsystem swerveSubsystem ;
103
103
104
- // private final IntakePivotSubsystem intakePivotSubsystem;
105
- // private final IntakeRollerSubsystem intakeRollerSubsystem;
104
+ private final IntakePivotSubsystem intakePivotSubsystem ;
105
+ private final IntakeRollerSubsystem intakeRollerSubsystem ;
106
106
107
- // private final ShooterFlywheelSubsystem shooterFlywheelSubsystem;
108
- // private final ShooterPivotSubsystem shooterPivotSubsystem;
107
+ private final ShooterFlywheelSubsystem shooterFlywheelSubsystem ;
108
+ private final ShooterPivotSubsystem shooterPivotSubsystem ;
109
109
110
110
// private final ClimbSubsystem climbSubsystem;
111
111
112
- // private final ElevatorSubsystem elevatorSubsystem;
112
+ private final ElevatorSubsystem elevatorSubsystem ;
113
113
114
114
private final FieldManagementSubsystem fmsSubsystem ;
115
115
private final LightBarSubsystem lightBarSubsystem ;
@@ -185,7 +185,7 @@ public RobotContainer() {
185
185
listEntry = autonTable .getEntry ("AutonList" );
186
186
selectedAutonEntry = autonTable .getEntry ("Selected" );
187
187
// System.out.print("Available Autons: " + AutoBuilder.getAllAutoNames().toArray(new String[0]));
188
- listEntry .setStringArray (AutoBuilder .getAllAutoNames ().toArray (new String [0 ]));
188
+ // listEntry.setStringArray(AutoBuilder.getAllAutoNames().toArray(new String[0]));
189
189
selectedAutonEntry .setString (autonValue );
190
190
autonTable .addListener ("Auton" , EnumSet .of (NetworkTableEvent .Kind .kValueAll ), (table , key , event ) -> {
191
191
this .autonValue = event .valueData .value .getString ();
@@ -199,19 +199,19 @@ public RobotContainer() {
199
199
swerveSubsystem = new SwerveSubsystem (fmsSubsystem ::isRedAlliance );
200
200
swerveSubsystem .setVerbose (false ); // SET THIS TO true FOR TUNING VALUES
201
201
202
- // intakePivotSubsystem = new IntakePivotSubsystem();
203
- // intakeRollerSubsystem = new IntakeRollerSubsystem(lightBarSubsystem);
202
+ intakePivotSubsystem = new IntakePivotSubsystem ();
203
+ intakeRollerSubsystem = new IntakeRollerSubsystem (lightBarSubsystem );
204
204
205
- // shooterPivotSubsystem = new ShooterPivotSubsystem(
206
- // swerveSubsystem::getShootingDistance,
207
- // fmsSubsystem::isRedAlliance
208
- // );
209
- // shooterFlywheelSubsystem = new ShooterFlywheelSubsystem(
210
- // swerveSubsystem::getShootingDistance,
211
- // fmsSubsystem::isRedAlliance
212
- // );
205
+ shooterPivotSubsystem = new ShooterPivotSubsystem (
206
+ swerveSubsystem ::getShootingDistance ,
207
+ fmsSubsystem ::isRedAlliance
208
+ );
209
+ shooterFlywheelSubsystem = new ShooterFlywheelSubsystem (
210
+ swerveSubsystem ::getShootingDistance ,
211
+ fmsSubsystem ::isRedAlliance
212
+ );
213
213
214
- // elevatorSubsystem = new ElevatorSubsystem();
214
+ elevatorSubsystem = new ElevatorSubsystem ();
215
215
216
216
// climbSubsystem = new ClimbSubsystem();
217
217
@@ -342,7 +342,7 @@ private void configureBindings() {
342
342
xError .setValue (xPID .getPositionError ());
343
343
yError .setValue (yPID .getPositionError ());
344
344
345
- }, swerveSubsystem ));}
345
+ }, swerveSubsystem ));
346
346
347
347
// /* Pressing the button resets the field axes to the current robot axes. */
348
348
// driveController.getDriverHeadingResetButton().onTrue(new InstantCommand(() -> {
@@ -490,8 +490,8 @@ private void configureBindings() {
490
490
// elevatorToZero.onTrue(new ElevatorToLimitSwitchCommand(elevatorSubsystem));
491
491
/* INTAKE TEST */
492
492
493
- // xButton.onTrue(new InstantCommand(() -> intakePivotSubsystem.setPosition(.3),
494
- // intakePivotSubsystem));
493
+ xButton .onTrue (new InstantCommand (() -> intakePivotSubsystem .setPosition (.3 ),
494
+ intakePivotSubsystem ));
495
495
496
496
// rightBumper.onTrue(new InstantCommand(() ->
497
497
// intakePivotSubsystem.setPosition(0), intakePivotSubsystem));
@@ -562,138 +562,139 @@ private void configureBindings() {
562
562
// );
563
563
564
564
565
- // // roll behaviors
566
- // leftBumper.onTrue(
567
- // new ConditionalCommand(
568
- // new InstantCommand(),
569
- // new ConditionalCommand(
570
- // new InstantCommand(),
571
- // new WaitCommand(.05).andThen(
572
- // new ConditionalWaitCommand(intakePivotSubsystem::atPosition), // extend pivot
573
- // new IntakeRollerOuttakeCommand(intakeRollerSubsystem, .17, .75) // run rollers to front sensor
574
- // .until(intakeRollerSubsystem::getFrontSensorReached)
575
- // ),
576
- // intakeRollerSubsystem::getAmpSensor
577
- // ), // raise the elevator
578
- // () -> elevatorSubsystem.getTargetState() == ElevatorState.AMP // check if targeting a high pos
579
- // || elevatorSubsystem.getTargetState() == ElevatorState.TRAP)
580
- // );
565
+ // roll behaviors
566
+ leftBumper .onTrue (
567
+ new ConditionalCommand (
568
+ new InstantCommand (),
569
+ new ConditionalCommand (
570
+ new InstantCommand (),
571
+ new WaitCommand (.05 ).andThen (
572
+ new ConditionalWaitCommand (intakePivotSubsystem ::atPosition ), // extend pivot
573
+ new IntakeRollerOuttakeCommand (intakeRollerSubsystem , .17 , .75 ) // run rollers to front sensor
574
+ .until (intakeRollerSubsystem ::getFrontSensorReached )
575
+ ),
576
+ intakeRollerSubsystem ::getAmpSensor
577
+ ), // raise the elevator
578
+ () -> elevatorSubsystem .getTargetState () == ElevatorState .AMP // check if targeting a high pos
579
+ || elevatorSubsystem .getTargetState () == ElevatorState .TRAP )
580
+ );
581
+
582
+
583
+
584
+ // aButton runs the intake sequence
585
+ aButton .onTrue (
586
+ Commands .runOnce (() -> intakePivotSubsystem .setPosition (1 ), intakePivotSubsystem ).alongWith (
587
+ new IntakeRollerAmpIntakeCommand (intakeRollerSubsystem )).andThen (
588
+ new ConditionalCommand (
589
+ new WaitCommand (.3 ).andThen (
590
+ new IntakeRollerIntakeCommand (intakeRollerSubsystem , lightBarSubsystem ).andThen (
591
+ new InstantCommand (() -> intakePivotSubsystem .setPosition (0 ), intakePivotSubsystem )
592
+ )
593
+ ),
594
+
595
+ new InstantCommand (
596
+ () -> intakePivotSubsystem .setPosition (0 ), intakePivotSubsystem
597
+ ),
598
+ aButton
599
+ )
600
+ ).until (() -> mechController .getLeftTriggerAxis () > .1 ) // cancel if try to outtake
601
+
602
+ );
603
+
604
+ // bButton stops the rollers
605
+ bButton .onTrue (Commands .idle (intakeRollerSubsystem ).withTimeout (0 ));
606
+
607
+ // xButton toggles the intake being stowed
608
+ xButton .onTrue (new InstantCommand (() -> {
609
+ double outPosition = 1 ;
610
+ double stowPosition = 0 ;
611
+ if (elevatorSubsystem .getExtensionPercent () > .5
612
+ && elevatorSubsystem .getTargetState () == ElevatorState .TRAP ) {
613
+ outPosition = .45 ; // push intake out
614
+ } else if (elevatorSubsystem .getExtensionPercent () > .5
615
+ && elevatorSubsystem .getTargetState () == ElevatorState .AMP ) {
616
+ stowPosition = .2 ;
617
+ }
581
618
619
+ intakePivotSubsystem .setPosition (
620
+ intakePivotSubsystem .getEncoderPosition () < (outPosition + stowPosition ) / 2
621
+ ? outPosition
622
+ : stowPosition
623
+ );
582
624
625
+ }, intakePivotSubsystem ));
583
626
584
- // // aButton runs the intake sequence
585
- // aButton.onTrue(
586
- // Commands.runOnce(() -> intakePivotSubsystem.setPosition(1), intakePivotSubsystem).alongWith(
587
- // new IntakeRollerAmpIntakeCommand(intakeRollerSubsystem)).andThen(
588
- // new ConditionalCommand(
589
- // new WaitCommand(.3).andThen(
590
- // new IntakeRollerIntakeCommand(intakeRollerSubsystem, lightBarSubsystem).andThen(
591
- // new InstantCommand(() -> intakePivotSubsystem.setPosition(0), intakePivotSubsystem)
592
- // )
593
- // ),
594
-
595
- // new InstantCommand(
596
- // () -> intakePivotSubsystem.setPosition(0), intakePivotSubsystem
597
- // ),
598
- // aButton
599
- // )
600
- // ).until(() -> mechController.getLeftTriggerAxis() > .1) // cancel if try to outtake
601
-
602
- // );
627
+ // yButton runs the flywheels
603
628
604
- // // bButton stops the rollers
605
- // bButton.onTrue(Commands.idle(intakeRollerSubsystem).withTimeout(0));
606
-
607
- // // xButton toggles the intake being stowed
608
- // xButton.onTrue(new InstantCommand(() -> {
609
- // double outPosition = 1;
610
- // double stowPosition = 0;
611
- // if (elevatorSubsystem.getExtensionPercent() > .5
612
- // && elevatorSubsystem.getTargetState() == ElevatorState.TRAP) {
613
- // outPosition = .45; // push intake out
614
- // } else if (elevatorSubsystem.getExtensionPercent() > .5
615
- // && elevatorSubsystem.getTargetState() == ElevatorState.AMP) {
616
- // stowPosition = .2;
617
- // }
618
-
619
- // intakePivotSubsystem.setPosition(
620
- // intakePivotSubsystem.getEncoderPosition() < (outPosition + stowPosition) / 2
621
- // ? outPosition
622
- // : stowPosition
623
- // );
624
-
625
- // }, intakePivotSubsystem));
626
-
627
- // // yButton runs the flywheels
628
-
629
- // yButton.onTrue(
630
- // new IntakePivotSetPositionCommand(intakePivotSubsystem, 1).andThen(
631
- // new IntakeRollerIntakeCommand(intakeRollerSubsystem, lightBarSubsystem),
632
- // new IntakePivotSetPositionCommand(intakePivotSubsystem, intakePosition)
633
- // ).unless(intakeRollerSubsystem::getRockwellSensorValue).andThen(
634
- // new IntakePivotSetPositionCommand(intakePivotSubsystem, 0)
635
- // )
636
- // );
629
+ yButton .onTrue (
630
+ new IntakePivotSetPositionCommand (intakePivotSubsystem , 1 ).andThen (
631
+ new IntakeRollerIntakeCommand (intakeRollerSubsystem , lightBarSubsystem ),
632
+ new IntakePivotSetPositionCommand (intakePivotSubsystem , intakePosition )
633
+ ).unless (intakeRollerSubsystem ::getRockwellSensorValue ).andThen (
634
+ new IntakePivotSetPositionCommand (intakePivotSubsystem , 0 )
635
+ )
636
+ );
637
+
637
638
638
- // shooterFlywheelSubsystem.setDefaultCommand(new InstantCommand(() -> {
639
- // if (yButton.getAsBoolean()) {
640
- // lightBarSubsystem.setLightBarStatus(LightBarStatus.SHOOTER_SPIN_UP, 2);
641
- // // shooterFlywheelSubsystem.setShooterMotorSpeed(shooterSpeed); // for tuning
639
+ shooterFlywheelSubsystem .setDefaultCommand (new InstantCommand (() -> {
640
+ if (yButton .getAsBoolean ()) {
641
+ lightBarSubsystem .setLightBarStatus (LightBarStatus .SHOOTER_SPIN_UP , 2 );
642
+ // shooterFlywheelSubsystem.setShooterMotorSpeed(shooterSpeed); // for tuning
642
643
643
- // shooterFlywheelSubsystem.setShooterMotorSpeed();
644
+ shooterFlywheelSubsystem .setShooterMotorSpeed ();
644
645
645
- // shooterPivotSubsystem.setAutoAimBoolean(true);
646
- // if (shooterFlywheelSubsystem.atSpeed()) {
647
- // mechController.setRumble(RumbleType.kBothRumble, .4);
648
- // } else {
649
- // mechController.setRumble(RumbleType.kBothRumble, 0);
650
- // if (lightBarSubsystem.getLightBarMechStatus() == LightBarStatus.SHOOTER_SPIN_UP) {
651
- // double top = shooterFlywheelSubsystem.getTopSpeed()
652
- // / shooterFlywheelSubsystem.getTargetTopRPS();
653
- // double bottom = shooterFlywheelSubsystem.getBottomSpeed()
654
- // / shooterFlywheelSubsystem.getTargetBottomRPS();
655
- // double avg = (top + bottom) / 2; // in case they're different, this just shows the average.
656
-
657
- // lightBarSubsystem.updateShooterSpeedPercentage(avg);
658
- // }
646
+ shooterPivotSubsystem .setAutoAimBoolean (true );
647
+ if (shooterFlywheelSubsystem .atSpeed ()) {
648
+ mechController .setRumble (RumbleType .kBothRumble , .4 );
649
+ } else {
650
+ mechController .setRumble (RumbleType .kBothRumble , 0 );
651
+ if (lightBarSubsystem .getLightBarMechStatus () == LightBarStatus .SHOOTER_SPIN_UP ) {
652
+ double top = shooterFlywheelSubsystem .getTopSpeed ()
653
+ / shooterFlywheelSubsystem .getTargetTopRPS ();
654
+ double bottom = shooterFlywheelSubsystem .getBottomSpeed ()
655
+ / shooterFlywheelSubsystem .getTargetBottomRPS ();
656
+ double avg = (top + bottom ) / 2 ; // in case they're different, this just shows the average.
657
+
658
+ lightBarSubsystem .updateShooterSpeedPercentage (avg );
659
+ }
659
660
660
- // }
661
- // } else {
662
- // // if(fmsSubsystem.getMatchStatus() != MatchStatus.AUTON){
663
- // // shooterPivotSubsystem.setAutoAimBoolean(false);
664
- // // }
665
- // if (mechController.getPOV() == 90) {
666
-
667
- // if (shooterFlywheelSubsystem.atSpeed()) {
668
- // mechController.setRumble(RumbleType.kBothRumble, .4);
669
- // } else {
670
- // mechController.setRumble(RumbleType.kBothRumble, 0);
671
- // }
672
- // } else {
673
- // shooterFlywheelSubsystem.stopShooter();
674
- // mechController.setRumble(RumbleType.kBothRumble, 0);
675
- // }
676
- // }
677
- // if (shooterFlywheelSubsystem.atSpeed()) {
678
- // mechController.setRumble(RumbleType.kBothRumble, .4);
679
- // } else {
680
- // mechController.setRumble(RumbleType.kBothRumble, 0);
681
- // }
682
-
683
- // if (noteInBack
684
- // && !intakeRollerSubsystem.getRockwellSensorValue()
685
- // && intakeRollerSubsystem.getIntegrationSpeed() > 0
686
- // ) {
687
- // System.out.println("Dist: " + GRTUtil.twoDecimals(shooterFlywheelSubsystem.getShootingDistance())
688
- // + " Angle: " + GRTUtil.twoDecimals(Units.radiansToDegrees(shooterPivotSubsystem.getPosition()))
689
- // + " Speed: " + GRTUtil.twoDecimals(shooterFlywheelSubsystem.getSplineSpeed()));
690
- // }
691
- // noteInBack = intakeRollerSubsystem.getRockwellSensorValue();
692
-
693
- // // if we are at speed, rumble the mech controller
661
+ }
662
+ } else {
663
+ // if(fmsSubsystem.getMatchStatus() != MatchStatus.AUTON){
664
+ // shooterPivotSubsystem.setAutoAimBoolean(false);
665
+ // }
666
+ if (mechController .getPOV () == 90 ) {
667
+
668
+ if (shooterFlywheelSubsystem .atSpeed ()) {
669
+ mechController .setRumble (RumbleType .kBothRumble , .4 );
670
+ } else {
671
+ mechController .setRumble (RumbleType .kBothRumble , 0 );
672
+ }
673
+ } else {
674
+ shooterFlywheelSubsystem .stopShooter ();
675
+ mechController .setRumble (RumbleType .kBothRumble , 0 );
676
+ }
677
+ }
678
+ if (shooterFlywheelSubsystem .atSpeed ()) {
679
+ mechController .setRumble (RumbleType .kBothRumble , .4 );
680
+ } else {
681
+ mechController .setRumble (RumbleType .kBothRumble , 0 );
682
+ }
683
+
684
+ if (noteInBack
685
+ && !intakeRollerSubsystem .getRockwellSensorValue ()
686
+ && intakeRollerSubsystem .getIntegrationSpeed () > 0
687
+ ) {
688
+ System .out .println ("Dist: " + GRTUtil .twoDecimals (shooterFlywheelSubsystem .getShootingDistance ())
689
+ + " Angle: " + GRTUtil .twoDecimals (Units .radiansToDegrees (shooterPivotSubsystem .getPosition ()))
690
+ + " Speed: " + GRTUtil .twoDecimals (shooterFlywheelSubsystem .getSplineSpeed ()));
691
+ }
692
+ noteInBack = intakeRollerSubsystem .getRockwellSensorValue ();
693
+
694
+ // if we are at speed, rumble the mech controller
694
695
695
- // }, shooterFlywheelSubsystem
696
- // ));
696
+ }, shooterFlywheelSubsystem
697
+ ));
697
698
698
699
// dPadRight.onTrue(new IntakePivotSetPositionCommand(intakePivotSubsystem, 1).andThen(
699
700
// new IntakeRollerIntakeCommand(intakeRollerSubsystem, lightBarSubsystem),
@@ -712,12 +713,12 @@ private void configureBindings() {
712
713
// // }, intakePivotSubsystem));
713
714
714
715
// // The triggers intake/outtake the rollers
715
- // intakeRollerSubsystem.setDefaultCommand(new InstantCommand(() -> {
716
+ intakeRollerSubsystem .setDefaultCommand (new InstantCommand (() -> {
716
717
717
- // double power = .7 * (mechController.getRightTriggerAxis() - mechController.getLeftTriggerAxis());
718
+ double power = .7 * (mechController .getRightTriggerAxis () - mechController .getLeftTriggerAxis ());
718
719
719
- // intakeRollerSubsystem.setRollSpeeds(power, power);
720
- // }, intakeRollerSubsystem));
720
+ intakeRollerSubsystem .setRollSpeeds (power , power );
721
+ }, intakeRollerSubsystem )); }
721
722
722
723
// // Offset buttons to correct the shooter if needed
723
724
// offsetUpButton.onTrue(new InstantCommand(
0 commit comments