Friday work

lasarobotics · Apr 6, 2024 · 2ac841d · 2ac841d
1 parent 10ab225
commit 2ac841d
Show file tree

Hide file tree

Showing 13 changed files with 134 additions and 57 deletions.
diff --git a/src/main/deploy/pathplanner/autos/New New Auto.auto b/src/main/deploy/pathplanner/autos/New New Auto.auto
@@ -0,0 +1,37 @@
+{
+  "version": 1.0,
+  "startingPose": {
+    "position": {
+      "x": 1.3674521584632144,
+      "y": 5.555162990025228
+    },
+    "rotation": 0
+  },
+  "command": {
+    "type": "sequential",
+    "data": {
+      "commands": [
+        {
+          "type": "path",
+          "data": {
+            "pathName": "deleteme"
+          }
+        },
+        {
+          "type": "named",
+          "data": {
+            "name": "auto intake"
+          }
+        },
+        {
+          "type": "path",
+          "data": {
+            "pathName": "deleteme"
+          }
+        }
+      ]
+    }
+  },
+  "folder": "Right",
+  "choreoAuto": false
+}
diff --git a/src/main/deploy/pathplanner/paths/CloseMid_CloseTop.path b/src/main/deploy/pathplanner/paths/CloseMid_CloseTop.path
@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 2.6570624081419103,
-        "y": 6.864468657712245
+        "x": 2.809086415893629,
+        "y": 6.913091720920284
       },
       "prevControl": {
-        "x": 2.3535496884404146,
-        "y": 6.575408924663201
+        "x": 2.1184278960036624,
+        "y": 6.409012116269848
       },
       "nextControl": null,
       "isLocked": false,

diff --git a/src/main/deploy/pathplanner/paths/CloseTop_FarTop.path b/src/main/deploy/pathplanner/paths/CloseTop_FarTop.path
@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 2.6570624081419103,
-        "y": 6.864468657712245
+        "x": 2.809086415893629,
+        "y": 6.913091720920284
       },
       "prevControl": null,
       "nextControl": {
-        "x": 2.868472675406889,
-        "y": 6.891750052645405
+        "x": 3.020496683158608,
+        "y": 6.940373115853444
       },
       "isLocked": false,
       "linkedName": "Close_Top"

diff --git a/src/main/deploy/pathplanner/paths/CloseTop_FarTop2.path b/src/main/deploy/pathplanner/paths/CloseTop_FarTop2.path
@@ -3,13 +3,13 @@
   "waypoints": [
     {
       "anchor": {
-        "x": 2.6570624081419103,
-        "y": 6.864468657712245
+        "x": 2.809086415893629,
+        "y": 6.913091720920284
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.445494610287466,
-        "y": 6.985103093013204
+        "x": 3.5975186180391847,
+        "y": 7.033726156221243
       },
       "isLocked": false,
       "linkedName": "Close_Top"

diff --git a/src/main/deploy/pathplanner/paths/Left_CloseTop.path b/src/main/deploy/pathplanner/paths/Left_CloseTop.path
@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 2.6570624081419103,
-        "y": 6.864468657712245
+        "x": 2.809086415893629,
+        "y": 6.913091720920284
       },
       "prevControl": {
-        "x": 2.5387822570044087,
-        "y": 6.855616648370379
+        "x": 2.6908062647561275,
+        "y": 6.904239711578418
       },
       "nextControl": null,
       "isLocked": false,

diff --git a/src/main/deploy/pathplanner/paths/Right_FarAutoIntake.path b/src/main/deploy/pathplanner/paths/Right_FarAutoIntake.path
@@ -16,12 +16,12 @@
     },
     {
       "anchor": {
-        "x": 6.552440037098384,
-        "y": 0.7662341204964105
+        "x": 7.385545397657212,
+        "y": 0.7818377541516989
       },
       "prevControl": {
-        "x": 4.161179197358394,
-        "y": 1.1300653648842527
+        "x": 4.994284557917222,
+        "y": 1.145668998539541
       },
       "nextControl": null,
       "isLocked": false,

diff --git a/src/main/deploy/pathplanner/paths/Stage_FarMidBottom_AutoIntake.path b/src/main/deploy/pathplanner/paths/Stage_FarMidBottom_AutoIntake.path
@@ -8,20 +8,20 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.662576647336394,
-        "y": 2.126282747283805
+        "x": 4.17589567008708,
+        "y": 1.6974109136188202
       },
       "isLocked": false,
       "linkedName": "RightStage"
     },
     {
       "anchor": {
-        "x": 6.726948559530921,
-        "y": 2.120693506351802
+        "x": 7.5089934641022165,
+        "y": 2.2117778571396745
       },
       "prevControl": {
-        "x": 5.4408585753791225,
-        "y": 1.785169679494289
+        "x": 6.222903479950418,
+        "y": 1.8762540302821615
       },
       "nextControl": null,
       "isLocked": false,

diff --git a/src/main/deploy/pathplanner/paths/deleteme.path b/src/main/deploy/pathplanner/paths/deleteme.path
@@ -0,0 +1,52 @@
+{
+  "version": 1.0,
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 1.3160154641111295,
+        "y": 5.58602500663648
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 2.316015464111129,
+        "y": 5.58602500663648
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 1.460038208296968,
+        "y": 5.58602500663648
+      },
+      "prevControl": {
+        "x": 0.46003820829696807,
+        "y": 5.58602500663648
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 0.5,
+    "maxAcceleration": 4.0,
+    "maxAngularVelocity": 2160.0,
+    "maxAngularAcceleration": 2160.0
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0,
+    "rotateFast": false
+  },
+  "reversed": false,
+  "folder": null,
+  "previewStartingState": {
+    "rotation": 0,
+    "velocity": 0
+  },
+  "useDefaultConstraints": false
+}
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
@@ -160,13 +160,13 @@ public static class Shooter {
       Units.DegreesPerSecond.of(360.0 * 10).per(Units.Second)
     );
     public static final List<Entry<Measure<Distance>,State>> SHOOTER_MAP = Arrays.asList(
-      Map.entry(Units.Meters.of(0.00), new State(Units.MetersPerSecond.of(14.90),    Units.Degrees.of(53.0))),
-      Map.entry(Units.Meters.of(1.00), new State(Units.MetersPerSecond.of(14.94),    Units.Degrees.of(53.0))),
-      Map.entry(Units.Meters.of(1.50), new State(Units.MetersPerSecond.of(15.00),    Units.Degrees.of(53.0))),
-      Map.entry(Units.Meters.of(2.00), new State(Units.MetersPerSecond.of(15.00781), Units.Degrees.of(44.5))),
-      Map.entry(Units.Meters.of(2.50), new State(Units.MetersPerSecond.of(15.10964), Units.Degrees.of(38.0))),
-      Map.entry(Units.Meters.of(3.00), new State(Units.MetersPerSecond.of(15.50),    Units.Degrees.of(33.5))),
-      Map.entry(Units.Meters.of(3.50), new State(Units.MetersPerSecond.of(16.25786), Units.Degrees.of(31.0))),
+      Map.entry(Units.Meters.of(0.00), new State(Units.MetersPerSecond.of(14.90),    Units.Degrees.of(54.0))),
+      Map.entry(Units.Meters.of(1.00), new State(Units.MetersPerSecond.of(14.94),    Units.Degrees.of(54.0))),
+      Map.entry(Units.Meters.of(1.50), new State(Units.MetersPerSecond.of(15.00),    Units.Degrees.of(54.0))),
+      Map.entry(Units.Meters.of(2.00), new State(Units.MetersPerSecond.of(15.00781), Units.Degrees.of(45.0))),
+      Map.entry(Units.Meters.of(2.50), new State(Units.MetersPerSecond.of(15.10964), Units.Degrees.of(39.0))),
+      Map.entry(Units.Meters.of(3.00), new State(Units.MetersPerSecond.of(15.50),    Units.Degrees.of(35.5))),
+      Map.entry(Units.Meters.of(3.50), new State(Units.MetersPerSecond.of(16.25786), Units.Degrees.of(33.0))),
       Map.entry(Units.Meters.of(4.00), new State(Units.MetersPerSecond.of(17.00),    Units.Degrees.of(28.5))),
       Map.entry(Units.Meters.of(4.50), new State(Units.MetersPerSecond.of(17.34737), Units.Degrees.of(25.5))),
       Map.entry(Units.Meters.of(5.00), new State(Units.MetersPerSecond.of(17.40),    Units.Degrees.of(25.0))),

diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
@@ -319,7 +319,7 @@ private Command aimAndIntakeObjectCommand() {
     //       false).until(() -> VISION_SUBSYSTEM.shouldIntake()),
     //  Commands.parallel(
       DRIVE_SUBSYSTEM.aimAtPointRobotCentricCommand(
-        () -> VISION_SUBSYSTEM.shouldIntake() ? 0.75 : 0,
+        () -> VISION_SUBSYSTEM.shouldIntake() ? -0.75 : 0,
         () -> 0,
         () -> 0,
         () -> {
@@ -380,6 +380,7 @@ private void autoModeChooser() {
     m_automodeChooser.addOption(Constants.AutoNames.LEFT_WAIT_FARTOP_AUTO_NAME.getFirst(), new AutoTrajectory(DRIVE_SUBSYSTEM, Constants.AutoNames.LEFT_WAIT_FARTOP_AUTO_NAME.getSecond()).getCommand());
     m_automodeChooser.addOption(Constants.AutoNames.RIGHT_FARDISRUPT_FARTOP_AUTO_NAME.getFirst(), new AutoTrajectory(DRIVE_SUBSYSTEM, Constants.AutoNames.RIGHT_FARDISRUPT_FARTOP_AUTO_NAME.getSecond()).getCommand());
     m_automodeChooser.addOption(Constants.AutoNames.PRELOAD_AUTO_NAME, SHOOTER_SUBSYSTEM.shootSpeakerCommand().withTimeout(2.5));
+    m_automodeChooser.addOption("deleteme", new AutoTrajectory(DRIVE_SUBSYSTEM, "New New Auto").getCommand());
   }
 
   /**

diff --git a/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java b/src/main/java/frc/robot/subsystems/drive/DriveSubsystem.java
@@ -891,6 +891,10 @@ public void setAlliance(Alliance alliance) {
     }
   }
 
+  /**
+   * Get current alliance
+   * @return Alliance
+   */
   public Alliance getAlliance() {
     return m_currentAlliance;
   }
@@ -968,23 +972,6 @@ public Command aimAtPointCommand(DoubleSupplier xRequestSupplier, DoubleSupplier
     );
   }
 
-  public Command aimAtPointCommand(DoubleSupplier xRequestSupplier, DoubleSupplier yRequestSupplier, DoubleSupplier rotateRequestSupplier,
-                                   Supplier<Translation2d> pointSupplier, boolean reversed, boolean velocityCorrection, boolean aimCorrection) {
-    return runEnd(
-      () -> aimAtPoint(
-        m_controlCentricity,
-        xRequestSupplier.getAsDouble(),
-        yRequestSupplier.getAsDouble(),
-        rotateRequestSupplier.getAsDouble(),
-        pointSupplier.get(),
-        aimCorrection ? m_aimOffset : new Translation2d(),
-        reversed,
-        velocityCorrection
-      ),
-      () -> resetRotatePID()
-    );
-  }
-
   /**
    * Aim robot at desired point on the field, while strafing
    * @param xRequestSupplier X axis speed supplier [-1.0, +1.0]

diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java b/src/main/java/frc/robot/subsystems/shooter/ShooterSubsystem.java
@@ -75,13 +75,13 @@ public static class State {
     public final Measure<Velocity<Distance>> speed;
     public final Measure<Angle> angle;
 
-    public static final State AMP_PREP_STATE = new State(ZERO_FLYWHEEL_SPEED, Units.Degrees.of(56.5));
-    public static final State AMP_SCORE_STATE = new State(Units.MetersPerSecond.of(+3.2), Units.Degrees.of(56.5));
-    public static final State SPEAKER_PREP_STATE = new State(ZERO_FLYWHEEL_SPEED, Units.Degrees.of(53.0));
-    public static final State SPEAKER_SCORE_STATE = new State(Units.MetersPerSecond.of(+15.0), Units.Degrees.of(53.0));
+    public static final State AMP_PREP_STATE = new State(ZERO_FLYWHEEL_SPEED, Units.Degrees.of(56.0));
+    public static final State AMP_SCORE_STATE = new State(Units.MetersPerSecond.of(+3.1), Units.Degrees.of(56.0));
+    public static final State SPEAKER_PREP_STATE = new State(ZERO_FLYWHEEL_SPEED, Units.Degrees.of(55.0));
+    public static final State SPEAKER_SCORE_STATE = new State(Units.MetersPerSecond.of(+15.0), Units.Degrees.of(55.0));
     public static final State SOURCE_PREP_STATE = new State(ZERO_FLYWHEEL_SPEED, Units.Degrees.of(55.0));
     public static final State SOURCE_INTAKE_STATE = new State(Units.MetersPerSecond.of(-10.0), Units.Degrees.of(55.0));
-    public static final State PASSING_STATE = new State(Units.MetersPerSecond.of(+16.0), Units.Degrees.of(45.0));
+    public static final State PASSING_STATE = new State(Units.MetersPerSecond.of(+15.5), Units.Degrees.of(45.0));
     public static final State TEST_STOP_STATE = new State(ZERO_FLYWHEEL_SPEED, Units.Degrees.of(30.0));
 
     public State(Measure<Velocity<Distance>> speed, Measure<Angle> angle) {

diff --git a/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java b/src/main/java/frc/robot/subsystems/vision/VisionSubsystem.java
@@ -56,7 +56,7 @@ public Hardware(AprilTagCamera... cameras) {
   private static final String OBJECT_POSE_LOG_ENTRY = "/ObjectPose";
   private static final String OBJECT_DETECTED_LOG_ENTRY = "/ObjectDetected";
 
-  private static final double INTAKE_YAW_TOLERANCE = 1;
+  private static final double INTAKE_YAW_TOLERANCE = 10;
 
   private AtomicReference<List<AprilTagCameraResult>> m_estimatedRobotPoses;
   private AtomicReference<List<AprilTag>> m_visibleTags;