Drive to note with bearing: Rotation slow move to center

src/main/java/competition/commandgroups/GoToGenericMidline.java

@@ -25,7 +25,7 @@ public class GoToGenericMidline extends SequentialCommandGroup {
     protected AutonomousCommandSelector autoSelector;
     protected double centerlineTimeout = 999;
     protected double meterThreshold = 0.3048;
-    protected double velocityThreshold = 0.05;
+    protected double velocityThreshold = 0.01;
     protected PoseSubsystem pose;
     protected DriveSubsystem drive;
 
@@ -120,6 +120,7 @@ protected ArrayList<XbotSwervePoint> goBackToBotSubwoofer(){
     }
 
     protected boolean alternativeIsFinishedForSubwoofer() {
+
         double speed = pose.getRobotCurrentSpeed();
 
         Translation2d robotLocation = pose.getCurrentPose2d().getTranslation();

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

@@ -86,11 +86,14 @@ public void setupDriverCommands(
         var pointAtSource = drive.createSetSpecialHeadingTargetCommand(
                 () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.FaceCollectorToBlueSource));
 
+        var pointAtAmpForLob = drive.createSetSpecialHeadingTargetCommand(
+                () -> PoseSubsystem.convertBlueToRedIfNeeded(PoseSubsystem.FaceShooterToBlueAmpForLob));
+
         var cancelSpecialPointAtPosition = drive.createClearAllSpecialTargetsCommand();
 
         operatorInterface.driverGamepad.getXboxButton(XboxButton.Back).whileTrue(listenToOracleCommandGroup);
         operatorInterface.driverGamepad.getXboxButton(XboxButton.Start).onTrue(resetHeading);
-        operatorInterface.driverGamepad.getXboxButton(XboxButton.RightBumper).whileTrue(driveToAmpCommand);
+        operatorInterface.driverGamepad.getXboxButton(XboxButton.RightBumper).whileTrue(pointAtAmpForLob);
         operatorInterface.driverGamepad.getXboxButton(XboxButton.LeftBumper).whileTrue(driveToNearestGoodScoringPositionCommand);
         operatorInterface.driverGamepad.getXboxButton(XboxButton.X).whileTrue(limitArmToUnderStageCommand);
         operatorInterface.driverGamepad.getXboxButton(XboxButton.A).whileTrue(alignToNoteCommand);

src/main/java/competition/subsystems/arm/ArmSubsystem.java

@@ -498,7 +498,7 @@ public double getUsefulArmPositionExtensionInMm(UsefulArmPosition usefulArmPosit
                 break;
             case LOB_SHOT:
                 //This still needs to be found
-                extension = 55;
+                extension = 180;
                 break;
             case HANG_APPROACH:
                 extension = 120;

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

@@ -182,6 +182,10 @@ protected boolean getErrorWithinTolerance() {
         if (arm.getTargetValue() >= 100) {
             tolerance = 5;
         }
+
+        if (arm.getTargetValue() >= 170) {
+            tolerance = 10;
+        }
 
         if (Math.abs(getErrorMagnitude()) < tolerance) {
             return true;

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

@@ -98,7 +98,7 @@ public void execute() {
             case SearchViaRotation:
                 if (currentAdvice.suggestedDrivePercentages.isPresent()) {
                     var driveValues = currentAdvice.suggestedDrivePercentages.get();
-                    drive.move(new XYPair(driveValues.dx, driveValues.dy), driveValues.dtheta);
+                    drive.fieldOrientedDrive(new XYPair(driveValues.dx, driveValues.dy), driveValues.dtheta, pose.getCurrentHeading().getDegrees(), new XYPair(0,0));
                 }
                 break;
             default:

src/main/java/competition/subsystems/pose/PoseSubsystem.java

@@ -96,6 +96,7 @@ public class PoseSubsystem extends BasePoseSubsystem {
     public static Pose2d BlueSpikeBottomWhiteLine = new Pose2d(1.93294, 4.1056, new Rotation2d());
 
     public static Rotation2d FaceCollectorToBlueSource = Rotation2d.fromDegrees(120);
+    public static Rotation2d FaceShooterToBlueAmpForLob = Rotation2d.fromDegrees(140);
 
     // More navigation points
     public static Pose2d BluePodiumWaypoint = new Pose2d(2.5, 4.25, new Rotation2d());

src/main/java/competition/subsystems/vision/NoteSeekLogic.java

@@ -11,10 +11,12 @@
 import org.apache.logging.log4j.LogManager;
 import xbot.common.advantage.AKitLogger;
 import xbot.common.controls.sensors.XTimer;
+import xbot.common.logic.TimeStableValidator;
 import xbot.common.properties.DoubleProperty;
 import xbot.common.properties.Property;
 import xbot.common.properties.PropertyFactory;
 import xbot.common.subsystems.drive.control_logic.HeadingModule;
+import xbot.common.subsystems.pose.BasePoseSubsystem;
 
 import javax.inject.Inject;
 import java.util.Optional;
@@ -49,6 +51,7 @@ public class NoteSeekLogic {
     final DoubleProperty terminalVisionModePowerFactor;
     final DoubleProperty backUpDuration;
     final DoubleProperty rotateToNoteDuration;
+    final DoubleProperty rotationSlowCenterYPower;
 
     boolean hasDoneVisionCheckYet = false;
     protected final AKitLogger aKitLog;
@@ -61,6 +64,7 @@ public class NoteSeekLogic {
     private final HeadingModule headingModule;
     private boolean allowRotationSearch = false;
     private VisionRange visionRange = VisionRange.Close;
+    final TimeStableValidator centerCamStableValidator = new TimeStableValidator(0.3); 
 
     @Inject
     public NoteSeekLogic(VisionSubsystem vision, DynamicOracle oracle, PoseSubsystem pose,
@@ -80,6 +84,7 @@ public NoteSeekLogic(VisionSubsystem vision, DynamicOracle oracle, PoseSubsystem
         terminalVisionModePowerFactor = pf.createPersistentProperty("TerminalVisionModePowerFactor", 0.5);
         backUpDuration = pf.createPersistentProperty("BackUpDuration", 1.0);
         rotateToNoteDuration = pf.createPersistentProperty("RotateToNoteDuration", 1.0);
+        rotationSlowCenterYPower = pf.createPersistentProperty("RotationSlowCenterYPower", 0.05);
 
         visionModeTimeoutTracker = new TimeoutTracker(() -> visionModeDuration.get());
         rotateToNoteModeTimeoutTracker = new TimeoutTracker(() -> rotateToNoteDuration.get());
@@ -111,6 +116,7 @@ public void reset() {
             visionModeTimeoutTracker.start();
         }
         hasDoneVisionCheckYet = false;
+        centerCamStableValidator.checkStable(false);
     }
 
     private void resetVisionModeTimers() {
@@ -133,7 +139,7 @@ private void checkForModeChanges(boolean atTargetPose) {
                 }
 
                 double rangeToStaticNote = pose.getCurrentPose2d().getTranslation().getDistance(
-                        drive.getTargetNote().getTranslation());
+                        (BasePoseSubsystem.convertBlueToRedIfNeeded(drive.getTargetNote())).getTranslation());
                 aKitLog.record("RangeToStaticNote", rangeToStaticNote);
 
                 if (rangeToStaticNote < vision.getBestRangeFromStaticNoteToSearchForNote()) {
@@ -250,7 +256,13 @@ public NoteSeekAdvice getAdvice(boolean atTargetPose) {
                 return new NoteSeekAdvice(
                         noteAcquisitionMode, Optional.of(suggestedLocation),Optional.empty());
             case SearchViaRotation:
-                suggestedPowers = new Twist2d(0, 0, rotationSearchPower.get());
+                // drive very slowly towards the center-x of the field so that we get unstuck from the wall if we're on it
+                var yPower = rotationSlowCenterYPower.get();
+                if(pose.getCurrentPose2d().getY() > PoseSubsystem.CenterLine3.getY()) {
+                    // on top half of field, head south
+                    yPower *= -1;
+                }
+                suggestedPowers = new Twist2d(0, yPower, rotationSearchPower.get());
                 return new NoteSeekAdvice(noteAcquisitionMode, Optional.empty(), Optional.of(suggestedPowers));
             case GiveUp:
             default:
@@ -359,8 +371,11 @@ private boolean hasSolidLockOnNoteWithCenterCamera() {
         // If the note is roughly centered on the center camera, we can try driving to it.
         var target = vision.getCenterCamLargestNoteTarget();
         if (target.isPresent()) {
-            return Math.abs(target.get().getYaw()) < 15;
+            var isNearCenter = Math.abs(target.get().getYaw()) < 15;
+            var isStable = centerCamStableValidator.checkStable(isNearCenter);
+            return isNearCenter && isStable;
         }
+        centerCamStableValidator.checkStable(false);
         return false;
     }