Prepare for Practice Field

src/main/java/competition/auto_programs/DistanceShotFromMidShootThenShootNearestThree.java

@@ -27,7 +27,7 @@ public DistanceShotFromMidShootThenShootNearestThree(AutonomousCommandSelector a
 
         // Force our location
         var startInFrontOfSpeaker = pose.createSetPositionCommand(
-                () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.SubwooferCentralScoringLocation));
+                () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.BlueSubwooferCentralScoringLocation));
         this.addCommands(startInFrontOfSpeaker);
 
         // Fire note into the speaker from starting position

src/main/java/competition/auto_programs/FromMidShootCollectShoot.java

@@ -9,7 +9,6 @@
 import competition.subsystems.shooter.commands.FireWhenReadyCommand;
 import competition.subsystems.shooter.commands.WarmUpShooterCommand;
 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.SwerveSimpleTrajectoryCommand;
@@ -38,7 +37,7 @@ public FromMidShootCollectShoot(
         this.autoSelector = autoSelector;
         // Force our location
         var startInFrontOfSpeaker = pose.createSetPositionCommand(
-                () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.SubwooferCentralScoringLocation));
+                () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.BlueSubwooferCentralScoringLocation));
         this.addCommands(startInFrontOfSpeaker);
 
         // Shoot the pre-loaded note from the subwoofer
@@ -75,7 +74,7 @@ public FromMidShootCollectShoot(
         driveToSubwoofer.logic.setConstantVelocity(1);
         driveToSubwoofer.logic.setKeyPointsProvider(() -> {
             ArrayList<XbotSwervePoint> points = new ArrayList<>();
-            var target = BasePoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.SubwooferCentralScoringLocation);
+            var target = BasePoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.BlueSubwooferCentralScoringLocation);
             points.add(new XbotSwervePoint(target.getTranslation(), target.getRotation(), 10));
             return points;
         });

src/main/java/competition/auto_programs/SubwooferShotFromMidShootThenShootNearestThree.java

@@ -2,7 +2,6 @@
 
 import competition.commandgroups.DriveToGivenNoteAndCollectCommandGroup;
 import competition.commandgroups.FireFromSubwooferCommandGroup;
-import competition.commandgroups.FireNoteCommandGroup;
 import competition.subsystems.drive.DriveSubsystem;
 import competition.subsystems.drive.commands.DriveToCentralSubwooferCommand;
 import competition.subsystems.pose.PoseSubsystem;
@@ -28,7 +27,7 @@ public SubwooferShotFromMidShootThenShootNearestThree(AutonomousCommandSelector
 
         // Force our location
         var startInFrontOfSpeaker = pose.createSetPositionCommand(
-                () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.SubwooferCentralScoringLocation));
+                () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.BlueSubwooferCentralScoringLocation));
         this.addCommands(startInFrontOfSpeaker);
 
         // Fire preload note into the speaker from starting position

src/main/java/competition/auto_programs/TwoNoteGriefAuto.java

@@ -1,29 +1,21 @@
 package competition.auto_programs;
 
-import competition.commandgroups.FireNoteCommandGroup;
 import competition.subsystems.arm.ArmSubsystem;
 import competition.subsystems.arm.commands.SetArmAngleCommand;
-import competition.subsystems.collector.commands.EjectCollectorCommand;
 import competition.subsystems.collector.commands.IntakeUntilNoteCollectedCommand;
-import competition.subsystems.collector.commands.StopCollectorCommand;
 import competition.subsystems.oracle.DynamicOracle;
 import competition.subsystems.pose.PoseSubsystem;
 import competition.subsystems.schoocher.commands.EjectScoocherCommand;
 import competition.subsystems.schoocher.commands.IntakeScoocherCommand;
 import competition.subsystems.shooter.ShooterWheelSubsystem;
 import competition.subsystems.shooter.commands.FireWhenReadyCommand;
 import competition.subsystems.shooter.commands.WarmUpShooterCommand;
-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.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
-import edu.wpi.first.wpilibj2.command.InstantCommand;
-import edu.wpi.first.wpilibj2.command.ParallelCommandGroup;
 import edu.wpi.first.wpilibj2.command.SequentialCommandGroup;
 import xbot.common.subsystems.autonomous.AutonomousCommandSelector;
 import xbot.common.subsystems.drive.SwerveSimpleTrajectoryCommand;
-import xbot.common.subsystems.pose.BasePoseSubsystem;
 import xbot.common.trajectory.XbotSwervePoint;
 
 import javax.inject.Inject;
@@ -59,7 +51,7 @@ public TwoNoteGriefAuto(Provider<SwerveSimpleTrajectoryCommand> swerveProvider,
 
         //starts us in front of the subwoofer, to score
         var startInFrontOfSpeaker = pose.createSetPositionCommand(
-                () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.SubwooferCentralScoringLocation));
+                () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.BlueSubwooferCentralScoringLocation));
         this.addCommands(startInFrontOfSpeaker);
 
         //fires the note we are holding at the start

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

@@ -1,36 +1,24 @@
 package competition.operator_interface;
 
-import javax.inject.Inject;
-import javax.inject.Provider;
-import javax.inject.Singleton;
-
-import competition.auto_programs.DistanceShotFromMidShootThenShootNearestThree;
-import competition.auto_programs.ShootThenMoveOutOfLine;
-import competition.commandgroups.FireNoteCommandGroup;
 import competition.auto_programs.FromMidShootCollectShoot;
-import competition.auto_programs.SubwooferShotFromMidShootThenShootNearestThree;
-import competition.commandgroups.DriveToGivenNoteAndCollectCommandGroup;
 import competition.commandgroups.PrepareToFireAtAmpCommandGroup;
 import competition.commandgroups.PrepareToFireAtSpeakerCommandGroup;
 import competition.subsystems.arm.ArmSubsystem;
-import competition.subsystems.arm.commands.ArmMaintainerCommand;
 import competition.subsystems.arm.commands.CalibrateArmsManuallyCommand;
 import competition.subsystems.arm.commands.ContinuouslyPointArmAtSpeakerCommand;
 import competition.subsystems.arm.commands.DisengageBrakeCommand;
 import competition.subsystems.arm.commands.EngageBrakeCommand;
-import competition.subsystems.arm.commands.ManipulateArmBrakeCommand;
 import competition.subsystems.arm.commands.SetArmAngleCommand;
 import competition.subsystems.arm.commands.SetArmExtensionCommand;
 import competition.subsystems.collector.commands.EjectCollectorCommand;
 import competition.subsystems.collector.commands.FireCollectorCommand;
 import competition.subsystems.collector.commands.IntakeCollectorCommand;
 import competition.subsystems.drive.DriveSubsystem;
 import competition.subsystems.drive.commands.AlignToNoteCommand;
-import competition.subsystems.drive.commands.DriveToCentralSubwooferCommand;
-import competition.subsystems.oracle.SuperstructureAccordingToOracleCommand;
-import competition.subsystems.oracle.SwerveAccordingToOracleCommand;
 import competition.subsystems.oracle.DynamicOracle;
 import competition.subsystems.oracle.ManualRobotKnowledgeSubsystem;
+import competition.subsystems.oracle.SuperstructureAccordingToOracleCommand;
+import competition.subsystems.oracle.SwerveAccordingToOracleCommand;
 import competition.subsystems.pose.PoseSubsystem;
 import competition.subsystems.schoocher.commands.EjectScoocherCommand;
 import competition.subsystems.schoocher.commands.IntakeScoocherCommand;
@@ -49,6 +37,9 @@
 import xbot.common.trajectory.LowResField;
 import xbot.common.trajectory.XbotSwervePoint;
 
+import javax.inject.Inject;
+import javax.inject.Provider;
+import javax.inject.Singleton;
 import java.util.ArrayList;
 
 /**
@@ -132,9 +123,18 @@ public void setupMobilityComands(
     {
         double typicalVelocity = 2.5;
         // Manipulate heading and position for easy testing
-        resetHeading.setHeadingToApply(180);
-        var teleportRobot = pose.createSetPositionCommand(PoseSubsystem.SubwooferCentralScoringLocation);
-        operatorInterface.driverGamepad.getPovIfAvailable(180).onTrue(teleportRobot);
+        resetHeading.setHeadingToApply(() -> PoseSubsystem.convertBlueToRedIfNeeded(Rotation2d.fromDegrees(180)).getDegrees());
+
+        var teleportRobotToSubwooferTop = pose.createSetPositionCommand(
+                () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.BlueSubwooferTopScoringLocation));
+        operatorInterface.driverGamepad.getXboxButton(XboxButton.Y).onTrue(teleportRobotToSubwooferTop);
+        var teleportRobotToSubwooferMid = pose.createSetPositionCommand(
+                () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.BlueSubwooferCentralScoringLocation));
+        operatorInterface.driverGamepad.getXboxButton(XboxButton.X).onTrue(teleportRobotToSubwooferMid);
+        var teleportRobotToSubwooferBottom = pose.createSetPositionCommand(
+                () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.BlueSubwooferBottomScoringLocation));
+        operatorInterface.driverGamepad.getXboxButton(XboxButton.A).onTrue(teleportRobotToSubwooferBottom);
+
 
         operatorInterface.driverGamepad.getXboxButton(XboxButton.Start).onTrue(resetHeading);
         LowResField fieldWithObstacles = oracle.getFieldWithObstacles();
@@ -182,7 +182,7 @@ public void setupMobilityComands(
         var goToAmp = createAndConfigureTypicalSwerveCommand(
                 swerveCommandProvider.get(), PoseSubsystem.AmpScoringLocation, typicalVelocity, fieldWithObstacles);
         var goToSpeaker = createAndConfigureTypicalSwerveCommand(
-                swerveCommandProvider.get(), PoseSubsystem.SubwooferCentralScoringLocation, typicalVelocity, fieldWithObstacles);
+                swerveCommandProvider.get(), PoseSubsystem.BlueSubwooferCentralScoringLocation, typicalVelocity, fieldWithObstacles);
 
         // 3) Or pick up a new note from the source!
         var goToNoteSource = createAndConfigureTypicalSwerveCommand(
@@ -226,6 +226,7 @@ public void setupOracleCommands(OperatorInterface oi,
 
         oi.driverGamepad.getXboxButton(XboxButton.Back).whileTrue(driveAccoringToOracle.alongWith(superstructureAccordingToOracle));
         oi.driverGamepad.getPovIfAvailable(0).onTrue(new InstantCommand(() -> oracle.resetNoteMap()));
+        oi.driverGamepad.getPovIfAvailable(270).onTrue(new InstantCommand(() -> oracle.freezeConfigurationForAutonomous()));
 
     }
 

src/main/java/competition/subsystems/drive/commands/DriveToCentralSubwooferCommand.java

@@ -32,7 +32,7 @@ public void initialize() {
         log.info("Intitializing");
         ArrayList<XbotSwervePoint> swervePoints = new ArrayList<>();
         swervePoints.add(XbotSwervePoint.createPotentiallyFilppedXbotSwervePoint(
-                PoseSubsystem.SubwooferCentralScoringLocation.getTranslation(), Rotation2d.fromDegrees(180), 10));
+                PoseSubsystem.BlueSubwooferCentralScoringLocation.getTranslation(), Rotation2d.fromDegrees(180), 10));
         this.logic.setKeyPoints(swervePoints);
         this.logic.setEnableConstantVelocity(true);
         this.logic.setConstantVelocity(drive.getMaxTargetSpeedMetersPerSecond());

src/main/java/competition/subsystems/drive/commands/SwerveDriveWithJoysticksCommand.java

@@ -67,6 +67,9 @@ public SwerveDriveWithJoysticksCommand(
         this.triggerOnlyPowerScaling = pf.createPersistentProperty("TriggerOnlyPowerScaling", 0.75);
         this.triggerOnlyExponent = pf.createPersistentProperty("TriggerOnlyExponent", 2.0);
 
+        // We are already applying deadband to the joysticks; this additional layer of deadband is unnecessary.
+        decider.setDeadband(0.001);
+
         // Set up a latch to trigger whenever we change the rotational mode. In either case,
         // there's some PIDs that will need to be reset, or goals that need updating.
         absoluteOrientationLatch = new Latch(absoluteOrientationMode.get(), EdgeType.Both, edge -> {
@@ -126,19 +129,16 @@ public void execute() {
 
         // Grab all human sources of rotation intent
         double humanRotateIntentFromTriggers = getRotationIntentFromDriverTriggers();
-        double humanRotateIntentFromStick = getRotationIntentFromDriverJoystick();
 
         // Fuse them together while keeping them in the -1 to 1 range. This is to help avoid doing some kind of
         // conflicting move like trying to rotate in two directions at once.
-        double fusedHumanRotateIntent = MathUtils.constrainDoubleToRobotScale(
-                humanRotateIntentFromStick + humanRotateIntentFromTriggers);
 
         double rotateIntent = 0;
         if (absoluteOrientationMode.get()) {
             rotateIntent = getSuggestedRotateIntentForAbsoluteStickControl(humanRotateIntentFromTriggers);
         } else {
             // If we are in the typical "rotate using joystick to turn" mode, use the Heading Assist module to get the suggested power.
-            rotateIntent = scaleHumanRotationInput(fusedHumanRotateIntent);
+            rotateIntent = scaleHumanRotationInput(humanRotateIntentFromTriggers);
         }
 
         // --------------------------------------------------
@@ -305,7 +305,13 @@ private XYPair getTranslationIntentFromDriver() {
                 (a) -> MathUtils.exponentAndRetainSign(a, (int) input_exponent.get()));
 
         // create new vector with the scaled magnitude and angle
-        XYPair translationIntent = XYPair.fromPolar(rawAngle-90, updatedMagnitude);
+        // However, if we are on the red alliance, we need to rotate the intent a lot more.
+        double angleOffset = 90;
+        if (DriverStation.getAlliance().orElse(DriverStation.Alliance.Blue) == DriverStation.Alliance.Red) {
+            angleOffset = 270;
+        }
+
+        XYPair translationIntent = XYPair.fromPolar(rawAngle-angleOffset, updatedMagnitude);
         return translationIntent;
     }