From 482957ff230b06811b43bc7d74fe99ea874c60eb Mon Sep 17 00:00:00 2001 From: JohnGilb Date: Thu, 4 Apr 2024 23:15:25 -0700 Subject: [PATCH] Make easier to use (#321) --- .../auto_programs/BotCenter4ThenCenter5.java | 43 +++++ .../auto_programs/BotCenter5ThenCenter4.java | 41 +++++ ...SubwooferShotFromBotThenTwoCenterline.java | 169 ------------------ .../commandgroups/GoToGenericMidline.java | 136 ++++++++++++++ .../components/BaseRobotComponent.java | 8 +- .../OperatorCommandMap.java | 20 ++- 6 files changed, 237 insertions(+), 180 deletions(-) create mode 100644 src/main/java/competition/auto_programs/BotCenter4ThenCenter5.java create mode 100644 src/main/java/competition/auto_programs/BotCenter5ThenCenter4.java delete mode 100644 src/main/java/competition/auto_programs/SubwooferShotFromBotThenTwoCenterline.java create mode 100644 src/main/java/competition/commandgroups/GoToGenericMidline.java diff --git a/src/main/java/competition/auto_programs/BotCenter4ThenCenter5.java b/src/main/java/competition/auto_programs/BotCenter4ThenCenter5.java new file mode 100644 index 00000000..ab52cb87 --- /dev/null +++ b/src/main/java/competition/auto_programs/BotCenter4ThenCenter5.java @@ -0,0 +1,43 @@ +package competition.auto_programs; + +import competition.commandgroups.CollectSequenceCommandGroup; +import competition.commandgroups.FireFromSubwooferCommandGroup; +import competition.commandgroups.GoToGenericMidline; +import competition.subsystems.collector.commands.IntakeCollectorCommand; +import competition.subsystems.drive.DriveSubsystem; +import competition.subsystems.drive.commands.DriveToGivenNoteWithBearingVisionCommand; +import competition.subsystems.drive.commands.DriveToListOfPointsCommand; +import competition.subsystems.pose.PoseSubsystem; +import xbot.common.subsystems.autonomous.AutonomousCommandSelector; + +import javax.inject.Inject; +import javax.inject.Provider; + +//three note centerline auto, centerline 5 then 4, fires from subwoofer for now +//when we have the time to tune a ranged shot will update to that +public class BotCenter4ThenCenter5 extends GoToGenericMidline { + + @Inject + public BotCenter4ThenCenter5(AutonomousCommandSelector autoSelector, PoseSubsystem pose, + DriveSubsystem drive, + Provider fireFromSubwooferCommandGroupProvider, + Provider driveToNoteProvider, + Provider collectSequenceCommandGroupProvider, + Provider driveToListOfPointsCommandProvider, + Provider intakeCollectorCommandProvider + ) { + super( + drive, + pose, + autoSelector, + fireFromSubwooferCommandGroupProvider, + driveToNoteProvider, + collectSequenceCommandGroupProvider, + driveToListOfPointsCommandProvider, + intakeCollectorCommandProvider); + + setupStart(); + collectCenterNoteReturnAndFire(PoseSubsystem.CenterLine4); + collectCenterNoteReturnAndFire(PoseSubsystem.CenterLine5); + } +} diff --git a/src/main/java/competition/auto_programs/BotCenter5ThenCenter4.java b/src/main/java/competition/auto_programs/BotCenter5ThenCenter4.java new file mode 100644 index 00000000..3f29181c --- /dev/null +++ b/src/main/java/competition/auto_programs/BotCenter5ThenCenter4.java @@ -0,0 +1,41 @@ +package competition.auto_programs; + +import competition.commandgroups.CollectSequenceCommandGroup; +import competition.commandgroups.FireFromSubwooferCommandGroup; +import competition.commandgroups.GoToGenericMidline; +import competition.subsystems.collector.commands.IntakeCollectorCommand; +import competition.subsystems.drive.DriveSubsystem; +import competition.subsystems.drive.commands.DriveToGivenNoteWithBearingVisionCommand; +import competition.subsystems.drive.commands.DriveToListOfPointsCommand; +import competition.subsystems.pose.PoseSubsystem; +import xbot.common.subsystems.autonomous.AutonomousCommandSelector; + +import javax.inject.Inject; +import javax.inject.Provider; + +public class BotCenter5ThenCenter4 extends GoToGenericMidline { + + @Inject + public BotCenter5ThenCenter4(AutonomousCommandSelector autoSelector, PoseSubsystem pose, + DriveSubsystem drive, + Provider fireFromSubwooferCommandGroupProvider, + Provider driveToNoteProvider, + Provider collectSequenceCommandGroupProvider, + Provider driveToListOfPointsCommandProvider, + Provider intakeCollectorCommandProvider + ) { + super( + drive, + pose, + autoSelector, + fireFromSubwooferCommandGroupProvider, + driveToNoteProvider, + collectSequenceCommandGroupProvider, + driveToListOfPointsCommandProvider, + intakeCollectorCommandProvider); + + setupStart(); + collectCenterNoteReturnAndFire(PoseSubsystem.CenterLine5); + collectCenterNoteReturnAndFire(PoseSubsystem.CenterLine4); + } +} diff --git a/src/main/java/competition/auto_programs/SubwooferShotFromBotThenTwoCenterline.java b/src/main/java/competition/auto_programs/SubwooferShotFromBotThenTwoCenterline.java deleted file mode 100644 index 04687b54..00000000 --- a/src/main/java/competition/auto_programs/SubwooferShotFromBotThenTwoCenterline.java +++ /dev/null @@ -1,169 +0,0 @@ -package competition.auto_programs; - -import competition.commandgroups.CollectSequenceCommandGroup; -import competition.commandgroups.DriveToGivenNoteAndCollectCommandGroup; -import competition.commandgroups.DriveToGivenNoteWithVisionCommand; -import competition.commandgroups.FireFromSubwooferCommandGroup; -import competition.subsystems.collector.commands.IntakeCollectorCommand; -import competition.subsystems.drive.DriveSubsystem; -import competition.subsystems.drive.commands.DriveToBottomSubwooferCommand; -import competition.subsystems.drive.commands.DriveToGivenNoteWithBearingVisionCommand; -import competition.subsystems.drive.commands.DriveToListOfPointsCommand; -import competition.subsystems.pose.PoseSubsystem; -import competition.subsystems.vision.VisionRange; -import edu.wpi.first.math.geometry.Rotation2d; -import edu.wpi.first.math.geometry.Translation2d; -import edu.wpi.first.wpilibj2.command.Commands; -import edu.wpi.first.wpilibj2.command.InstantCommand; -import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; -import xbot.common.subsystems.autonomous.AutonomousCommandSelector; -import xbot.common.subsystems.drive.swerve.SwerveDriveSubsystem; -import xbot.common.trajectory.XbotSwervePoint; - -import javax.inject.Inject; -import javax.inject.Provider; -import java.util.ArrayList; - -//three note centerline auto, centerline 5 then 4, fires from subwoofer for now -//when we have the time to tune a ranged shot will update to that -public class SubwooferShotFromBotThenTwoCenterline extends SequentialCommandGroup { - AutonomousCommandSelector autoSelector; - double centerlineTimeout = 999; - - double meterThreshold = 0.3048; - double velocityThreshold = 0.05; - final PoseSubsystem pose; - final DriveSubsystem drive; - - @Inject - public SubwooferShotFromBotThenTwoCenterline(AutonomousCommandSelector autoSelector, PoseSubsystem pose, - DriveSubsystem drive, - Provider fireFromSubwooferCommandGroupProvider, - Provider driveToNoteProvider, - Provider collectSequenceCommandGroupProvider, - Provider driveToListOfPointsCommandProvider, - Provider intakeCollectorCommandProvider - ){ - this.pose = pose; - this.drive = drive; - - this.autoSelector = autoSelector; - - var limitCurrentCommand = drive.createChangeDriveCurrentLimitsCommand(SwerveDriveSubsystem.CurrentLimitMode.Auto); - this.addCommands(limitCurrentCommand); - - var startInFrontOfSpeaker = pose.createSetPositionCommand( - () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.BlueSubwooferBottomScoringLocation)); - this.addCommands(startInFrontOfSpeaker); - - // Fire preload note into the speaker from starting position - queueMessageToAutoSelector("Shoot pre-loaded note from subwoofer (middle)"); - var fireFirstNoteCommand = fireFromSubwooferCommandGroupProvider.get(); - this.addCommands(fireFirstNoteCommand); - - // Drive to centerline5 note and collect - queueMessageToAutoSelector("Drive to bottom spike note, collect, drive back to sub(middle) and shoot"); - this.addCommands( - new InstantCommand(() -> { - drive.setTargetNote(PoseSubsystem.CenterLine4); - }) - ); - var driveToCenterline4 = driveToNoteProvider.get(); - this.addCommands( - new InstantCommand(()->{ - driveToCenterline4.setWaypoints(new Translation2d( - PoseSubsystem.BlueSpikeBottom.getX() + 2.06, - PoseSubsystem.BlueSpikeBottom.getY() - 2.3951 - )); - }) - ); - driveToCenterline4.logic.setEnableConstantVelocity(true); - driveToCenterline4.setMaximumSpeedOverride(drive.getSuggestedAutonomousExtremeSpeed()); - driveToCenterline4.setVisionRangeOverride(VisionRange.Far); - - var collect1 = collectSequenceCommandGroupProvider.get(); - //swap collect and drive for testing - this.addCommands(Commands.deadline(collect1,driveToCenterline4).withTimeout(centerlineTimeout)); - - addCommands(drive.createChangeDriveCurrentLimitsCommand(SwerveDriveSubsystem.CurrentLimitMode.Teleop)); - - var driveBackToBottomSubwooferFirst = driveToListOfPointsCommandProvider.get(); - driveBackToBottomSubwooferFirst.addPointsSupplier(this::goBackToBotSubwoofer); - driveBackToBottomSubwooferFirst.setMaximumSpeedOverride(drive.getSuggestedAutonomousExtremeSpeed()); - driveBackToBottomSubwooferFirst.setAlternativeIsFinishedSupplier(this::alternativeIsFinishedForSubwoofer); - - var collect3 = intakeCollectorCommandProvider.get(); - this.addCommands(Commands.deadline(driveBackToBottomSubwooferFirst.withTimeout(centerlineTimeout),collect3)); - - // Fire second note into the speaker - var fireSecondNoteCommand = fireFromSubwooferCommandGroupProvider.get(); - this.addCommands(fireSecondNoteCommand); - - this.addCommands( - new InstantCommand(() -> { - drive.setTargetNote(PoseSubsystem.CenterLine5); - }) - ); - var driveToCenterline5 = driveToNoteProvider.get(); - this.addCommands( - new InstantCommand(()->{ - driveToCenterline5.setWaypoints(new Translation2d( - PoseSubsystem.BlueSpikeBottom.getX() + 2.06, - PoseSubsystem.BlueSpikeBottom.getY() - 2.3951 - )); - }) - ); - - addCommands(drive.createChangeDriveCurrentLimitsCommand(SwerveDriveSubsystem.CurrentLimitMode.Auto)); - - driveToCenterline5.logic.setEnableConstantVelocity(true); - driveToCenterline5.setMaximumSpeedOverride(drive.getSuggestedAutonomousExtremeSpeed()); - driveToCenterline5.setVisionRangeOverride(VisionRange.Far); - - var collect2 = collectSequenceCommandGroupProvider.get(); - //swap collect and drive for testing - this.addCommands(Commands.deadline(collect2,driveToCenterline5).withTimeout(centerlineTimeout)); - - addCommands(drive.createChangeDriveCurrentLimitsCommand(SwerveDriveSubsystem.CurrentLimitMode.Auto)); - - var driveBackToBottomSubwooferSecond = driveToListOfPointsCommandProvider.get(); - driveBackToBottomSubwooferSecond.addPointsSupplier(this::goBackToBotSubwoofer); - driveBackToBottomSubwooferSecond.setMaximumSpeedOverride(drive.getSuggestedAutonomousExtremeSpeed()); - driveBackToBottomSubwooferSecond.setAlternativeIsFinishedSupplier(this::alternativeIsFinishedForSubwoofer); - - var collect4 = intakeCollectorCommandProvider.get(); - this.addCommands(Commands.deadline(driveBackToBottomSubwooferSecond.withTimeout(centerlineTimeout),collect4)); - - // Fire second note into the speaker - var fireThirdNoteCommand = fireFromSubwooferCommandGroupProvider.get(); - this.addCommands(fireThirdNoteCommand); - - } - private void queueMessageToAutoSelector(String message) { - this.addCommands(autoSelector.createAutonomousStateMessageCommand(message)); - } - private ArrayList goBackToBotSubwoofer(){ - ArrayList points = new ArrayList<>(); - points.add(XbotSwervePoint.createPotentiallyFilppedXbotSwervePoint( - new Translation2d( - PoseSubsystem.BlueSpikeBottom.getX() + 2.06, - PoseSubsystem.BlueSpikeBottom.getY() - 2.3951), Rotation2d.fromDegrees(140),10)); - points.add(XbotSwervePoint.createPotentiallyFilppedXbotSwervePoint(PoseSubsystem.BlueSubwooferBottomScoringLocation,10)); - return points; - } - - private boolean alternativeIsFinishedForSubwoofer() { - double speed = pose.getRobotCurrentSpeed(); - - Translation2d robotLocation = pose.getCurrentPose2d().getTranslation(); - - // Returns finished if both position and velocity are under threshold - boolean nearPositionThreshold = PoseSubsystem.convertBlueToRedIfNeeded( - PoseSubsystem.BlueSubwooferBottomScoringLocation) - .getTranslation().getDistance(robotLocation) < meterThreshold; - - boolean nearVelocityThreshold = speed < velocityThreshold; - - return (nearPositionThreshold && nearVelocityThreshold); - } -} diff --git a/src/main/java/competition/commandgroups/GoToGenericMidline.java b/src/main/java/competition/commandgroups/GoToGenericMidline.java new file mode 100644 index 00000000..d8fef074 --- /dev/null +++ b/src/main/java/competition/commandgroups/GoToGenericMidline.java @@ -0,0 +1,136 @@ +package competition.commandgroups; + +import competition.subsystems.collector.commands.IntakeCollectorCommand; +import competition.subsystems.drive.DriveSubsystem; +import competition.subsystems.drive.commands.DriveToGivenNoteWithBearingVisionCommand; +import competition.subsystems.drive.commands.DriveToListOfPointsCommand; +import competition.subsystems.pose.PoseSubsystem; +import competition.subsystems.vision.VisionRange; +import edu.wpi.first.math.geometry.Pose2d; +import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.math.geometry.Translation2d; +import edu.wpi.first.wpilibj2.command.Commands; +import edu.wpi.first.wpilibj2.command.InstantCommand; +import edu.wpi.first.wpilibj2.command.SequentialCommandGroup; +import xbot.common.subsystems.autonomous.AutonomousCommandSelector; +import xbot.common.subsystems.drive.swerve.SwerveDriveSubsystem; +import xbot.common.trajectory.XbotSwervePoint; + +import javax.inject.Inject; +import javax.inject.Provider; +import java.util.ArrayList; + +public class GoToGenericMidline extends SequentialCommandGroup { + + protected AutonomousCommandSelector autoSelector; + protected double centerlineTimeout = 999; + protected double meterThreshold = 0.3048; + protected double velocityThreshold = 0.05; + protected PoseSubsystem pose; + protected DriveSubsystem drive; + + protected Provider fireFromSubwooferCommandGroupProvider; + protected Provider driveToNoteProvider; + protected Provider collectSequenceCommandGroupProvider; + protected Provider driveToListOfPointsCommandProvider; + protected Provider intakeCollectorCommandProvider; + + + @Inject + public GoToGenericMidline( + DriveSubsystem drive, + PoseSubsystem pose, + AutonomousCommandSelector autoSelector, + Provider fireFromSubwooferCommandGroupProvider, + Provider driveToNoteProvider, + Provider collectSequenceCommandGroupProvider, + Provider driveToListOfPointsCommandProvider, + Provider intakeCollectorCommandProvider) { + this.pose = pose; + this.drive = drive; + this.autoSelector = autoSelector; + + this.fireFromSubwooferCommandGroupProvider = fireFromSubwooferCommandGroupProvider; + this.driveToNoteProvider = driveToNoteProvider; + this.collectSequenceCommandGroupProvider = collectSequenceCommandGroupProvider; + this.driveToListOfPointsCommandProvider = driveToListOfPointsCommandProvider; + this.intakeCollectorCommandProvider = intakeCollectorCommandProvider; + } + + protected void setupStart() { + var startInFrontOfSpeaker = pose.createSetPositionCommand( + () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.BlueSubwooferBottomScoringLocation)); + this.addCommands(startInFrontOfSpeaker); + + // Fire preload note into the speaker from starting position + queueMessageToAutoSelector("Shoot pre-loaded note from subwoofer (middle)"); + var fireFirstNoteCommand = fireFromSubwooferCommandGroupProvider.get(); + this.addCommands(fireFirstNoteCommand); + } + + protected void collectCenterNoteReturnAndFire(Pose2d centerNoteLocation) { + this.addCommands( + new InstantCommand(() -> { + drive.setTargetNote(centerNoteLocation); + }) + ); + var drivetoCenterLine = driveToNoteProvider.get(); + this.addCommands( + new InstantCommand(()->{ + drivetoCenterLine.setWaypoints(new Translation2d( + PoseSubsystem.BlueSpikeBottom.getX() + 2.06, + PoseSubsystem.BlueSpikeBottom.getY() - 2.3951 + )); + }) + ); + drivetoCenterLine.logic.setEnableConstantVelocity(true); + drivetoCenterLine.setMaximumSpeedOverride(drive.getSuggestedAutonomousExtremeSpeed()); + drivetoCenterLine.setVisionRangeOverride(VisionRange.Far); + + var collectSequenceCommand = collectSequenceCommandGroupProvider.get(); + //swap collect and drive for testing + this.addCommands(Commands.deadline(collectSequenceCommand,drivetoCenterLine).withTimeout(centerlineTimeout)); + + addCommands(drive.createChangeDriveCurrentLimitsCommand(SwerveDriveSubsystem.CurrentLimitMode.Teleop)); + + var driveBackToBottomSubwoofer = driveToListOfPointsCommandProvider.get(); + driveBackToBottomSubwoofer.addPointsSupplier(this::goBackToBotSubwoofer); + driveBackToBottomSubwoofer.setMaximumSpeedOverride(drive.getSuggestedAutonomousExtremeSpeed()); + driveBackToBottomSubwoofer.setAlternativeIsFinishedSupplier(this::alternativeIsFinishedForSubwoofer); + + var collectAgain = intakeCollectorCommandProvider.get(); + this.addCommands(Commands.deadline(driveBackToBottomSubwoofer.withTimeout(centerlineTimeout),collectAgain)); + + // Fire second note into the speaker + var fireNoteCommand = fireFromSubwooferCommandGroupProvider.get(); + this.addCommands(fireNoteCommand); + } + + protected void queueMessageToAutoSelector(String message) { + this.addCommands(autoSelector.createAutonomousStateMessageCommand(message)); + } + protected ArrayList goBackToBotSubwoofer(){ + ArrayList points = new ArrayList<>(); + points.add(XbotSwervePoint.createPotentiallyFilppedXbotSwervePoint( + new Translation2d( + PoseSubsystem.BlueSpikeBottom.getX() + 2.06, + PoseSubsystem.BlueSpikeBottom.getY() - 2.3951), Rotation2d.fromDegrees(140),10)); + points.add(XbotSwervePoint.createPotentiallyFilppedXbotSwervePoint(PoseSubsystem.BlueSubwooferBottomScoringLocation,10)); + return points; + } + + protected boolean alternativeIsFinishedForSubwoofer() { + double speed = pose.getRobotCurrentSpeed(); + + Translation2d robotLocation = pose.getCurrentPose2d().getTranslation(); + + // Returns finished if both position and velocity are under threshold + boolean nearPositionThreshold = PoseSubsystem.convertBlueToRedIfNeeded( + PoseSubsystem.BlueSubwooferBottomScoringLocation) + .getTranslation().getDistance(robotLocation) < meterThreshold; + + boolean nearVelocityThreshold = speed < velocityThreshold; + + return (nearPositionThreshold && nearVelocityThreshold); + } +} diff --git a/src/main/java/competition/injection/components/BaseRobotComponent.java b/src/main/java/competition/injection/components/BaseRobotComponent.java index 227bedb5..59d58ab1 100644 --- a/src/main/java/competition/injection/components/BaseRobotComponent.java +++ b/src/main/java/competition/injection/components/BaseRobotComponent.java @@ -1,5 +1,7 @@ package competition.injection.components; +import competition.auto_programs.BotCenter4ThenCenter5; +import competition.auto_programs.BotCenter5ThenCenter4; import competition.auto_programs.GriefMiddle; import competition.auto_programs.SubwooferShotFromMidShootThenShootNearestThree; import competition.auto_programs.TestVisionAuto; @@ -23,11 +25,7 @@ import competition.operator_interface.OperatorCommandMap; import competition.operator_interface.OperatorInterface; import competition.subsystems.SubsystemDefaultCommandMap; -import competition.subsystems.oracle.ManualRobotKnowledgeSubsystem; -import competition.subsystems.oracle.NoteCollectionInfoSource; -import competition.subsystems.oracle.NoteFiringInfoSource; import competition.subsystems.vision.VisionSubsystem; -import dagger.Binds; import xbot.common.injection.components.BaseComponent; import xbot.common.subsystems.drive.swerve.SwerveDefaultCommandMap; @@ -71,4 +69,6 @@ public abstract class BaseRobotComponent extends BaseComponent { public abstract ListenToOracleCommandGroup listenToOracleCommandGroup(); public abstract TestVisionAuto testVisionAuto(); public abstract GriefMiddle griefMiddle(); + public abstract BotCenter4ThenCenter5 botCenter4ThenCenter5(); + public abstract BotCenter5ThenCenter4 botCenter5ThenCenter4(); } \ No newline at end of file diff --git a/src/main/java/competition/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java index 447c210d..16052917 100644 --- a/src/main/java/competition/operator_interface/OperatorCommandMap.java +++ b/src/main/java/competition/operator_interface/OperatorCommandMap.java @@ -1,15 +1,15 @@ package competition.operator_interface; +import competition.auto_programs.BotCenter5ThenCenter4; import competition.auto_programs.DoNothingAuto; import competition.auto_programs.GriefMiddle; import competition.auto_programs.SixNoteBnbExtended; import competition.auto_programs.SubwooferShotFromBotShootThenShootBotSpikeThenShootBotCenter; import competition.auto_programs.SubwooferShotFromBotShootThenShootSpikes; -import competition.auto_programs.SubwooferShotFromBotThenTwoCenterline; +import competition.auto_programs.BotCenter4ThenCenter5; import competition.auto_programs.SubwooferShotFromMidShootThenShootNearestThree; import competition.auto_programs.SubwooferShotFromTopShootThenShootSpikes; import competition.auto_programs.SubwooferShotFromTopShootThenShootTopSpikeThenShootTwoCenter; -import competition.auto_programs.TwoNoteGriefAuto; import competition.commandgroups.PrepareToFireAtSpeakerFromPodiumCommand; import competition.commandgroups.PrepareToFireNearestGoodScoringPositionCommand; import competition.commandgroups.PrepareToLobShotCommand; @@ -29,7 +29,6 @@ import competition.subsystems.drive.commands.DriveToAmpCommand; import competition.subsystems.drive.commands.DriveToNearestGoodScoringPositionCommand; import competition.subsystems.drive.commands.LineUpForHangingCommand; -import competition.subsystems.drive.commands.PointAtNoteCommand; import competition.subsystems.drive.commands.PointAtNoteWithBearingCommand; import competition.subsystems.drive.commands.TryDriveToBearingNote; import competition.subsystems.flipper.commands.SetFlipperServoToHangPositionCommand; @@ -239,7 +238,8 @@ public void setupAutonomousCommandSelection(OperatorInterface oi, SixNoteBnbExtended bnbExtended, DoNothingAuto doNothing, GriefMiddle grief, - SubwooferShotFromBotThenTwoCenterline botThenTwoCenter, + BotCenter4ThenCenter5 botCenter4ThenCenter5, + BotCenter5ThenCenter4 botCenter5ThenCenter4, ArmSubsystem arm) { var setOracleAuto = setAutonomousCommandProvider.get(); setOracleAuto.setAutoCommand(listenToOracleCommandGroup); @@ -274,9 +274,15 @@ public void setupAutonomousCommandSelection(OperatorInterface oi, setDoNothing.setAutoCommand(doNothing); oi.neoTrellis.getifAvailable(8).onTrue(setDoNothing); - var setBotThenTwoCenter = setAutonomousCommandProvider.get(); - setBotThenTwoCenter.setAutoCommand(botThenTwoCenter); - oi.neoTrellis.getifAvailable(30).onTrue(setBotThenTwoCenter); + var setbotCenter4ThenCenter5 = setAutonomousCommandProvider.get(); + setbotCenter4ThenCenter5.setAutoCommand(botCenter4ThenCenter5); + oi.neoTrellis.getifAvailable(30).onTrue(setbotCenter4ThenCenter5); + + var setbotCenter5ThenCenter4 = setAutonomousCommandProvider.get(); + setbotCenter5ThenCenter4.setAutoCommand(botCenter5ThenCenter4); + oi.neoTrellis.getifAvailable(22).onTrue(setbotCenter5ThenCenter4); + + oi.neoTrellis.getifAvailable(1).onTrue(new InstantCommand(() -> arm.increaseTrimInMeters(0.1524))); oi.neoTrellis.getifAvailable(9).onTrue(new InstantCommand(() -> arm.increaseTrimInMeters(-0.1524)));