Restore Arm Fine Adjustment & Auto Program Setting

src/main/java/competition/Robot.java

@@ -8,6 +8,7 @@
 import competition.injection.components.DaggerSimulationComponent;
 import competition.simulation.Simulator2024;
 import competition.subsystems.drive.DriveSubsystem;
+import competition.subsystems.oracle.DynamicOracle;
 import competition.subsystems.pose.PoseSubsystem;
 import edu.wpi.first.hal.AllianceStationID;
 import edu.wpi.first.wpilibj.Preferences;
@@ -24,6 +25,7 @@ public Robot() {
     }
 
     Simulator2024 simulator;
+    DynamicOracle oracle;
 
     @Override
     protected void initializeSystems() {
@@ -33,6 +35,7 @@ protected void initializeSystems() {
         getInjectorComponent().operatorCommandMap();
 
         simulator = getInjectorComponent().simulator2024();
+        oracle = getInjectorComponent().dynamicOracle();
 
         dataFrameRefreshables.add((DriveSubsystem)getInjectorComponent().driveSubsystem());
         dataFrameRefreshables.add(getInjectorComponent().poseSubsystem());
@@ -74,6 +77,12 @@ public BaseRobotComponent getInjectorComponent() {
         return (BaseRobotComponent)super.getInjectorComponent();
     }
 
+    @Override
+    public void autonomousInit() {
+        oracle.freezeConfigurationForAutonomous();
+        super.autonomousInit();
+    }
+
     @Override
     public void simulationInit() {
         super.simulationInit();

src/main/java/competition/injection/components/BaseRobotComponent.java

@@ -8,6 +8,7 @@
 import competition.subsystems.arm.commands.RetractArmCommand;
 import competition.subsystems.arm.commands.StopArmCommand;
 import competition.subsystems.collector.CollectorSubsystem;
+import competition.subsystems.oracle.DynamicOracle;
 import competition.subsystems.schoocher.ScoocherSubsystem;
 import competition.subsystems.shooter.ShooterWheelSubsystem;
 import competition.subsystems.shooter.commands.ShooterWheelMaintainerCommand;
@@ -53,4 +54,5 @@ public abstract class BaseRobotComponent extends BaseComponent {
 
     public abstract Simulator2024 simulator2024();
     public abstract ShooterWheelMaintainerCommand shooterWheelMaintainerCommand();
+    public abstract DynamicOracle dynamicOracle();
 }

src/main/java/competition/operator_interface/OperatorCommandMap.java

@@ -33,6 +33,7 @@
 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.autonomous.SetAutonomousCommand;
 import xbot.common.subsystems.drive.SwerveSimpleTrajectoryCommand;
 import xbot.common.subsystems.pose.commands.SetRobotHeadingCommand;
 import xbot.common.trajectory.LowResField;
@@ -127,13 +128,13 @@ public void setupMobilityComands(
         resetHeading.setHeadingToApply(() -> PoseSubsystem.convertBlueToRedIfNeeded(Rotation2d.fromDegrees(180)).getDegrees());
 
         var teleportRobotToSubwooferTop = pose.createSetPositionCommand(
-                () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.BlueSubwooferTopScoringLocation));
+                () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.BlueSubwooferTopScoringLocation)).ignoringDisable(true);
         operatorInterface.driverGamepad.getXboxButton(XboxButton.Y).onTrue(teleportRobotToSubwooferTop);
         var teleportRobotToSubwooferMid = pose.createSetPositionCommand(
-                () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.BlueSubwooferCentralScoringLocation));
+                () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.BlueSubwooferCentralScoringLocation)).ignoringDisable(true);
         operatorInterface.driverGamepad.getXboxButton(XboxButton.X).onTrue(teleportRobotToSubwooferMid);
         var teleportRobotToSubwooferBottom = pose.createSetPositionCommand(
-                () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.BlueSubwooferBottomScoringLocation));
+                () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.BlueSubwooferBottomScoringLocation)).ignoringDisable(true);
         operatorInterface.driverGamepad.getXboxButton(XboxButton.A).onTrue(teleportRobotToSubwooferBottom);
 
 
@@ -347,4 +348,18 @@ private SwerveSimpleTrajectoryCommand createAndConfigureTypicalSwerveCommand(
 
         return command;
     }
+
+    @Inject
+    public void setupAutonomousCommandSelection(OperatorInterface oi,
+                                                Provider<SetAutonomousCommand> setAutonomousCommandProvider,
+                                                SwerveAccordingToOracleCommand driveAccoringToOracle,
+                                                SuperstructureAccordingToOracleCommand superstructureAccordingToOracle) {
+
+        driveAccoringToOracle.logic.setEnableConstantVelocity(true);
+        driveAccoringToOracle.logic.setConstantVelocity(2.8);
+        var oracleAuto = driveAccoringToOracle.alongWith(superstructureAccordingToOracle);
+        var setOracleAuto = setAutonomousCommandProvider.get();
+        setOracleAuto.setAutoCommand(oracleAuto);
+        oi.neoTrellis.getifAvailable(31).onTrue(setOracleAuto);
+    }
 }

src/main/java/competition/subsystems/SubsystemDefaultCommandMap.java

@@ -36,7 +36,10 @@ public void setupArmSubsystem(ArmSubsystem armSubsystem,
                                   ArmMaintainerCommand command,
                                   SetArmTargetToCurrentPositionCommand setArmTargetToCurrentPositionCommand) {
         armSubsystem.setDefaultCommand(command);
-        armSubsystem.getSetpointLock().setDefaultCommand(setArmTargetToCurrentPositionCommand);
+        // Commenting this out until we have a better way of having the arm target position commands
+        // interrupt themselves. Another option would be for commands that set the arm position explicitly
+        // set the arm target to the current position when interrupted.
+        //armSubsystem.getSetpointLock().setDefaultCommand(setArmTargetToCurrentPositionCommand);
     }
     @Inject
     public void setupScoocherSubsystem(ScoocherSubsystem scoocherSubsystem, StopScoocherCommand command){

src/main/java/competition/subsystems/arm/commands/SetArmExtensionCommand.java

@@ -38,11 +38,6 @@ public void initialize() {
         armSubsystem.initializeRampingPowerTarget();
     }
 
-    @Override
-    public void execute() {
-        // no-op
-    }
-
     @Override
     public boolean isFinished() {
         return true;