From 34ec001191ec2784828c1ac96dda70b84b3a595c Mon Sep 17 00:00:00 2001 From: JohnGilb Date: Fri, 5 Apr 2024 10:51:43 -0700 Subject: [PATCH 1/6] Small tuning changes --- .../java/competition/commandgroups/GoToGenericMidline.java | 3 ++- .../competition/operator_interface/OperatorCommandMap.java | 5 ++++- src/main/java/competition/subsystems/arm/ArmSubsystem.java | 2 +- .../subsystems/arm/commands/ArmMaintainerCommand.java | 4 ++++ src/main/java/competition/subsystems/pose/PoseSubsystem.java | 1 + 5 files changed, 12 insertions(+), 3 deletions(-) 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/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()); From f1c3d9c8d01dfcb79c48ba21284ea43fd9c40621 Mon Sep 17 00:00:00 2001 From: Alex Schokking Date: Fri, 5 Apr 2024 12:35:50 -0700 Subject: [PATCH 2/6] fix bug in NoteSeekingLogic not being red/blue aware for distance to note --- src/main/java/competition/subsystems/vision/NoteSeekLogic.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/java/competition/subsystems/vision/NoteSeekLogic.java b/src/main/java/competition/subsystems/vision/NoteSeekLogic.java index 61723d4b..7d5b5ce5 100644 --- a/src/main/java/competition/subsystems/vision/NoteSeekLogic.java +++ b/src/main/java/competition/subsystems/vision/NoteSeekLogic.java @@ -15,6 +15,7 @@ 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; @@ -133,7 +134,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()) { From 8bd48e4763bfa4870f8988f2d10dbad1629e151a Mon Sep 17 00:00:00 2001 From: Alex Schokking Date: Fri, 5 Apr 2024 14:42:20 -0700 Subject: [PATCH 3/6] Time stable center cam read in auto (#325) * Make sure center cam sees note for at least 2 cycles * fix bug --- .../java/competition/subsystems/vision/NoteSeekLogic.java | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/java/competition/subsystems/vision/NoteSeekLogic.java b/src/main/java/competition/subsystems/vision/NoteSeekLogic.java index 7d5b5ce5..4723dc6f 100644 --- a/src/main/java/competition/subsystems/vision/NoteSeekLogic.java +++ b/src/main/java/competition/subsystems/vision/NoteSeekLogic.java @@ -11,6 +11,7 @@ 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; @@ -62,6 +63,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, @@ -112,6 +114,7 @@ public void reset() { visionModeTimeoutTracker.start(); } hasDoneVisionCheckYet = false; + centerCamStableValidator.checkStable(false); } private void resetVisionModeTimers() { @@ -360,8 +363,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; } From b2f399a438b284cbfdd97a97d05f04febf64a867 Mon Sep 17 00:00:00 2001 From: Alex Schokking Date: Fri, 5 Apr 2024 14:51:37 -0700 Subject: [PATCH 4/6] Start implemtning slow move --- .../commands/DriveToGivenNoteWithBearingVisionCommand.java | 2 +- .../java/competition/subsystems/vision/NoteSeekLogic.java | 6 ++++++ 2 files changed, 7 insertions(+), 1 deletion(-) 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/vision/NoteSeekLogic.java b/src/main/java/competition/subsystems/vision/NoteSeekLogic.java index 4723dc6f..850c625f 100644 --- a/src/main/java/competition/subsystems/vision/NoteSeekLogic.java +++ b/src/main/java/competition/subsystems/vision/NoteSeekLogic.java @@ -255,6 +255,12 @@ public NoteSeekAdvice getAdvice(boolean atTargetPose) { 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 = 0.05; + if(pose.getCurrentPose2d().getY() > fieldMidPoint) { + // on top half of field, head south + yPower -= 1; + } return new NoteSeekAdvice(noteAcquisitionMode, Optional.empty(), Optional.of(suggestedPowers)); case GiveUp: default: From be44a9fc471f4b075b884f882e54e74f8b6ac89f Mon Sep 17 00:00:00 2001 From: Alex Schokking Date: Fri, 5 Apr 2024 15:06:21 -0700 Subject: [PATCH 5/6] finish logic and fix bug --- .../java/competition/subsystems/vision/NoteSeekLogic.java | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/java/competition/subsystems/vision/NoteSeekLogic.java b/src/main/java/competition/subsystems/vision/NoteSeekLogic.java index 850c625f..020bd950 100644 --- a/src/main/java/competition/subsystems/vision/NoteSeekLogic.java +++ b/src/main/java/competition/subsystems/vision/NoteSeekLogic.java @@ -254,13 +254,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 = 0.05; - if(pose.getCurrentPose2d().getY() > fieldMidPoint) { + if(pose.getCurrentPose2d().getY() > PoseSubsystem.CenterLine3.getY()) { // on top half of field, head south - yPower -= 1; + yPower *= -1; } + suggestedPowers = new Twist2d(0, yPower, rotationSearchPower.get()); return new NoteSeekAdvice(noteAcquisitionMode, Optional.empty(), Optional.of(suggestedPowers)); case GiveUp: default: From 6de1dfd2597d57694b1bc21dde8e0274b8291cc1 Mon Sep 17 00:00:00 2001 From: Alex Schokking Date: Fri, 5 Apr 2024 15:10:20 -0700 Subject: [PATCH 6/6] use a double property instead so easier to tune --- .../java/competition/subsystems/vision/NoteSeekLogic.java | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/java/competition/subsystems/vision/NoteSeekLogic.java b/src/main/java/competition/subsystems/vision/NoteSeekLogic.java index 020bd950..ea6abdf6 100644 --- a/src/main/java/competition/subsystems/vision/NoteSeekLogic.java +++ b/src/main/java/competition/subsystems/vision/NoteSeekLogic.java @@ -51,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; @@ -83,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()); @@ -255,7 +257,7 @@ public NoteSeekAdvice getAdvice(boolean atTargetPose) { noteAcquisitionMode, Optional.of(suggestedLocation),Optional.empty()); case SearchViaRotation: // 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 = 0.05; + var yPower = rotationSlowCenterYPower.get(); if(pose.getCurrentPose2d().getY() > PoseSubsystem.CenterLine3.getY()) { // on top half of field, head south yPower *= -1;