edit autos to include note filtering stuff

davidchen20 · davidchen20 · commit d321e3f00a3d · 2024-08-16T15:00:21.000-04:00
diff --git a/src/main/deploy/pathplanner/autos/$a!p-b1-c1-c2.auto b/src/main/deploy/pathplanner/autos/$a!p-b1-c1-c2.auto
@@ -29,6 +29,12 @@
             "pathName": "get the note"
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "B1"
+          }
+        },
         {
           "type": "named",
           "data": {
@@ -53,6 +59,12 @@
             "pathName": "$apb1c(12)!score b1 c1"
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "C1"
+          }
+        },
         {
           "type": "named",
           "data": {
@@ -83,6 +95,12 @@
             "pathName": "$apb1c(12)!c2"
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "C2"
+          }
+        },
         {
           "type": "named",
           "data": {
diff --git a/src/main/deploy/pathplanner/autos/$a!p-b1-c2-c3.auto b/src/main/deploy/pathplanner/autos/$a!p-b1-c2-c3.auto
@@ -29,6 +29,12 @@
             "pathName": "get the note"
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "B1"
+          }
+        },
         {
           "type": "named",
           "data": {
@@ -53,6 +59,12 @@
             "pathName": "$apb1c(23)!c2"
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "C2"
+          }
+        },
         {
           "type": "named",
           "data": {
@@ -83,6 +95,12 @@
             "pathName": "$apb1c(23)!c3"
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "C3"
+          }
+        },
         {
           "type": "named",
           "data": {
diff --git a/src/main/deploy/pathplanner/autos/$c!p-b2-c3.auto b/src/main/deploy/pathplanner/autos/$c!p-b2-c3.auto
@@ -11,6 +11,12 @@
     "type": "sequential",
     "data": {
       "commands": [
+        {
+          "type": "named",
+          "data": {
+            "name": "B2"
+          }
+        },
         {
           "type": "sequential",
           "data": {
@@ -78,6 +84,12 @@
             "pathName": "$cpb2c3!c3"
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "C3"
+          }
+        },
         {
           "type": "named",
           "data": {
diff --git a/src/main/deploy/pathplanner/autos/$s!p-c3-c4.auto b/src/main/deploy/pathplanner/autos/$s!p-c3-c4.auto
@@ -23,6 +23,12 @@
             "pathName": "$spc(34)!p-c3"
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "C3"
+          }
+        },
         {
           "type": "named",
           "data": {
@@ -53,6 +59,12 @@
             "pathName": "$spc(34)!c4"
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "C4"
+          }
+        },
         {
           "type": "named",
           "data": {
diff --git a/src/main/deploy/pathplanner/autos/$s!p-c3-c5.auto b/src/main/deploy/pathplanner/autos/$s!p-c3-c5.auto
@@ -23,6 +23,12 @@
             "pathName": "$spc(35)!p-c3"
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "C3"
+          }
+        },
         {
           "type": "named",
           "data": {
@@ -53,6 +59,12 @@
             "pathName": "$spc(35)!c5"
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "C5"
+          }
+        },
         {
           "type": "named",
           "data": {
diff --git a/src/main/deploy/pathplanner/autos/$s!p-c4-c3.auto b/src/main/deploy/pathplanner/autos/$s!p-c4-c3.auto
@@ -23,6 +23,12 @@
             "pathName": "$spc(43)!p-c4"
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "C4"
+          }
+        },
         {
           "type": "named",
           "data": {
@@ -53,6 +59,12 @@
             "pathName": "$spc(43)!c3"
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "C3"
+          }
+        },
         {
           "type": "named",
           "data": {
diff --git a/src/main/deploy/pathplanner/autos/$s!p-c4-c5.auto b/src/main/deploy/pathplanner/autos/$s!p-c4-c5.auto
@@ -23,6 +23,12 @@
             "pathName": "$spc(45)!p-c4"
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "C4"
+          }
+        },
         {
           "type": "named",
           "data": {
@@ -53,6 +59,12 @@
             "pathName": "$spc(45)!c5"
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "C5"
+          }
+        },
         {
           "type": "named",
           "data": {
diff --git a/src/main/deploy/pathplanner/autos/$s!p-c5-c3.auto b/src/main/deploy/pathplanner/autos/$s!p-c5-c3.auto
@@ -23,6 +23,12 @@
             "pathName": "$spc(53)!p-c5"
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "C5"
+          }
+        },
         {
           "type": "named",
           "data": {
@@ -53,6 +59,12 @@
             "pathName": "$spc(53)!c3"
           }
         },
+        {
+          "type": "named",
+          "data": {
+            "name": "C3"
+          }
+        },
         {
           "type": "named",
           "data": {
diff --git a/src/main/java/frc/robot/BuildConstants.java b/src/main/java/frc/robot/BuildConstants.java
@@ -5,12 +5,12 @@ public final class BuildConstants {
   public static final String MAVEN_GROUP = "";
   public static final String MAVEN_NAME = "2024RobotCode";
   public static final String VERSION = "unspecified";
-  public static final int GIT_REVISION = 320;
-  public static final String GIT_SHA = "7e4a0469a390a05483a79d80ab72f641679887d4";
-  public static final String GIT_DATE = "2024-08-16 13:52:21 EDT";
+  public static final int GIT_REVISION = 321;
+  public static final String GIT_SHA = "37cc387fc312ccfeee5787b06b5d201dc503fda5";
+  public static final String GIT_DATE = "2024-08-16 14:13:15 EDT";
   public static final String GIT_BRANCH = "note-filtering";
-  public static final String BUILD_DATE = "2024-08-16 14:12:54 EDT";
-  public static final long BUILD_UNIX_TIME = 1723831974633L;
+  public static final String BUILD_DATE = "2024-08-16 14:41:26 EDT";
+  public static final long BUILD_UNIX_TIME = 1723833686448L;
   public static final int DIRTY = 1;
 
   private BuildConstants() {}
diff --git a/src/main/java/frc/robot/commands/AimbotStatic.java b/src/main/java/frc/robot/commands/AimbotStatic.java
@@ -258,5 +258,6 @@ public boolean isFinished() {
     Logger.recordOutput("i am currently this angle", drive.getRotation().getDegrees());
     return pid.atSetpoint() && shooter.atFlywheelSetpoints() && pivot.atGoal()
         || (Timer.getFPGATimestamp() - startTime > 1.323);
+    // return true;
   }
 }
diff --git a/src/main/java/frc/robot/commands/AlignToNoteAuto.java b/src/main/java/frc/robot/commands/AlignToNoteAuto.java
@@ -50,7 +50,7 @@ public void initialize() {
     led.setState(LED_STATE.FLASHING_RED);
     intake.runRollers(12);
     shooter.setFeedersRPM(500);
-    pivot.setPivotGoal(Constants.PivotConstants.INTAKE_SETPOINT_DEG);
+    pivot.setPivotGoal(Constants.PivotConstants.STOW_SETPOINT_DEG);
     targetNoteLocation = drive.getTargetNoteLocation();
 
     generatedPathCommand =
@@ -85,7 +85,7 @@ public boolean isFinished() {
     Logger.recordOutput("isFinished align note", shooter.seesNote());
     // return false;
     return shooter.seesNote() == NoteState.SENSOR
-        || shooter.seesNote() == NoteState.CURRENT
-        || finished;
+    || shooter.seesNote() == NoteState.CURRENT
+    || finished;
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -59,6 +59,7 @@
 import frc.robot.util.LimelightHelpers.PoseEstimate;
 import frc.robot.util.LimelightHelpers.RawFiducial;
 import frc.robot.util.LocalADStarAK;
+
 import java.util.HashMap;
 import java.util.List;
 import java.util.Optional;
@@ -734,27 +735,25 @@ public Translation2d getTargetNoteLocation() {
     Translation2d fieldCoords = noteLocations.get(getNote());
     boolean useVisionNoteCoords =
         getCachedNoteLocation().getDistance(fieldCoords) < 1.25
-            && getCachedNoteLocation() != null
-            && getCachedNoteTime() != -1
-            && noteImageIsNew();
+            && getCachedNoteLocation() != null && getCachedNoteTime() != -1 && noteImageIsNew();
 
     Logger.recordOutput(
-        "cached note distance to field ", getCachedNoteLocation().getDistance(fieldCoords));
+      "cached note distance to field ", getCachedNoteLocation().getDistance(fieldCoords));
     Logger.recordOutput("use vision note coords", useVisionNoteCoords);
 
     if (useVisionNoteCoords) return visionCoords;
     return fieldCoords;
   }
 
-  public PathPlannerPath generateTrajectoryToNote(
+    public PathPlannerPath generateTrajectoryToNote(
       Translation2d target,
       double maxVelMetersPerSec,
       double maxAccelMetersPerSecSquared,
       double maxAngVelDegPerSec,
       double maxAngAccelDegPerSecSquared,
       double endVelMetersPerSec) {
     Rotation2d targetRotation =
-        new Rotation2d(target.getX() - getPose().getX(), target.getY() - getPose().getY());
+      AllianceFlipUtil.apply(new Rotation2d(target.getX() - getPose().getX(), target.getY() - getPose().getY()));
 
     Logger.recordOutput("Target Note Pose3d", new Pose3d(new Pose2d(target, new Rotation2d())));
     List<Translation2d> points =
@@ -771,7 +770,7 @@ public PathPlannerPath generateTrajectoryToNote(
                 Units.degreesToRadians(maxAngAccelDegPerSecSquared)),
             new GoalEndState(endVelMetersPerSec, targetRotation, true));
 
-    path.preventFlipping = true;
+    path.preventFlipping = false;
 
     return path;
   }

Original file line number	Diff line number	Diff line change
`@@ -258,5 +258,6 @@ public boolean isFinished() {`
`258`	`258`	`Logger.recordOutput("i am currently this angle", drive.getRotation().getDegrees());`
`259`	`259`	`return pid.atSetpoint() && shooter.atFlywheelSetpoints() && pivot.atGoal()`
`260`	`260`	`\|\| (Timer.getFPGATimestamp() - startTime > 1.323);`
	`261`	`+ // return true;`
`261`	`262`	`}`
`262`	`263`	`}`