diff --git a/src/main/java/competition/auto_programs/vision/LeftFourCoralAuto.java b/src/main/java/competition/auto_programs/vision/LeftFourCoralAuto.java index f65c9d3c..0733ad7f 100644 --- a/src/main/java/competition/auto_programs/vision/LeftFourCoralAuto.java +++ b/src/main/java/competition/auto_programs/vision/LeftFourCoralAuto.java @@ -7,6 +7,7 @@ import competition.auto_programs.BaseAutonomousSequentialCommandGroup; import competition.commandgroups.DriveToFaceAndScoreCommandGroupFactory; +import competition.commandgroups.DriveToReefFaceThenAlignCommandGroupFactory; import competition.commandgroups.vision_path.PathDriveToLocationForCoralStationFactory; import competition.subsystems.pose.Landmarks; import competition.subsystems.pose.vision.Paths; @@ -23,6 +24,7 @@ public class LeftFourCoralAuto extends BaseAutonomousSequentialCommandGroup { public LeftFourCoralAuto( AutonomousCommandSelector autoSelector, DriveToFaceAndScoreCommandGroupFactory driveToFaceAndScoreFact, + DriveToReefFaceThenAlignCommandGroupFactory driveToReefFaceThenAlignCommandGroupFactory, Provider pathDriveToLocationAndIntakeUntilCollectedProvider) { super(autoSelector); @@ -68,41 +70,46 @@ public LeftFourCoralAuto( Landmarks.CoralStationSection.MID)); this.addCommands(driveToRightStationAndIntakeSecond); - // Score 3 - var driveAndScoreCloseLeftBranchALevelFour = - driveToFaceAndScoreFact - .create(Landmarks.ReefFace.CLOSE_LEFT, Landmarks.Branch.A, - Landmarks.CoralLevel.FOUR) - .alongWith(getDriveAndScoreStatusMessageCommand( - Landmarks.ReefFace.CLOSE_LEFT, Landmarks.Branch.A, - Landmarks.CoralLevel.FOUR)); - this.addCommands(driveAndScoreCloseLeftBranchALevelFour); - - // Coral Station - var driveToLeftStationAndIntakeSecond = - pathDriveToLocationAndIntakeUntilCollectedProvider.get() - .create(Paths.Side.LEFT, blueCloseLeftBranchAToLeftCoralStation) - .alongWith(getDriveAndIntakeStatusMessageCommand( - Landmarks.CoralStation.LEFT, - Landmarks.CoralStationSection.MID)); - this.addCommands(driveToLeftStationAndIntakeSecond); - - // Score 4 - var driveAndScoreCloseBranchALevelFour = - driveToFaceAndScoreFact - .create(Landmarks.ReefFace.CLOSE, Landmarks.Branch.A, - Landmarks.CoralLevel.FOUR) - .alongWith( - getDriveAndScoreStatusMessageCommand(Landmarks.ReefFace.CLOSE, - Landmarks.Branch.A, Landmarks.CoralLevel.FOUR)); - this.addCommands(driveAndScoreCloseBranchALevelFour); - - // Back to coral station - var homed = pathDriveToLocationAndIntakeUntilCollectedProvider.get() - .create(Paths.Side.LEFT, blueCloseBranchAToLeftCoralStation) - .alongWith(getDriveAndIntakeStatusMessageCommand( - Landmarks.CoralStation.LEFT, - Landmarks.CoralStationSection.MID)); - this.addCommands(homed); + // Go to 3, but not score it + var driveToCloseLeftBranchALevelFour = driveToReefFaceThenAlignCommandGroupFactory.create( + Landmarks.ReefFace.CLOSE_LEFT, Landmarks.Branch.A); + this.addCommands(driveToCloseLeftBranchALevelFour); + +// // Score 3 +// var driveAndScoreCloseLeftBranchALevelFour = +// driveToFaceAndScoreFact +// .create(Landmarks.ReefFace.CLOSE_LEFT, Landmarks.Branch.A, +// Landmarks.CoralLevel.FOUR) +// .alongWith(getDriveAndScoreStatusMessageCommand( +// Landmarks.ReefFace.CLOSE_LEFT, Landmarks.Branch.A, +// Landmarks.CoralLevel.FOUR)); +// this.addCommands(driveAndScoreCloseLeftBranchALevelFour); +// +// // Coral Station +// var driveToLeftStationAndIntakeSecond = +// pathDriveToLocationAndIntakeUntilCollectedProvider.get() +// .create(Paths.Side.LEFT, blueCloseLeftBranchAToLeftCoralStation) +// .alongWith(getDriveAndIntakeStatusMessageCommand( +// Landmarks.CoralStation.LEFT, +// Landmarks.CoralStationSection.MID)); +// this.addCommands(driveToLeftStationAndIntakeSecond); +// +// // Score 4 +// var driveAndScoreCloseBranchALevelFour = +// driveToFaceAndScoreFact +// .create(Landmarks.ReefFace.CLOSE, Landmarks.Branch.A, +// Landmarks.CoralLevel.FOUR) +// .alongWith( +// getDriveAndScoreStatusMessageCommand(Landmarks.ReefFace.CLOSE, +// Landmarks.Branch.A, Landmarks.CoralLevel.FOUR)); +// this.addCommands(driveAndScoreCloseBranchALevelFour); +// +// // Back to coral station +// var homed = pathDriveToLocationAndIntakeUntilCollectedProvider.get() +// .create(Paths.Side.LEFT, blueCloseBranchAToLeftCoralStation) +// .alongWith(getDriveAndIntakeStatusMessageCommand( +// Landmarks.CoralStation.LEFT, +// Landmarks.CoralStationSection.MID)); +// this.addCommands(homed); } } \ No newline at end of file diff --git a/src/main/java/competition/auto_programs/vision/RightFourCoralAuto.java b/src/main/java/competition/auto_programs/vision/RightFourCoralAuto.java index 3cc99a21..d9a59fce 100644 --- a/src/main/java/competition/auto_programs/vision/RightFourCoralAuto.java +++ b/src/main/java/competition/auto_programs/vision/RightFourCoralAuto.java @@ -7,6 +7,7 @@ import competition.auto_programs.BaseAutonomousSequentialCommandGroup; import competition.commandgroups.DriveToFaceAndScoreCommandGroupFactory; +import competition.commandgroups.DriveToReefFaceThenAlignCommandGroupFactory; import competition.commandgroups.vision_path.PathDriveToLocationForCoralStationFactory; import competition.subsystems.pose.Landmarks; import competition.subsystems.pose.vision.Paths; @@ -22,6 +23,7 @@ public class RightFourCoralAuto extends BaseAutonomousSequentialCommandGroup { public RightFourCoralAuto( AutonomousCommandSelector autoSelector, DriveToFaceAndScoreCommandGroupFactory driveToFaceAndScoreFact, + DriveToReefFaceThenAlignCommandGroupFactory driveToReefFaceThenAlignCommandGroupFactory, Provider pathDriveToLocationAndIntakeUntilCollectedProvider) { super(autoSelector); @@ -68,42 +70,46 @@ public RightFourCoralAuto( Landmarks.CoralStationSection.MID)); this.addCommands(driveToRightStationAndIntakeSecond); - // Score 3 - var driveAndScoreCloseLeftBranchALevelFour = - driveToFaceAndScoreFact - .create(Landmarks.ReefFace.CLOSE_RIGHT, Landmarks.Branch.B, - Landmarks.CoralLevel.FOUR) - .alongWith(getDriveAndScoreStatusMessageCommand( - Landmarks.ReefFace.CLOSE_RIGHT, Landmarks.Branch.B, - Landmarks.CoralLevel.FOUR)); - this.addCommands(driveAndScoreCloseLeftBranchALevelFour); - - // Coral Station - var driveToLeftStationAndIntakeSecond = - pathDriveToLocationAndIntakeUntilCollectedProvider.get() - .create(Paths.Side.RIGHT, blueCloseLeftBranchAToLeftCoralStation) - .alongWith(getDriveAndIntakeStatusMessageCommand( - Landmarks.CoralStation.RIGHT, - Landmarks.CoralStationSection.MID)); - this.addCommands(driveToLeftStationAndIntakeSecond); - - // Score 4 - var driveAndScoreCloseBranchALevelFour = - driveToFaceAndScoreFact - .create(Landmarks.ReefFace.CLOSE, Landmarks.Branch.B, - Landmarks.CoralLevel.FOUR) - .alongWith( - getDriveAndScoreStatusMessageCommand(Landmarks.ReefFace.CLOSE, - Landmarks.Branch.B, Landmarks.CoralLevel.FOUR)); - this.addCommands(driveAndScoreCloseBranchALevelFour); - - // Back to coral station - var homed = - pathDriveToLocationAndIntakeUntilCollectedProvider.get() - .create(Paths.Side.RIGHT, blueCloseBranchAToLeftCoralStation) - .alongWith(getDriveAndIntakeStatusMessageCommand( - Landmarks.CoralStation.RIGHT, - Landmarks.CoralStationSection.MID)); - this.addCommands(homed); + // Go to 3, but not score it + var driveToCloseLeftBranchALevelFour = driveToReefFaceThenAlignCommandGroupFactory.create( + Landmarks.ReefFace.CLOSE_RIGHT, Landmarks.Branch.B); + this.addCommands(driveToCloseLeftBranchALevelFour); + +// // Score 3 +// var driveAndScoreCloseLeftBranchALevelFour = +// driveToFaceAndScoreFact +// .create(Landmarks.ReefFace.CLOSE_RIGHT, Landmarks.Branch.B, +// Landmarks.CoralLevel.FOUR) +// .alongWith(getDriveAndScoreStatusMessageCommand( +// Landmarks.ReefFace.CLOSE_RIGHT, Landmarks.Branch.B, +// Landmarks.CoralLevel.FOUR)); +// this.addCommands(driveAndScoreCloseLeftBranchALevelFour); +// +// // Coral Station +// var driveToLeftStationAndIntakeSecond = +// pathDriveToLocationAndIntakeUntilCollectedProvider.get() +// .create(Paths.Side.RIGHT, blueCloseLeftBranchAToLeftCoralStation) +// .alongWith(getDriveAndIntakeStatusMessageCommand( +// Landmarks.CoralStation.RIGHT, +// Landmarks.CoralStationSection.MID)); +// this.addCommands(driveToLeftStationAndIntakeSecond); +// +// // Score 4 +// var driveAndScoreCloseBranchALevelFour = +// driveToFaceAndScoreFact +// .create(Landmarks.ReefFace.CLOSE, Landmarks.Branch.B, +// Landmarks.CoralLevel.FOUR) +// .alongWith( +// getDriveAndScoreStatusMessageCommand(Landmarks.ReefFace.CLOSE, +// Landmarks.Branch.B, Landmarks.CoralLevel.FOUR)); +// this.addCommands(driveAndScoreCloseBranchALevelFour); +// +// // Back to coral station +// var homed = pathDriveToLocationAndIntakeUntilCollectedProvider.get() +// .create(Paths.Side.RIGHT, blueCloseBranchAToLeftCoralStation) +// .alongWith(getDriveAndIntakeStatusMessageCommand( +// Landmarks.CoralStation.RIGHT, +// Landmarks.CoralStationSection.MID)); +// this.addCommands(homed); } } \ No newline at end of file