Updated note filtering and made drive assist the default

Robocubs · Jul 22, 2024 · 7086f19 · 7086f19
1 parent 4fed08d
commit 7086f19
Show file tree

Hide file tree

Showing 3 changed files with 6 additions and 4 deletions.
diff --git a/src/main/java/com/team1701/robot/commands/DriveWithAssist.java b/src/main/java/com/team1701/robot/commands/DriveWithAssist.java
@@ -29,7 +29,7 @@ public class DriveWithAssist extends Command {
             new LoggedTunableNumber(kLoggingPrefix + "TranslationKd", 0.0);
 
     private static final double kTranslationTolerance = 0.01;
-    private static final double kMaxTranslationPIDOutput = 0.5;
+    private static final double kMaxTranslationPIDOutput = 0.75;
 
     private final Drive mDrive;
     private final RobotState mRobotState;

diff --git a/src/main/java/com/team1701/robot/states/RobotState.java b/src/main/java/com/team1701/robot/states/RobotState.java
@@ -114,11 +114,13 @@ public void periodic() {
 
         mDetectedNoteForPickup = mDetectedNotes.stream()
                 .filter(note -> GeometryUtil.isNear(
-                        robotRotationReverse,
+                        (fieldRelativeSpeeds.vxMetersPerSecond > 0 && fieldRelativeSpeeds.vyMetersPerSecond > 0)
+                                ? robotPoseWithDirection.getRotation()
+                                : robotRotationReverse,
                         GeometryUtil.getTranslation2d(note.pose())
                                 .minus(robotTranslation)
                                 .getAngle(),
-                        Rotation2d.fromDegrees(35)))
+                        Rotation2d.fromDegrees(25)))
                 .min((note1, note2) -> Double.compare(
                         rankNote(note1, robotPoseWithDirection), rankNote(note2, robotPoseWithDirection)));
 

diff --git a/src/main/java/com/team1701/robot/subsystems/drive/Drive.java b/src/main/java/com/team1701/robot/subsystems/drive/Drive.java
@@ -111,7 +111,7 @@ public Drive(GyroIO gyroIO, SwerveModuleIO[] moduleIOs, RobotState robotState) {
                         .ignoringDisable(true)
                         .withName("DriveEnabledStart"));
 
-        mUseDriveAssist = false;
+        mUseDriveAssist = true;
     }
 
     @Override