6565import frc .robot .subsystems .drive .commands .DriveCommandFactory ;
6666import frc .robot .subsystems .drive .commands .DriveWeightCommand ;
6767import frc .robot .subsystems .drive .weights .AntiTipWeight ;
68+ import frc .robot .subsystems .drive .weights .DynamicAlignWeight ;
6869import frc .robot .subsystems .drive .weights .FollowPathDirect ;
6970import frc .robot .subsystems .drive .weights .FollowPathNearest ;
7071import frc .robot .subsystems .drive .weights .JoystickDriveWeight ;
72+ import frc .robot .subsystems .drive .weights .LocalTagAlignWeight ;
7173import frc .robot .subsystems .drive .weights .PathplannerWeight ;
7274import frc .robot .subsystems .gantry .GantryIO ;
7375import frc .robot .subsystems .gantry .GantryIOSim ;
@@ -148,6 +150,8 @@ public class RobotContainer {
148150
149151 private FollowPathNearest followPathReef ;
150152 private FollowPathDirect followPathCoral ;
153+ private LocalTagAlignWeight localAlign ;
154+ private DynamicAlignWeight dynamicAlign ;
151155
152156 private final JoystickDriveWeight joystickDriveWeight ;
153157
@@ -167,15 +171,15 @@ public RobotContainer() {
167171 case REAL :
168172 new CoProcessInput (new OrangePILogger ());
169173 gyro = new Gyro (new GyroIONavX ());
170- aprilTagVisions .add (
171- new AprilTagVision (
172- new AprilTagVisionIOPhotonvision (CameraConstants .frontCameraRight ),
173- CameraConstants .frontCameraRight ));
174+ // aprilTagVisions.add(
175+ // new AprilTagVision(
176+ // new AprilTagVisionIOPhotonvision(CameraConstants.frontCameraRight),
177+ // CameraConstants.frontCameraRight));
174178
175- aprilTagVisions .add (
176- new AprilTagVision (
177- new AprilTagVisionIOPhotonvision (CameraConstants .frontCameraLeft ),
178- CameraConstants .frontCameraLeft ));
179+ // aprilTagVisions.add(
180+ // new AprilTagVision(
181+ // new AprilTagVisionIOPhotonvision(CameraConstants.frontCameraLeft),
182+ // CameraConstants.frontCameraLeft));
179183
180184 aprilTagVisions .add (
181185 new AprilTagVision (
@@ -298,20 +302,9 @@ public RobotContainer() {
298302 algaeCommandFactory ,
299303 algaeIntakeSubsystem );
300304 AprilTagVision [] visionArray = aprilTagVisions .toArray (AprilTagVision []::new );
301- generateNamedCommands ();
302305 driveSubsystem = new DriveSubsystem (gyro );
303306 driveCommandFactory = new DriveCommandFactory (driveSubsystem );
304307 robotOdometry = new RobotOdometry (driveSubsystem , gyro , visionArray );
305- dashboard =
306- new Dashboard (
307- driveSubsystem ,
308- liftSubsystem ,
309- gantrySubsystem ,
310- climberSubsystem ,
311- algaeIntakeSubsystem ,
312- coralOuttakeSubsystem ,
313- winchSubsystem ,
314- driveController );
315308 alertsManager = new AlertsManager ();
316309 AlertsManager .addAlert (
317310 () -> RobotController .getBatteryVoltage () < WarningThresholdConstants .minBatteryVoltage ,
@@ -383,6 +376,21 @@ public RobotContainer() {
383376 DriveWeightCommand .create (
384377 driveCommandFactory , () -> liftSubsystem .getMotorPosition () > 0.3 ));
385378
379+ localAlign =
380+ new LocalTagAlignWeight (
381+ () ->
382+ DistanceManager .getNearestPosition (
383+ RobotOdometry .instance .getPose ("Main" ),
384+ AllianceManager .chooseFromAlliance (
385+ FieldConstants .reefPositionsBlue , FieldConstants .reefPositionsRed )),
386+ () -> RobotOdometry .instance .getPose ("Main" ).getRotation (),
387+ driveSubsystem ,
388+ driveCommandFactory ,
389+ gyro ,
390+ visionArray );
391+
392+ dynamicAlign = new DynamicAlignWeight (followPathReef , localAlign );
393+
386394 // winchSubsystem.setDefaultCommand(
387395 // climberCommandFactory.winchApplyVoltageCommand(() -> -operatorController.getLeftY() *
388396 // 4));
@@ -399,7 +407,8 @@ public void periodic() {
399407 Logger .recordOutput ("AlgaeMode" , algaeMode );
400408 Logger .recordOutput ("CoralPreset" , coralPreset );
401409 Logger .recordOutput ("TargetPosAutoalign" , getTarget ());
402- Logger .recordOutput ("AutoAlignDone" , followPathReef .isAutoalignComplete ());
410+ Logger .recordOutput ("AutoAlignDone" , dynamicAlign .isAutoalignComplete ());
411+ Logger .recordOutput ("LocalTagAlign/alignStage" , dynamicAlign .getStage ());
403412 Logger .recordOutput ("LiftDone" , liftSubsystem .isAtPreset (presetActive ));
404413 Logger .recordOutput (
405414 "LiftDoneAuto" , liftSubsystem .isAtPreset (coralPreset .getLift ()));
@@ -423,6 +432,19 @@ public void periodic() {
423432 Logger .recordOutput ("target" , getTarget ());
424433 }
425434 });
435+
436+ generateNamedCommands ();
437+ driveSubsystem .configurePathplanner ();
438+ dashboard =
439+ new Dashboard (
440+ driveSubsystem ,
441+ liftSubsystem ,
442+ gantrySubsystem ,
443+ climberSubsystem ,
444+ algaeIntakeSubsystem ,
445+ coralOuttakeSubsystem ,
446+ winchSubsystem ,
447+ driveController );
426448 }
427449
428450 public int getPoleID () {
@@ -510,14 +532,19 @@ private void configureBindings() {
510532 .getTranslation ()
511533 .getDistance (getTarget ().getTranslation ())
512534 < 1.3
513- && followPathReef .isEnabled ()
535+ && dynamicAlign .isEnabled ()
514536 && Robot .getState () != RobotState .AUTONOMOUS )
515537 .onTrue (setupAutoPlace (() -> coralPreset ));
516538 // coral place routine for autoalign
517539 // new Trigger(() -> coralAutoAlignWeight.isAutoalignComplete())
518540 // .onTrue(new InstantCommand(() -> driveController.setRumble(RumbleType.kRightRumble, 1)));
519- followPathReef .generateTrigger (
520- () -> driveHID .getAButton () && !followPathReef .isAutoalignComplete ());
541+ // followPathReef.generateTrigger(
542+ // () ->
543+ // driveController.a().getAsBoolean()
544+ // && !followPathReef.isAutoalignComplete());
545+ DriveWeightCommand .createWeightTrigger (
546+ dynamicAlign ,
547+ () -> driveController .a ().getAsBoolean () && !dynamicAlign .globalAlignComplete ());
521548 followPathCoral .generateTrigger (
522549 () -> driveHID .getLeftBumperButton () && !followPathCoral .isAutoalignComplete ());
523550
@@ -1021,5 +1048,13 @@ public void generateNamedCommands() {
10211048 NamedCommands .registerCommand ("PlaceTrough" , autoScoringCommandFactory .placeTrough ());
10221049 NamedCommands .registerCommand (
10231050 "logtest" , new InstantCommand (() -> Logger .recordOutput ("logtest" , true )));
1051+
1052+ NamedCommands .registerCommand (
1053+ "LocalAlign" ,
1054+ localAlign
1055+ .getAutoCommand ()
1056+ .deadlineFor (autonAutoPlace (() -> coralPreset ))
1057+ .until (() -> localAlign .isAutoalignComplete () || !localAlign .isReady ()));
1058+ NamedCommands .registerCommand ("WaitForLocal" , new WaitUntilCommand (() -> localAlign .isReady ()));
10241059 }
10251060}
0 commit comments