diff --git a/src/main/java/competition/commandgroups/GoToGenericMidline.java b/src/main/java/competition/commandgroups/GoToGenericMidline.java index d8fef074..f2490c62 100644 --- a/src/main/java/competition/commandgroups/GoToGenericMidline.java +++ b/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 goBackToBotSubwoofer(){ } protected boolean alternativeIsFinishedForSubwoofer() { + double speed = pose.getRobotCurrentSpeed(); Translation2d robotLocation = pose.getCurrentPose2d().getTranslation(); diff --git a/src/main/java/competition/operator_interface/OperatorCommandMap.java b/src/main/java/competition/operator_interface/OperatorCommandMap.java index 16052917..45ce235d 100644 --- a/src/main/java/competition/operator_interface/OperatorCommandMap.java +++ b/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); diff --git a/src/main/java/competition/subsystems/arm/ArmSubsystem.java b/src/main/java/competition/subsystems/arm/ArmSubsystem.java index 24293bf5..a81b5234 100644 --- a/src/main/java/competition/subsystems/arm/ArmSubsystem.java +++ b/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; diff --git a/src/main/java/competition/subsystems/arm/commands/ArmMaintainerCommand.java b/src/main/java/competition/subsystems/arm/commands/ArmMaintainerCommand.java index 4e23ecc1..cff5951c 100644 --- a/src/main/java/competition/subsystems/arm/commands/ArmMaintainerCommand.java +++ b/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; diff --git a/src/main/java/competition/subsystems/drive/commands/DriveToGivenNoteWithBearingVisionCommand.java b/src/main/java/competition/subsystems/drive/commands/DriveToGivenNoteWithBearingVisionCommand.java index a9cf7b10..c0c92477 100644 --- a/src/main/java/competition/subsystems/drive/commands/DriveToGivenNoteWithBearingVisionCommand.java +++ b/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: diff --git a/src/main/java/competition/subsystems/pose/PoseSubsystem.java b/src/main/java/competition/subsystems/pose/PoseSubsystem.java index 15fd7d91..8a75da7b 100644 --- a/src/main/java/competition/subsystems/pose/PoseSubsystem.java +++ b/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()); diff --git a/src/main/java/competition/subsystems/vision/NoteSeekLogic.java b/src/main/java/competition/subsystems/vision/NoteSeekLogic.java index 61723d4b..ea6abdf6 100644 --- a/src/main/java/competition/subsystems/vision/NoteSeekLogic.java +++ b/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; }