10
10
11
11
import java .util .EnumSet ;
12
12
13
- import com .choreo .lib .ChoreoTrajectory ;
14
13
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 ;
20
14
21
15
import edu .wpi .first .cscore .MjpegServer ;
22
16
import edu .wpi .first .cscore .UsbCamera ;
@@ -151,8 +145,6 @@ public class RobotContainer {
151
145
private UsbCamera driverCamera ;
152
146
private MjpegServer driverCameraServer ;
153
147
154
- ChoreoTrajectory trajectory ;
155
-
156
148
private PIDController xPID ;
157
149
private PIDController yPID ;
158
150
@@ -184,14 +176,6 @@ public RobotContainer() {
184
176
autonTable = ntInstance .getTable ("Auton" );
185
177
listEntry = autonTable .getEntry ("AutonList" );
186
178
selectedAutonEntry = autonTable .getEntry ("Selected" );
187
- // System.out.print("Available Autons: " + AutoBuilder.getAllAutoNames().toArray(new String[0]));
188
- listEntry .setStringArray (AutoBuilder .getAllAutoNames ().toArray (new String [0 ]));
189
- selectedAutonEntry .setString (autonValue );
190
- autonTable .addListener ("Auton" , EnumSet .of (NetworkTableEvent .Kind .kValueAll ), (table , key , event ) -> {
191
- this .autonValue = event .valueData .value .getString ();
192
- this .selectedAutonEntry .setString (this .autonValue );
193
- System .out .print ("New auton value: " + this .autonValue );
194
- });
195
179
fmsSubsystem = new FieldManagementSubsystem ();
196
180
lightBarSubsystem = new LightBarSubsystem ();
197
181
superstructureSubsystem = new SuperstructureSubsystem (lightBarSubsystem , fmsSubsystem );
@@ -271,46 +255,6 @@ public RobotContainer() {
271
255
272
256
swerveCrauton .add (autonPathChooser );
273
257
274
- NamedCommands .registerCommand ("Shoot" , new ParallelDeadlineGroup (
275
- new SequentialCommandGroup (
276
- new WaitCommand (.05 ),
277
- new ConditionalWaitCommand (swerveSubsystem ::atTargetAngle ).withTimeout (1 ),
278
- new IntakeRollerFeedCommand (intakeRollerSubsystem , 1 ).until (intakeRollerSubsystem ::getRockwellSensorValue ),
279
- new IntakeRollerFeedCommand (intakeRollerSubsystem , 1 ).until (() ->
280
- !intakeRollerSubsystem .getRockwellSensorValue ()
281
- &&
282
- !intakeRollerSubsystem .getAmpSensor ()
283
- &&
284
- !intakeRollerSubsystem .getFrontSensorValue ()),
285
- new IntakeRollerFeedCommand (intakeRollerSubsystem , 1 ).withTimeout (.3 )
286
- ),
287
- new SwerveAimCommand (swerveSubsystem )
288
- ));
289
-
290
- NamedCommands .registerCommand ("StationaryShoot" , new ParallelDeadlineGroup (
291
- new SequentialCommandGroup (
292
- new ConditionalWaitCommand (shooterFlywheelSubsystem ::atSpeed ),
293
- new ConditionalWaitCommand (swerveSubsystem ::atTargetAngle ),
294
- new IntakeRollerFeedCommand (intakeRollerSubsystem , 1 ).until (() -> !intakeRollerSubsystem .getRockwellSensorValue ()),
295
- new IntakeRollerFeedCommand (intakeRollerSubsystem , 1 ).withTimeout (.3 )
296
- ),
297
- new SwerveAimCommand (swerveSubsystem )
298
- ));
299
-
300
- NamedCommands .registerCommand (
301
- "Intake" ,
302
- new IntakeRollerAmpIntakeCommand (intakeRollerSubsystem ).andThen (new IntakeRollerIntakeCommand (intakeRollerSubsystem , lightBarSubsystem ))
303
- );
304
-
305
- NamedCommands .registerCommand ("AutoIntake" ,
306
- new NoteAlignCommand (swerveSubsystem , noteDetector , driveController ).until (intakeRollerSubsystem ::getAmpSensor ).alongWith (
307
- new IntakeRollerAmpIntakeCommand (intakeRollerSubsystem )
308
- )
309
- );
310
- NamedCommands .registerCommand ("Feed" ,
311
- new IntakeRollerFeedCommand (intakeRollerSubsystem ).until (intakeRollerSubsystem ::getRockwellSensorValue )
312
- );
313
-
314
258
swerveSubsystem .targetSpeaker ();
315
259
316
260
configureBindings ();
@@ -349,169 +293,9 @@ private void configureBindings() {
349
293
swerveSubsystem .resetDriverHeading ();
350
294
}));
351
295
352
- /* SWERVE BINDINGS */
353
-
354
- /* Shooter Aim -- Holding down the button will change the shooter's pitch to aim it at the speaker. */
355
- // drive
356
-
357
- /* Automatic Amping -- Pressing and holding the button will cause the robot to automatically path find to the
358
- * amp and deposit its note. Releasing the button will stop the robot (and the path finding). */
359
- // TODO: Bring back LED integration.
360
- driveController .getAutoAmp ().whileTrue (
361
- Commands .deferredProxy (() -> new AutoAmpSequence (
362
- fmsSubsystem ,
363
- swerveSubsystem ,
364
- elevatorSubsystem ,
365
- intakePivotSubsystem ,
366
- intakeRollerSubsystem
367
- ))
368
- );
369
-
370
- // DEPRECATED AMP ALIGN
371
- // driveController.getAmpAlign().onTrue(new InstantCommand(
372
- // () -> lightBarSubsystem.setLightBarStatus(LightBarStatus.AUTO_ALIGN, 1)
373
- // ).andThen(new ParallelRaceGroup(
374
- // AlignCommand.getAmpAlignCommand(swerveSubsystem, fmsSubsystem.isRedAlliance()),
375
- // new ConditionalWaitCommand(
376
- // () -> !driveController.getAmpAlign().getAsBoolean()))
377
- // ).andThen(
378
- // new InstantCommand(() -> lightBarSubsystem.setLightBarStatus(LightBarStatus.DORMANT, 1))
379
- // )
380
- // );
381
-
382
- /* Stage Align -- Pressing and holding the button will cause the robot to automatically pathfind such that its
383
- * climb hooks will end up directly above the center of the nearest chain. */
384
- driveController .getStageAlignButton ().whileTrue (
385
- Commands .deferredProxy (() -> AlignCommand .getStageAlignCommand (
386
- swerveSubsystem ,
387
- swerveSubsystem ::getRobotPosition ,
388
- fmsSubsystem ::isRedAlliance
389
- ))
390
- );
391
-
392
-
393
- /* Note align -- Auto-intakes the nearest visible note, leaving left power control to the driver. */
394
- driveController .getNoteAlign ().onTrue (
395
- new AutoIntakeSequence (intakeRollerSubsystem , intakePivotSubsystem ,
396
- swerveSubsystem , noteDetector ,
397
- driveController , lightBarSubsystem )
398
- .andThen (new IntakePivotSetPositionCommand (intakePivotSubsystem , 0 ))
399
- .onlyWhile (driveController .getNoteAlign ())
400
- );
401
-
402
296
/* Swerve Stop -- Pressing the button completely stops the robot's motion. */
403
297
driveController .getSwerveStop ().onTrue (new SwerveStopCommand (swerveSubsystem ));
404
298
405
- driveController .getShooterAimButton ().onTrue (Commands .runOnce (
406
- () -> {
407
- swerveSubsystem .setTargetPoint (fmsSubsystem .isRedAlliance () ? SwerveConstants .RED_SPEAKER_POS : SwerveConstants .BLUE_SPEAKER_POS );
408
- swerveSubsystem .setAim (true );
409
- }
410
- ));
411
-
412
- driveController .getShooterAimButton ().onFalse (Commands .runOnce (
413
- () -> {
414
- swerveSubsystem .setAim (false );
415
- }
416
- ));
417
-
418
-
419
-
420
- /* SHOOTER PIVOT TEST */
421
-
422
- // rightBumper.onTrue(new ShooterPivotSetAngleCommand(shooterPivotSubsystem,
423
- // Units.degreesToRadians(18)));
424
-
425
- // leftBumper.onTrue(new ShooterPivotSetAngleCommand(shooterPivotSubsystem,
426
- // Units.degreesToRadians(60)));
427
-
428
- /* SHOOTER PIVOT TUNE */
429
-
430
- shooterPivotSubsystem .setDefaultCommand (new InstantCommand (() -> {
431
- //TODO: switch to enum for dpad angles
432
-
433
- switch (mechController .getPOV ()) {
434
- case 0 :
435
- shooterPivotSetPosition += .003 ;
436
- break ;
437
-
438
- case 180 :
439
- shooterPivotSetPosition -= .003 ;
440
- break ;
441
-
442
- case 90 :
443
- shooterSpeed += .001 ;
444
- break ;
445
-
446
- case 270 :
447
- shooterSpeed -= .001 ;
448
- break ;
449
-
450
- default :
451
- break ;
452
- }
453
-
454
- // System.out.println(intakePosition);
455
-
456
-
457
-
458
- // System.out.print(" Speed: " + GRTUtil.twoDecimals(shooterSpeed)
459
- // );
460
-
461
- // shooterPivotSubsystem.getAutoAimAngle();
462
- // shooterPivotSubsystem.setAngle(shooterPivotSetPosition);
463
- }, shooterPivotSubsystem ));
464
-
465
- /* ElEVATOR TEST */
466
-
467
- dPadUp .onTrue (new ElevatorToTrapCommand (elevatorSubsystem ));
468
- dPadUp .onTrue (new InstantCommand (() -> intakePivotSubsystem .enablePowerLimit (false )));
469
- dPadDown .onTrue (new ElevatorToZeroCommand (elevatorSubsystem ));
470
- dPadDown .onTrue (new InstantCommand (() -> intakePivotSubsystem .enablePowerLimit (true )));
471
-
472
-
473
- dPadLeft .onTrue (new ElevatorToMidCommand (elevatorSubsystem ));
474
- dPadLeft .onFalse (new ElevatorToZeroCommand (elevatorSubsystem ));
475
- // elevatorSubsystem.setManual();
476
-
477
- // elevatorSubsystem.setDefaultCommand(new InstantCommand(() -> {
478
- // if (mechController.getPOV() == 0) {
479
- // elevatorSubsystem.setTargetState(ElevatorState.TRAP);
480
- // // elevatorSubsystem.setMotorPower(0.1);
481
- // } else if (mechController.getPOV() == 180) {
482
- // elevatorSubsystem.setTargetState(ElevatorState.ZERO);
483
- // // elevatorSubsystem.setMotorPower(-0.1);
484
- // }
485
- // else{
486
- // // elevatorSubsystem.setMotorPower(0);
487
- // }
488
- // }, elevatorSubsystem));
489
-
490
- elevatorToZero .onTrue (new ElevatorToLimitSwitchCommand (elevatorSubsystem ));
491
- /* INTAKE TEST */
492
-
493
- // xButton.onTrue(new InstantCommand(() -> intakePivotSubsystem.setPosition(.3),
494
- // intakePivotSubsystem));
495
-
496
- // rightBumper.onTrue(new InstantCommand(() ->
497
- // intakePivotSubsystem.setPosition(0), intakePivotSubsystem));
498
-
499
- // leftBumper.onTrue(new InstantCommand(() ->
500
- // intakePivotSubsystem.setPosition(.85), intakePivotSubsystem));
501
-
502
- /* MECHANISM BINDINGS */
503
-
504
- /* Climb Controls -- Left and right joystick up/down controls left and right arm up/down, respectively. Pressing
505
- * down on either the left or right joystick toggles automatic lowering (on by default), which brings down both
506
- * arms automatically when there is no joystick input. */
507
- climbSubsystem .setDefaultCommand (Commands .run (
508
- () -> climbSubsystem .setSpeeds (-mechController .getLeftY (), -mechController .getRightY ()), climbSubsystem
509
- ));
510
-
511
- leftStickButton .onTrue (Commands .runOnce (() -> climbSubsystem .toggleAutomaticLowering (), climbSubsystem ));
512
- rightStickButton .onTrue (Commands .runOnce (() -> climbSubsystem .toggleAutomaticLowering (), climbSubsystem ));
513
-
514
-
515
299
// rightBumper toggles the amp sequence
516
300
// if the elevator is up, lower it and stow the intake
517
301
// if the elevator is down, run the amp sequence
@@ -536,50 +320,6 @@ private void configureBindings() {
536
320
)
537
321
);
538
322
539
- // leftBumper toggles the trap position for the elevator
540
- leftBumper .onTrue (
541
- new ConditionalCommand (
542
- new ElevatorToZeroCommand (elevatorSubsystem ).alongWith (new InstantCommand (// lower the elevator
543
- () -> {
544
- intakePivotSubsystem .enablePowerLimit (true );
545
- intakePivotSubsystem .setPosition (0 );
546
- }, intakePivotSubsystem )), // stow intake
547
- new ConditionalCommand (
548
- new ElevatorToTrapCommand (elevatorSubsystem ).andThen (
549
- new InstantCommand (() -> {
550
- intakePivotSubsystem .enablePowerLimit (false );
551
- intakePivotSubsystem .setPosition (.45 );
552
- })
553
- ),
554
- new IntakePivotSetPositionCommand (intakePivotSubsystem , 1 ).andThen (// extend pivot
555
- new ConditionalWaitCommand (intakeRollerSubsystem ::getFrontSensorReached ),
556
- new InstantCommand (() -> intakePivotSubsystem .setPosition (0 ))
557
- ),
558
- intakeRollerSubsystem ::getAmpSensor
559
- ), // raise the elevator
560
- () -> (elevatorSubsystem .getTargetState () == ElevatorState .AMP // check if targeting a high pos
561
- || elevatorSubsystem .getTargetState () == ElevatorState .TRAP ) && elevatorSubsystem .getExtensionPercent () > .5 )
562
- );
563
-
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
- );
581
-
582
-
583
323
584
324
// aButton runs the intake sequence
585
325
aButton .onTrue (
@@ -719,34 +459,5 @@ private void configureBindings() {
719
459
intakeRollerSubsystem .setRollSpeeds (power , power );
720
460
}, intakeRollerSubsystem ));
721
461
722
- // Offset buttons to correct the shooter if needed
723
- offsetUpButton .onTrue (new InstantCommand (
724
- () -> shooterPivotSubsystem .setAngleOffset (Units .degreesToRadians (3 )))
725
- );
726
- offsetUpButton .onFalse (new InstantCommand (
727
- () -> shooterPivotSubsystem .setAngleOffset (Units .degreesToRadians (0 )))
728
- );
729
-
730
- offsetDownButton .onTrue (new InstantCommand (
731
- () -> shooterPivotSubsystem .setAngleOffset (Units .degreesToRadians (-3 )))
732
- );
733
- offsetDownButton .onFalse (new InstantCommand (
734
- () -> shooterPivotSubsystem .setAngleOffset (Units .degreesToRadians (0 )))
735
- );
736
-
737
- }
738
-
739
- /**
740
- * Returns the autonomous command.
741
- *
742
- * @return The selected autonomous command.
743
- */
744
- public Command getAutonomousCommand () {
745
- return AutoBuilder .buildAuto (autonValue ).alongWith (
746
- new ShooterFlywheelReadyCommand (shooterFlywheelSubsystem , lightBarSubsystem ),
747
- new InstantCommand (() -> shooterPivotSubsystem .setAutoAimBoolean (true )),
748
- new IntakePivotSetPositionCommand (intakePivotSubsystem , 1 ),
749
- new ClimbLowerCommand (climbSubsystem )
750
- );
751
462
}
752
463
}
0 commit comments