diff --git a/SeriouslyCommonLib b/SeriouslyCommonLib index 3e65d596..3ad97ebb 160000 --- a/SeriouslyCommonLib +++ b/SeriouslyCommonLib @@ -1 +1 @@ -Subproject commit 3e65d596f90fc67290f86cd8bc6af999b130d37f +Subproject commit 3ad97ebb330df7ea18c2f3c60e6ae1a98ac0f445 diff --git a/src/main/java/competition/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java index a85a6f51..7e13b630 100644 --- a/src/main/java/competition/operator_interface/OperatorCommandMap.java +++ b/src/main/java/competition/operator_interface/OperatorCommandMap.java @@ -6,6 +6,7 @@ import competition.subsystems.collector.commands.EjectCollectorCommand; import competition.subsystems.collector.commands.IntakeCollectorCommand; +import competition.subsystems.drive.DriveSubsystem; import competition.subsystems.oracle.SwerveAccordingToOracleCommand; import competition.subsystems.oracle.DynamicOracle; import competition.subsystems.oracle.ManualRobotKnowledgeSubsystem; @@ -16,6 +17,7 @@ import competition.subsystems.shooter.commands.WarmUpShooterCommand; import edu.wpi.first.math.geometry.Pose2d; import edu.wpi.first.math.geometry.Rotation2d; +import edu.wpi.first.wpilibj2.command.InstantCommand; import xbot.common.controls.sensors.XXboxController.XboxButton; import xbot.common.subsystems.drive.SwerveSimpleTrajectoryCommand; import xbot.common.subsystems.pose.commands.SetRobotHeadingCommand; @@ -65,7 +67,8 @@ public void setupMobilityComands( OperatorInterface operatorInterface, Provider swerveCommandProvider, SetRobotHeadingCommand resetHeading, - DynamicOracle oracle + DynamicOracle oracle, + DriveSubsystem drive ) { double typicalVelocity = 2.5; @@ -76,6 +79,12 @@ public void setupMobilityComands( operatorInterface.driverGamepad.getXboxButton(XboxButton.A).onTrue(resetHeading); LowResField fieldWithObstacles = oracle.getFieldWithObstacles(); + var noviceMode = new InstantCommand(() -> drive.setNoviceMode(true)); + var expertMode = new InstantCommand(() -> drive.setNoviceMode(false)); + + operatorInterface.driverGamepad.getXboxButton(XboxButton.LeftStick).onTrue(noviceMode); + operatorInterface.driverGamepad.getXboxButton(XboxButton.RightStick).onTrue(expertMode); + // Where are some cool places we may want to go.. // 1) Where there are Notes! var goToTopSpike = createAndConfigureTypicalSwerveCommand(