Repository: RAR1741/RA25_RobotCode
Pull request #35: State
Author: login: kryllyxofficial01
Created: 2025-04-06T20:38:00Z | Merged: 2025-04-08T23:20:53Z

holy crap we're going to worlds


Diff:
diff --git a/AdvantageScope.json b/AdvantageScope.json
index a6d3886..46ea330 100644
--- a/AdvantageScope.json
+++ b/AdvantageScope.json
@@ -18,7 +18,6 @@
             "/RealOutputs/ASPoseHelper/LimelightCheckPose/translation",
             "/RealOutputs/ASPoseHelper/LimelightCheckPose/rotation",
             "/RealOutputs/Odometry/Limelight/Pose/rotation",
-            "/RealOutputs/Odometry/Limelight",
             "/RealOutputs/Odometry/Limelight/limelight-front",
             "/RealOutputs/Odometry/Limelight/limelight-front/RawPoseEstimation",
             "/RealOutputs/LaserCans/Entrance",
@@ -31,7 +30,6 @@
             "/RealOutputs/ASPoseHelper/PoseAligner/GoalPose",
             "/RealOutputs/ASPoseHelper/Red/Reef",
             "/RealOutputs/ASPoseHelper/Red/Reef/targets",
-            "/RealOutputs/OperatorController",
             "/RealOutputs/SysID",
             "/RealOutputs/SysID/drive",
             "/RealOutputs/SwerveDrive/Modules/FL/AbsoluteEncoder",
@@ -40,7 +38,9 @@
             "/RealOutputs/SwerveDrive/Modules/BL/AbsoluteEncoder",
             "/RealOutputs/LaserCans/Exit",
             "/RealOutputs/Intakes/Right",
-            "/RealOutputs/Hopper"
+            "/RealOutputs/EndEffector/Position",
+            "/RealOutputs/Odometry",
+            "/RealOutputs/ASPoseHelper/Red"
           ]
         },
         "tabs": {
@@ -287,18 +287,68 @@
                 "cameraIndex": -1,
                 "orbitFov": 50,
                 "cameraPosition": [
-                  -10.089997247474917,
-                  10.27788622425322,
-                  0.4781192794940041
+                  -7.319288299989129,
+                  9.52346144067519,
+                  0.12265759074077881
                 ],
                 "cameraTarget": [
-                  -2.378076749845899,
-                  -4.943052211825455,
-                  0.48926170872458286
+                  -4.500154969510189,
+                  -3.16339397123331,
+                  0.12525163873521075
                 ]
               },
               "controlsHeight": 295
             },
+            {
+              "type": 2,
+              "title": "2D",
+              "controller": {
+                "sources": [
+                  {
+                    "type": "ghost",
+                    "logKey": "/RealOutputs/Odometry/PoseEstimator/Pose2d",
+                    "logType": "Pose2d",
+                    "visible": true,
+                    "options": {
+                      "color": "#ff0000"
+                    }
+                  },
+                  {
+                    "type": "ghost",
+                    "logKey": "/RealOutputs/Odometry/GoalPose",
+                    "logType": "Pose2d",
+                    "visible": true,
+                    "options": {
+                      "color": "#ff00ff"
+                    }
+                  },
+                  {
+                    "type": "arrow",
+                    "logKey": "/RealOutputs/ASPoseHelper/Red/Reef/pose",
+                    "logType": "Pose3d",
+                    "visible": true,
+                    "options": {
+                      "position": "center"
+                    }
+                  },
+                  {
+                    "type": "robot",
+                    "logKey": "/RealOutputs/ASPoseHelper/Red/Reef/targets",
+                    "logType": "Pose2d[]",
+                    "visible": true,
+                    "options": {}
+                  }
+                ],
+                "game": "2025 Field (Welded)",
+                "bumpers": "auto",
+                "origin": "blue",
+                "orientation": 0,
+                "size": 30
+              },
+              "controllerUUID": "nwnoogx8bu7m474o1snvizdej2hshing",
+              "renderer": null,
+              "controlsHeight": 200
+            },
             {
               "type": 1,
               "title": "AutoAlign",
@@ -318,11 +368,11 @@
                 "rightSources": [
                   {
                     "type": "stepped",
-                    "logKey": "/RealOutputs/AutoAligner/targetSpeed",
+                    "logKey": "/RealOutputs/AutoAligner/TargetSpeed",
                     "logType": "Number",
                     "visible": false,
                     "options": {
-                      "color": "#2b66a2",
+                      "color": "#e48b32",
                       "size": "normal"
                     }
                   },
@@ -1013,7 +1063,7 @@
                     "type": "stepped",
                     "logKey": "/RealOutputs/LaserCans/Entrance/distance",
                     "logType": "Number",
-                    "visible": true,
+                    "visible": false,
                     "options": {
                       "color": "#e5b31b",
                       "size": "normal"
@@ -1023,7 +1073,7 @@
                     "type": "stepped",
                     "logKey": "/RealOutputs/LaserCans/Exit/distance",
                     "logType": "Number",
-                    "visible": true,
+                    "visible": false,
                     "options": {
                       "color": "#af2437",
                       "size": "normal"
@@ -1033,7 +1083,7 @@
                     "type": "stepped",
                     "logKey": "/RealOutputs/EndEffector/LeftMotorVelocityRPM",
                     "logType": "Number",
-                    "visible": false,
+                    "visible": true,
                     "options": {
                       "color": "#3b875a",
                       "size": "normal"
@@ -1043,11 +1093,21 @@
                     "type": "stepped",
                     "logKey": "/RealOutputs/EndEffector/RightMotorVelocityRPM",
                     "logType": "Number",
-                    "visible": false,
+                    "visible": true,
                     "options": {
                       "color": "#d993aa",
                       "size": "normal"
                     }
+                  },
+                  {
+                    "type": "stepped",
+                    "logKey": "/RealOutputs/EndEffector/Position/Target",
+                    "logType": "Number",
+                    "visible": true,
+                    "options": {
+                      "color": "#5f4528",
+                      "size": "normal"
+                    }
                   }
                 ],
                 "discreteSources": [
@@ -1094,7 +1154,7 @@
               },
               "controllerUUID": "hasxa3gq788i8l063bz5gsr2bxjqbgm3",
               "renderer": null,
-              "controlsHeight": 310
+              "controlsHeight": 398
             },
             {
               "type": 1,
diff --git a/elastic-layout.json b/elastic-layout.json
new file mode 100644
index 0000000..c8640b3
--- /dev/null
+++ b/elastic-layout.json
@@ -0,0 +1,252 @@
+{
+  "version": 1.0,
+  "grid_size": 128,
+  "tabs": [
+    {
+      "name": "Teleoperated",
+      "grid_layout": {
+        "layouts": [],
+        "containers": [
+          {
+            "title": "limelight-front",
+            "x": 0.0,
+            "y": 0.0,
+            "width": 1024.0,
+            "height": 768.0,
+            "type": "Camera Stream",
+            "properties": {
+              "topic": "/CameraPublisher/limelight-front",
+              "period": 0.06,
+              "rotation_turns": 0
+            }
+          },
+          {
+            "title": "Has Telly Tasks",
+            "x": 1024.0,
+            "y": 512.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/TaskScheduler/hasAnyTasks",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "Arm",
+            "x": 1024.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "Large Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/Arm/State",
+              "period": 0.06,
+              "data_type": "string"
+            }
+          },
+          {
+            "title": "End Effector",
+            "x": 1024.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "Large Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/EndEffector/State",
+              "period": 0.06,
+              "data_type": "string"
+            }
+          },
+          {
+            "title": "Hopper",
+            "x": 1280.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "Large Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/Hopper/State",
+              "period": 0.06,
+              "data_type": "string"
+            }
+          },
+          {
+            "title": "Elevator",
+            "x": 1536.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "Large Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/Elevator/ElevatorState",
+              "period": 0.06,
+              "data_type": "string"
+            }
+          },
+          {
+            "title": "Intake Left",
+            "x": 1280.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "Large Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/Intakes/Left/Desired/IntakeState",
+              "period": 0.06,
+              "data_type": "string"
+            }
+          },
+          {
+            "title": "Intake Right",
+            "x": 1536.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "Large Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/Intakes/Right/Desired/IntakeState",
+              "period": 0.06,
+              "data_type": "string"
+            }
+          },
+          {
+            "title": "Entrance Laser",
+            "x": 1152.0,
+            "y": 512.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/LaserCans/Entrance/seesCoral",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "Exit Laser",
+            "x": 1408.0,
+            "y": 512.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/LaserCans/Exit/seesCoral",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "Middle Laser",
+            "x": 1280.0,
+            "y": 512.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/LaserCans/Middle/seesCoral",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "Batt Volts",
+            "x": 1024.0,
+            "y": 640.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "Radial Gauge",
+            "properties": {
+              "topic": "/AdvantageKit/SystemStats/BatteryVoltage",
+              "period": 0.033,
+              "data_type": "double",
+              "start_angle": -140.0,
+              "end_angle": 140.0,
+              "min_value": 6.0,
+              "max_value": 14.0,
+              "number_of_labels": 8,
+              "wrap_value": false,
+              "show_pointer": true,
+              "show_ticks": true
+            }
+          }
+        ]
+      }
+    },
+    {
+      "name": "Autonomous",
+      "grid_layout": {
+        "layouts": [],
+        "containers": [
+          {
+            "title": "Field",
+            "x": 0.0,
+            "y": 0.0,
+            "width": 1792.0,
+            "height": 768.0,
+            "type": "Field",
+            "properties": {
+              "topic": "/SmartDashboard/Field",
+              "period": 0.06,
+              "field_game": "Reefscape",
+              "robot_width": 0.85,
+              "robot_length": 0.85,
+              "show_other_objects": true,
+              "show_trajectories": true,
+              "field_rotation": 0.0,
+              "robot_color": 4294198070,
+              "trajectory_color": 4294967295
+            }
+          },
+          {
+            "title": "IsSafeToScore",
+            "x": 384.0,
+            "y": 768.0,
+            "width": 1408.0,
+            "height": 256.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/EndEffector/IsSafeToScore",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "Auto picker",
+            "x": 0.0,
+            "y": 768.0,
+            "width": 384.0,
+            "height": 256.0,
+            "type": "ComboBox Chooser",
+            "properties": {
+              "topic": "/SmartDashboard/Auto picker",
+              "period": 0.06,
+              "sort_options": false
+            }
+          }
+        ]
+      }
+    }
+  ]
+}
\ No newline at end of file
diff --git a/elastic-layout_hunter.json b/elastic-layout_hunter.json
new file mode 100644
index 0000000..b9f49e3
--- /dev/null
+++ b/elastic-layout_hunter.json
@@ -0,0 +1,265 @@
+{
+  "version": 1.0,
+  "grid_size": 128,
+  "tabs": [
+    {
+      "name": "Teleoperated",
+      "grid_layout": {
+        "layouts": [],
+        "containers": [
+          {
+            "title": "limelight-front",
+            "x": 0.0,
+            "y": 0.0,
+            "width": 768.0,
+            "height": 384.0,
+            "type": "Camera Stream",
+            "properties": {
+              "topic": "/CameraPublisher/limelight-front",
+              "period": 0.06,
+              "rotation_turns": 0
+            }
+          },
+          {
+            "title": "Has Tele Tasks",
+            "x": 1024.0,
+            "y": 256.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/TaskScheduler/hasAnyTasks",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "Arm",
+            "x": 1280.0,
+            "y": 128.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Large Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/Arm/State",
+              "period": 0.06,
+              "data_type": "string"
+            }
+          },
+          {
+            "title": "End Effector",
+            "x": 768.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "Large Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/EndEffector/State",
+              "period": 0.06,
+              "data_type": "string"
+            }
+          },
+          {
+            "title": "Hopper",
+            "x": 1024.0,
+            "y": 128.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Large Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/Hopper/State",
+              "period": 0.06,
+              "data_type": "string"
+            }
+          },
+          {
+            "title": "Elevator",
+            "x": 768.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "Large Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/Elevator/ElevatorState",
+              "period": 0.06,
+              "data_type": "string"
+            }
+          },
+          {
+            "title": "Intake Left",
+            "x": 1024.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Large Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/Intakes/Left/Desired/IntakeState",
+              "period": 0.06,
+              "data_type": "string"
+            }
+          },
+          {
+            "title": "Intake Right",
+            "x": 1280.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Large Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/Intakes/Right/Desired/IntakeState",
+              "period": 0.06,
+              "data_type": "string"
+            }
+          },
+          {
+            "title": "Entrance Laser",
+            "x": 1152.0,
+            "y": 256.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/LaserCans/Entrance/seesCoral",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "Exit Laser",
+            "x": 1408.0,
+            "y": 256.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/LaserCans/Exit/seesCoral",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "Middle Laser",
+            "x": 1280.0,
+            "y": 256.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/LaserCans/Middle/seesCoral",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "Battery Volts",
+            "x": 0.0,
+            "y": 384.0,
+            "width": 768.0,
+            "height": 128.0,
+            "type": "Number Bar",
+            "properties": {
+              "topic": "/AdvantageKit/SystemStats/BatteryVoltage",
+              "period": 0.033,
+              "data_type": "double",
+              "min_value": 6.0,
+              "max_value": 14.0,
+              "divisions": 5,
+              "inverted": false,
+              "orientation": "horizontal"
+            }
+          },
+          {
+            "title": "MatchTime",
+            "x": 1024.0,
+            "y": 384.0,
+            "width": 512.0,
+            "height": 128.0,
+            "type": "Match Time",
+            "properties": {
+              "topic": "/AdvantageKit/DriverStation/MatchTime",
+              "period": 0.06,
+              "data_type": "double",
+              "time_display_mode": "Minutes and Seconds",
+              "red_start_time": 15,
+              "yellow_start_time": 30
+            }
+          }
+        ]
+      }
+    },
+    {
+      "name": "Autonomous",
+      "grid_layout": {
+        "layouts": [],
+        "containers": [
+          {
+            "title": "Field",
+            "x": 0.0,
+            "y": 0.0,
+            "width": 1536.0,
+            "height": 384.0,
+            "type": "Field",
+            "properties": {
+              "topic": "/SmartDashboard/Field",
+              "period": 0.06,
+              "field_game": "Reefscape",
+              "robot_width": 0.85,
+              "robot_length": 0.85,
+              "show_other_objects": true,
+              "show_trajectories": true,
+              "field_rotation": 0.0,
+              "robot_color": 4294198070,
+              "trajectory_color": 4294967295
+            }
+          },
+          {
+            "title": "IsSafeToScore",
+            "x": 384.0,
+            "y": 384.0,
+            "width": 1152.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/EndEffector/IsSafeToScore",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "Auto picker",
+            "x": 0.0,
+            "y": 384.0,
+            "width": 384.0,
+            "height": 128.0,
+            "type": "ComboBox Chooser",
+            "properties": {
+              "topic": "/SmartDashboard/Auto picker",
+              "period": 0.06,
+              "sort_options": false
+            }
+          }
+        ]
+      }
+    }
+  ]
+}
\ No newline at end of file
diff --git a/elastic-layout_lazy-notification-fix.json b/elastic-layout_lazy-notification-fix.json
new file mode 100644
index 0000000..71d6a70
--- /dev/null
+++ b/elastic-layout_lazy-notification-fix.json
@@ -0,0 +1,278 @@
+{
+  "version": 1.0,
+  "grid_size": 128,
+  "tabs": [
+    {
+      "name": "Teleoperated",
+      "grid_layout": {
+        "layouts": [],
+        "containers": [
+          {
+            "title": "limelight-front",
+            "x": 0.0,
+            "y": 0.0,
+            "width": 768.0,
+            "height": 384.0,
+            "type": "Camera Stream",
+            "properties": {
+              "topic": "/CameraPublisher/limelight-front",
+              "period": 0.06,
+              "rotation_turns": 0
+            }
+          },
+          {
+            "title": "Has Tele Tasks",
+            "x": 1024.0,
+            "y": 256.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/TaskScheduler/hasAnyTasks",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "Arm",
+            "x": 1280.0,
+            "y": 128.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Large Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/Arm/State",
+              "period": 0.06,
+              "data_type": "string"
+            }
+          },
+          {
+            "title": "End Effector",
+            "x": 768.0,
+            "y": 256.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "Large Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/EndEffector/State",
+              "period": 0.06,
+              "data_type": "string"
+            }
+          },
+          {
+            "title": "Hopper",
+            "x": 1024.0,
+            "y": 128.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Large Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/Hopper/State",
+              "period": 0.06,
+              "data_type": "string"
+            }
+          },
+          {
+            "title": "Elevator",
+            "x": 768.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 256.0,
+            "type": "Large Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/Elevator/ElevatorState",
+              "period": 0.06,
+              "data_type": "string"
+            }
+          },
+          {
+            "title": "Intake Left",
+            "x": 1024.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Large Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/Intakes/Left/Desired/IntakeState",
+              "period": 0.06,
+              "data_type": "string"
+            }
+          },
+          {
+            "title": "Intake Right",
+            "x": 1280.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 128.0,
+            "type": "Large Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/Intakes/Right/Desired/IntakeState",
+              "period": 0.06,
+              "data_type": "string"
+            }
+          },
+          {
+            "title": "Entrance Laser",
+            "x": 1152.0,
+            "y": 256.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/LaserCans/Entrance/seesCoral",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "Exit Laser",
+            "x": 1408.0,
+            "y": 256.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/LaserCans/Exit/seesCoral",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "Middle Laser",
+            "x": 1280.0,
+            "y": 256.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/LaserCans/Middle/seesCoral",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "Battery Volts",
+            "x": 0.0,
+            "y": 384.0,
+            "width": 640.0,
+            "height": 128.0,
+            "type": "Number Bar",
+            "properties": {
+              "topic": "/AdvantageKit/SystemStats/BatteryVoltage",
+              "period": 0.033,
+              "data_type": "double",
+              "min_value": 6.0,
+              "max_value": 14.0,
+              "divisions": 5,
+              "inverted": false,
+              "orientation": "horizontal"
+            }
+          },
+          {
+            "title": "MatchTime",
+            "x": 1024.0,
+            "y": 384.0,
+            "width": 512.0,
+            "height": 128.0,
+            "type": "Match Time",
+            "properties": {
+              "topic": "/AdvantageKit/DriverStation/MatchTime",
+              "period": 0.06,
+              "data_type": "double",
+              "time_display_mode": "Minutes and Seconds",
+              "red_start_time": 15,
+              "yellow_start_time": 30
+            }
+          },
+          {
+            "title": "DesiredElevatorState",
+            "x": 640.0,
+            "y": 384.0,
+            "width": 128.0,
+            "height": 128.0,
+            "type": "Large Text Display",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/OperatorController/DesiredElevatorState",
+              "period": 0.06,
+              "data_type": "string"
+            }
+          }
+        ]
+      }
+    },
+    {
+      "name": "Autonomous",
+      "grid_layout": {
+        "layouts": [],
+        "containers": [
+          {
+            "title": "Field",
+            "x": 0.0,
+            "y": 0.0,
+            "width": 1536.0,
+            "height": 384.0,
+            "type": "Field",
+            "properties": {
+              "topic": "/SmartDashboard/Field",
+              "period": 0.06,
+              "field_game": "Reefscape",
+              "robot_width": 0.85,
+              "robot_length": 0.85,
+              "show_other_objects": true,
+              "show_trajectories": true,
+              "field_rotation": 0.0,
+              "robot_color": 4294198070,
+              "trajectory_color": 4294967295
+            }
+          },
+          {
+            "title": "IsSafeToScore",
+            "x": 384.0,
+            "y": 384.0,
+            "width": 1152.0,
+            "height": 128.0,
+            "type": "Boolean Box",
+            "properties": {
+              "topic": "/AdvantageKit/RealOutputs/EndEffector/IsSafeToScore",
+              "period": 0.06,
+              "data_type": "boolean",
+              "true_color": 4283215696,
+              "false_color": 4294198070,
+              "true_icon": "None",
+              "false_icon": "None"
+            }
+          },
+          {
+            "title": "Auto picker",
+            "x": 0.0,
+            "y": 384.0,
+            "width": 384.0,
+            "height": 128.0,
+            "type": "ComboBox Chooser",
+            "properties": {
+              "topic": "/SmartDashboard/Auto picker",
+              "period": 0.06,
+              "sort_options": false
+            }
+          }
+        ]
+      }
+    }
+  ]
+}
\ No newline at end of file
diff --git a/limelight_pipelines/limelightfront-monarch.vpr b/limelight_pipelines/limelightfront-monarch.vpr
index 0aa9701..f61eda9 100644
--- a/limelight_pipelines/limelightfront-monarch.vpr
+++ b/limelight_pipelines/limelightfront-monarch.vpr
@@ -1 +1 @@
-{"area_max":100.0,"area_min":0.001,"area_similarity":0.0,"aspect_max":20.0,"aspect_min":0.0,"barcode_type":"qrzx","black_level":0,"blue_balance":1975.0,"botfloorsnap":1,"botlength":0.8128,"bottype":"swerve","botwidth":0.7112,"calibration_type":0,"classifier_conf":0.1,"classifier_runtime":"cpu","clip_labels":"face,hand,computer,keyboard","contour_grouping":3,"contour_sort_final":0,"convexity_max":100.0,"convexity_min":10.0,"corner_approx":5.0,"crop_focus":0.0,"crop_x_max":1.0,"crop_x_min":-1.0,"crop_y_max":1.0,"crop_y_min":-1.0,"cross_a_a":1.0,"cross_a_x":0.0,"cross_a_y":0.0,"cross_b_a":1.0,"cross_b_x":0.0,"cross_b_y":0.0,"debugpipe":0,"depthrange_max":100.0,"depthrange_min":0.0,"desc":"limelightfront","desired_contour_region":0,"detector_conf":0.8,"detector_idfilters":"","detector_runtime":"cpu","dilation_steps":0,"direction_filter":0,"dual_close_sort_origin":0,"erosion_steps":0,"exposure":200.0,"fiducial_backend":"umich","fiducial_denoise":0.0,"fiducial_idfilters":"6,7,8,9,10,11,17,18,19,20,21,22","fiducial_locfilters":"","fiducial_qualitythreshold":2.0,"fiducial_resdiv":2,"fiducial_size":165.1,"fiducial_skip3d":0,"fiducial_type":"aprilClassic36h11","fiducial_vis_mode":"3dbotposefieldspace","flicker":2,"force_convex":1,"hue_max":85,"hue_min":55,"image_flip":0,"image_source":0,"img_to_show":0,"intersection_filter":0,"invert_hue":0,"lcgain":1.0,"margin_tv":0.2,"multigroup_max":7,"multigroup_min":1,"multigroup_rejector":0,"nnp_rotate":0,"pipeline_led_enabled":0,"pipeline_led_power":100,"pipeline_res":0,"pipeline_type":"pipe_fiducial","python_snapscript_name":"","quality_focus":0.3,"red_balance":1200.0,"reverse_morpho":0,"roi_x":0.0,"roi_y":0.0,"rsf":0.38735,"rspitch":0.0,"rsroll":0.0,"rss":0.19685,"rsu":0.276225,"rsyaw":12.5,"sat_max":255,"sat_min":70,"send_corners":0,"send_json":0,"tsf":0.0,"tss":0.0,"tsu":0.0,"tv_conf":0.5,"val_max":255,"val_min":70,"x_outlier_miqr":1.5,"y_outlier_miqr":1.5,"yaw_latency_adjustment":0.0,"zsclassifier_conf":0.1}
\ No newline at end of file
+{"area_max":100.0,"area_min":0.001,"area_similarity":0.0,"aspect_max":20.0,"aspect_min":0.0,"barcode_type":"qrzx","black_level":0,"blue_balance":1975.0,"botfloorsnap":1,"botlength":0.8128,"bottype":"swerve","botwidth":0.7112,"calibration_type":0,"classifier_conf":0.1,"classifier_runtime":"cpu","clip_labels":"face,hand,computer,keyboard","contour_grouping":3,"contour_sort_final":0,"convexity_max":100.0,"convexity_min":10.0,"corner_approx":5.0,"crop_focus":0.0,"crop_x_max":1.0,"crop_x_min":-1.0,"crop_y_max":1.0,"crop_y_min":-1.0,"cross_a_a":1.0,"cross_a_x":0.0,"cross_a_y":0.0,"cross_b_a":1.0,"cross_b_x":0.0,"cross_b_y":0.0,"debugpipe":0,"depthrange_max":100.0,"depthrange_min":0.0,"desc":"limelightfront","desired_contour_region":0,"detector_conf":0.8,"detector_idfilters":"","detector_runtime":"cpu","dilation_steps":0,"direction_filter":0,"dual_close_sort_origin":0,"erosion_steps":0,"exposure":200.0,"fiducial_backend":"umich","fiducial_denoise":0.0,"fiducial_idfilters":"6,7,8,9,10,11,17,18,19,20,21,22","fiducial_locfilters":"","fiducial_qualitythreshold":2.0,"fiducial_resdiv":2,"fiducial_size":165.1,"fiducial_skip3d":0,"fiducial_type":"aprilClassic36h11","fiducial_vis_mode":"3dbotposefieldspace","flicker":2,"force_convex":1,"hue_max":85,"hue_min":55,"image_flip":0,"image_source":0,"img_to_show":0,"intersection_filter":0,"invert_hue":0,"lcgain":1.0,"margin_tv":0.2,"multigroup_max":7,"multigroup_min":1,"multigroup_rejector":0,"nnp_rotate":0,"pipeline_led_enabled":0,"pipeline_led_power":100,"pipeline_res":0,"pipeline_type":"pipe_fiducial","python_snapscript_name":"","quality_focus":0.3,"red_balance":1200.0,"reverse_morpho":0,"roi_x":0.0,"roi_y":0.0,"rsf":0.25,"rspitch":-15.0,"rsroll":8.8,"rss":0.23,"rsu":0.42,"rsyaw":30.0,"sat_max":255,"sat_min":70,"send_corners":0,"send_json":0,"tsf":0.0,"tss":0.0,"tsu":0.0,"tv_conf":0.5,"val_max":255,"val_min":70,"x_outlier_miqr":1.5,"y_outlier_miqr":1.5,"yaw_latency_adjustment":0.0,"zsclassifier_conf":0.1}
diff --git a/src/main/java/frc/robot/LaserCanHandler.java b/src/main/java/frc/robot/LaserCanHandler.java
index dc3f30a..6881f13 100644
--- a/src/main/java/frc/robot/LaserCanHandler.java
+++ b/src/main/java/frc/robot/LaserCanHandler.java
@@ -12,8 +12,10 @@ public class LaserCanHandler {
 
   private LaserCan m_entranceLaser;
   private LaserCan m_exitLaser;
+  private LaserCan m_middleLaser;
 
   private int entranceDebounceCounter = 0;
+  private int middleDebounceCounter = 0;
   private int exitDebounceCounter = 0;
   private int debounceMax = 3;
 
@@ -26,20 +28,51 @@ public static LaserCanHandler getInstance() {
 
   private LaserCanHandler() {
     m_entranceLaser = new LaserCan(RobotConstants.robotConfig.LaserCan.k_entranceId);
+    m_middleLaser = new LaserCan(RobotConstants.robotConfig.LaserCan.k_middleId);
     m_exitLaser = new LaserCan(RobotConstants.robotConfig.LaserCan.k_exitId);
 
-    try {
-      m_entranceLaser.setRangingMode(LaserCan.RangingMode.SHORT);
-      m_entranceLaser.setRegionOfInterest(new LaserCan.RegionOfInterest(8, 8, 16, 16));
-      m_entranceLaser.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS);
+    if (RobotBase.isReal()) {
+      try {
+        m_entranceLaser.setRangingMode(LaserCan.RangingMode.SHORT);
+        m_entranceLaser.setRegionOfInterest(new LaserCan.RegionOfInterest(8, 8, 16, 16));
+        m_entranceLaser.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_20MS);
 
-      m_exitLaser.setRangingMode(LaserCan.RangingMode.SHORT);
-      m_exitLaser.setRegionOfInterest(new LaserCan.RegionOfInterest(8, 8, 2, 2));
-      m_exitLaser.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_33MS);
+        m_middleLaser.setRangingMode(LaserCan.RangingMode.SHORT);
+        m_middleLaser.setRegionOfInterest(new LaserCan.RegionOfInterest(8, 8, 2, 2));
+        m_middleLaser.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_20MS);
 
-    } catch (ConfigurationFailedException e) {
-      System.out.println("Configuration failed! " + e);
+        m_exitLaser.setRangingMode(LaserCan.RangingMode.SHORT);
+        m_exitLaser.setRegionOfInterest(new LaserCan.RegionOfInterest(8, 8, 2, 2));
+        m_exitLaser.setTimingBudget(LaserCan.TimingBudget.TIMING_BUDGET_20MS);
+
+      } catch (ConfigurationFailedException e) {
+        System.out.println("Configuration failed! " + e);
+      }
+    }
+  }
+
+  @AutoLogOutput(key = "LaserCans/Entrance/distance")
+  public double getEntranceDistance() {
+    if (RobotBase.isSimulation()) {
+      return -1;
+    }
+    return m_entranceLaser.getMeasurement().distance_mm;
+  }
+
+  @AutoLogOutput(key = "LaserCans/Exit/distance")
+  public double getExitDistance() {
+    if (RobotBase.isSimulation()) {
+      return -1;
+    }
+    return m_exitLaser.getMeasurement().distance_mm;
+  }
+
+  @AutoLogOutput(key = "LaserCans/Middle/distance")
+  public double getMiddleDistance() {
+    if (RobotBase.isSimulation()) {
+      return -1;
     }
+    return m_middleLaser.getMeasurement().distance_mm;
   }
 
   @AutoLogOutput(key = "LaserCans/Entrance/seesCoral")
@@ -57,16 +90,6 @@ public boolean getEntranceSeesCoral() {
     return entranceDebounceCounter >= debounceMax;
   }
 
-  @AutoLogOutput(key = "LaserCans/Entrance/distance")
-  public double getEntranceDistance() {
-    return m_entranceLaser.getMeasurement().distance_mm;
-  }
-
-  @AutoLogOutput(key = "LaserCans/Exit/distance")
-  public double getExitDistance() {
-    return m_exitLaser.getMeasurement().distance_mm;
-  }
-
   @AutoLogOutput(key = "LaserCans/Exit/seesCoral")
   public boolean getExitSeesCoral() {
     if (RobotBase.isSimulation()) {
@@ -81,4 +104,19 @@ public boolean getExitSeesCoral() {
 
     return exitDebounceCounter >= debounceMax;
   }
+
+  @AutoLogOutput(key = "LaserCans/Middle/seesCoral")
+  public boolean getMiddleSeesCoral() {
+    if (RobotBase.isSimulation()) {
+      return true;
+    }
+
+    if (getMiddleDistance() < RobotConstants.robotConfig.LaserCan.k_middleThreshold) {
+      middleDebounceCounter++;
+    } else {
+      middleDebounceCounter = 0;
+    }
+
+    return middleDebounceCounter >= debounceMax;
+  }
 }
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 9ed49da..2a226b4 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -15,12 +15,15 @@
 import edu.wpi.first.wpilibj.DriverStation.Alliance;
 import edu.wpi.first.wpilibj.DriverStation.MatchType;
 import edu.wpi.first.wpilibj.GenericHID;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.util.Color;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.autonomous.AutoChooser;
 import frc.robot.autonomous.AutoRunner;
 import frc.robot.autonomous.modes.AutoModeBase;
+import frc.robot.autonomous.tasks.DriveToPoseTask;
 import frc.robot.autonomous.tasks.Task;
 import frc.robot.constants.RobotConstants;
 import frc.robot.controls.controllers.DriverController;
@@ -65,7 +68,7 @@ public class Robot extends LoggedRobot {
   private final Intakes m_intakes;
   private final Hopper m_hopper;
   private final TaskScheduler m_taskScheduler;
-  private final LEDs m_leds;
+  // private final LEDs m_leds;
   private final DriverController m_driverController;
 
   private final AutoRunner m_autoRunner;
@@ -81,6 +84,7 @@ public class Robot extends LoggedRobot {
   private final SwerveSysId m_swerveSysId;
   private Alliance m_alliance;
   private Pose2d m_reefPose;
+  private Field2d m_field;
 
   /**
    * This function is run when the robot is first started up and should be used
@@ -101,7 +105,7 @@ public Robot() {
     m_intakes = Intakes.getInstance();
     m_hopper = Hopper.getInstance();
     m_taskScheduler = TaskScheduler.getInstance();
-    m_leds = LEDs.getInstance();
+    // m_leds = LEDs.getInstance();
 
     // CanBridge.runTCP(); // For LaserCan configuration
 
@@ -119,9 +123,12 @@ public Robot() {
     m_subsystems.add(m_intakes);
     m_subsystems.add(m_hopper);
     m_subsystems.add(m_taskScheduler);
-    m_subsystems.add(m_leds);
+    // m_subsystems.add(m_leds);
 
     m_swerveSysId = new SwerveSysId(m_swerve.getSwerveModules(), "SwerveSysId");
+
+    m_field = new Field2d();
+    SmartDashboard.putData(m_field);
   }
 
   @Override
@@ -161,6 +168,8 @@ public void robotPeriodic() {
     if (m_operatorController.getWantsResetElevator()) {
       m_elevator.reset();
     }
+
+    m_field.setRobotPose(m_odometry.getPose());
   }
 
   @Override
@@ -196,6 +205,9 @@ public void autonomousPeriodic() {
 
   @Override
   public void teleopInit() {
+    // Elastic.selectTab("Teleoperated");
+    m_endEffector.setShouldBeIndexingCoral(false);
+
     m_swerve.setBrake(false);
   }
 
@@ -217,10 +229,9 @@ public void teleopPeriodic() {
       ySpeed *= slowScaler * boostScaler;
       rot *= slowScaler * boostScaler;
 
-      // Pose2d targetPose = m_poseAligner.getAndCalculateTargetPose(
-      // m_virtualRobotController.getCurrentPose(),
-      // m_virtualRobotController.getWantsAutoPositionBranch());
-      // ASPoseHelper.addPose("VirtualRobot/target", targetPose);
+      ElevatorState desiredElevatorState = m_operatorController.getDesiredElevatorState();
+      m_poseAligner.setDesiredElevatorState(desiredElevatorState);
+      // m_leds.setColorFromElevatorState(desiredElevatorState);
 
       Pose2d currentPose = m_odometry.getPose();
       Pose2d desiredPose = m_poseAligner.getAndCalculateTargetPose(
@@ -237,24 +248,30 @@ public void teleopPeriodic() {
         m_intakes.setIntakeState(IntakeVariant.LEFT, IntakeState.EJECT);
         m_intakes.setIntakeState(IntakeVariant.RIGHT, IntakeState.EJECT);
       } else if (m_operatorController.getWantsIntakeEjectStopped()) {
-        m_intakes.setIntakeState(IntakeVariant.LEFT, IntakeState.STOW);
-          m_intakes.setIntakeState(IntakeVariant.RIGHT, IntakeState.STOW);
+        m_intakes.setIntakeState(IntakeVariant.LEFT, IntakeState.END_EJECT);
+        m_intakes.setIntakeState(IntakeVariant.RIGHT, IntakeState.END_EJECT);
+      } else if (m_operatorController.getWantsIntakeAlgae()) {
+        m_intakes.setIntakeState(IntakeVariant.LEFT, IntakeState.ALGAE);
+        m_intakes.setIntakeState(IntakeVariant.RIGHT, IntakeState.ALGAE);
+      } else if (m_operatorController.getWantsScoreAlgae()) {
+        m_intakes.setIntakeState(IntakeVariant.LEFT, IntakeState.SCORE_ALGAE);
+        m_intakes.setIntakeState(IntakeVariant.RIGHT, IntakeState.SCORE_ALGAE);
       } else {
         // if (!isSafeToIndex() || isSafeToExtend()) {
-        //   m_intakes.setIntakeState(IntakeVariant.LEFT, IntakeState.STOW);
-        //   m_intakes.setIntakeState(IntakeVariant.RIGHT, IntakeState.STOW);
+        // m_intakes.setIntakeState(IntakeVariant.LEFT, IntakeState.STOW);
+        // m_intakes.setIntakeState(IntakeVariant.RIGHT, IntakeState.STOW);
         // } else {
-          if (m_operatorController.getWantsLeftIntakeGround()) {
-            m_intakes.setIntakeState(IntakeVariant.LEFT, IntakeState.INTAKE);
-          } else if (m_operatorController.getWantsLeftIntakeStow()) {
-            m_intakes.setIntakeState(IntakeVariant.LEFT, IntakeState.STOW);
-          }
-  
-          if (m_operatorController.getWantsRightIntakeGround()) {
-            m_intakes.setIntakeState(IntakeVariant.RIGHT, IntakeState.INTAKE);
-          } else if (m_operatorController.getWantsRightIntakeStow()) {
-            m_intakes.setIntakeState(IntakeVariant.RIGHT, IntakeState.STOW);
-          }
+        if (m_operatorController.getWantsLeftIntakeGround()) {
+          m_intakes.setIntakeState(IntakeVariant.LEFT, IntakeState.INTAKE);
+        } else if (m_operatorController.getWantsLeftIntakeStow()) {
+          m_intakes.setIntakeState(IntakeVariant.LEFT, IntakeState.STOW);
+        }
+
+        if (m_operatorController.getWantsRightIntakeGround()) {
+          m_intakes.setIntakeState(IntakeVariant.RIGHT, IntakeState.INTAKE);
+        } else if (m_operatorController.getWantsRightIntakeStow()) {
+          m_intakes.setIntakeState(IntakeVariant.RIGHT, IntakeState.STOW);
+        }
         // }
       }
 
@@ -262,23 +279,31 @@ public void teleopPeriodic() {
         m_odometry.reset();
       }
 
-      ElevatorState desiredElevatorState = m_operatorController.getDesiredElevatorState();
-      m_leds.setColorFromElevatorState(desiredElevatorState);
-
       if (m_driverController.getWantsAutoScoreLeft()) {
-        m_leds.setLeftColor(Color.kGreen);
-        m_leds.setRightColor(Color.kBlack);
-        m_taskScheduler.scheduleTasks(AutoModeBase.getAutoScoreTasks(
-            desiredElevatorState,
-            Branch.LEFT));
+        // m_leds.setLeftColor(Color.kGreen);
+        // m_leds.setRightColor(Color.kBlack);
+
+        ArrayList<Task> tasks = AutoModeBase.getAutoScoreTasks(desiredElevatorState, Branch.LEFT);
+
+        if (m_driverController.getWantsAutoScorePlusAlgae()) {
+          tasks.add(new DriveToPoseTask(Branch.NONE));
+          tasks.addAll(AutoModeBase.getDeAlgaeTasks());
+        }
+        m_taskScheduler.scheduleTasks(tasks);
       } else if (m_driverController.getWantsAutoScoreRight()) {
-        m_leds.setRightColor(Color.kGreen);
-        m_leds.setLeftColor(Color.kBlack);
-        m_taskScheduler.scheduleTasks(AutoModeBase.getAutoScoreTasks(
-            desiredElevatorState,
-            Branch.RIGHT));
+        // m_leds.setRightColor(Color.kGreen);
+        // m_leds.setLeftColor(Color.kBlack);
+
+        ArrayList<Task> tasks = AutoModeBase.getAutoScoreTasks(desiredElevatorState, Branch.RIGHT);
+
+        if (m_driverController.getWantsAutoScorePlusAlgae()) {
+          tasks.add(new DriveToPoseTask(Branch.NONE));
+          tasks.addAll(AutoModeBase.getDeAlgaeTasks());
+        }
+        m_taskScheduler.scheduleTasks(tasks);
       } else if (m_driverController.getWantsDeAlgaeTasks()) {
-        m_leds.setAllColor(Color.kAqua);
+        // m_leds.setAllColor(Color.kAqua);
+
         m_taskScheduler.scheduleTasks(AutoModeBase.getDeAlgaeTasks());
       }
 
@@ -290,12 +315,6 @@ public void teleopPeriodic() {
         }
       }
 
-      if (isSafeToIndex()) {
-        m_hopper.on();
-      } else {
-        m_hopper.off();
-      }
-
       if (m_operatorController.getWantsScore()) {
         if (m_elevator.getTargetState() == ElevatorState.L1) {
           m_endEffector.setState(EndEffectorState.SCORE_TROUGH);
@@ -323,7 +342,11 @@ public void teleopPeriodic() {
     } else if (m_operatorController.getStoppedReverseHopper()) {
       m_hopper.forward();
     } else {
-      m_hopper.forward();
+      if (isSafeToIndex()) {
+        m_hopper.on();
+      } else {
+        m_hopper.off();
+      }
     }
 
     ElevatorState elevatorState = m_operatorController.getDesiredElevatorState();
@@ -336,9 +359,11 @@ public void teleopPeriodic() {
       }
     }
 
-    if (m_elevator.getIsAtState() && m_elevator.getTargetState() == ElevatorState.L4 && m_operatorController.getWantsReverseEndEffector()) {
+    if (m_elevator.getIsAtState() && m_elevator.getTargetState() == ElevatorState.L4
+        && m_operatorController.getWantsReverseEndEffector()) {
       m_endEffector.setState(EndEffectorState.L4_REVERSE);
-    } else if (m_elevator.getIsAtState() && m_elevator.getTargetState() == ElevatorState.L4 && m_operatorController.getWantsReverseEndEffectorStopped()) {
+    } else if (m_elevator.getIsAtState() && m_elevator.getTargetState() == ElevatorState.L4
+        && m_operatorController.getWantsReverseEndEffectorStopped()) {
       m_endEffector.setState(EndEffectorState.INDEXED);
     }
 
diff --git a/src/main/java/frc/robot/autonomous/AutoRunner.java b/src/main/java/frc/robot/autonomous/AutoRunner.java
index ad32aa4..1c3ec0a 100644
--- a/src/main/java/frc/robot/autonomous/AutoRunner.java
+++ b/src/main/java/frc/robot/autonomous/AutoRunner.java
@@ -20,7 +20,7 @@ public class AutoRunner {
   private static AutoRunner m_autoRunner = null;
   private AutoChooser m_autoChooser;
   private AutoModeBase m_autoMode;
-  private LEDs m_leds;
+  // private LEDs m_leds;
 
   public enum AutoMode {
     DO_NOTHING,
@@ -34,7 +34,7 @@ public enum AutoMode {
   private AutoRunner() {
     // Use this to set the default auto mode
     AutoMode defaultAuto = AutoMode.CENTER;
-    m_leds = LEDs.getInstance();
+    // m_leds = LEDs.getInstance();
 
     m_autoChooser = AutoChooser.getInstance();
 
@@ -75,25 +75,25 @@ private void onAutoChange(String newAuto) {
 
     switch (m_selectedAuto) {
       case DO_NOTHING -> {
-        m_leds.setAllColor(Color.kBlack);
+        // m_leds.setAllColor(Color.kBlack);
         m_autoMode = new DoNothingMode();
       }
       case TEST -> {
-        m_leds.setAllColorMode(LEDModes.rainbowChase);
+        // m_leds.setAllColorMode(LEDModes.rainbowChase);
         m_autoMode = new TestMode();
       }
       case LEFT -> {
-        m_leds.setRightColor(color);
-        m_leds.setLeftColor(Color.kBlack);
+        // m_leds.setRightColor(color);
+        // m_leds.setLeftColor(Color.kBlack);
         m_autoMode = new LeftMode();
       }
       case RIGHT -> {
-        m_leds.setLeftColor(color);
-        m_leds.setRightColor(Color.kBlack);
+        // m_leds.setLeftColor(color);
+        // m_leds.setRightColor(Color.kBlack);
         m_autoMode = new RightMode();
       }
       case CENTER -> {
-        m_leds.setAllColor(color);
+        // m_leds.setAllColor(color);
         m_autoMode = new CenterMode();
       }
       default -> {
diff --git a/src/main/java/frc/robot/autonomous/modes/AutoModeBase.java b/src/main/java/frc/robot/autonomous/modes/AutoModeBase.java
index 9644a4c..937f706 100644
--- a/src/main/java/frc/robot/autonomous/modes/AutoModeBase.java
+++ b/src/main/java/frc/robot/autonomous/modes/AutoModeBase.java
@@ -3,7 +3,6 @@
 import java.util.ArrayList;
 
 import frc.robot.autonomous.tasks.ArmTask;
-import frc.robot.autonomous.tasks.DoNothingTask;
 import frc.robot.autonomous.tasks.DriveForwardTask;
 import frc.robot.autonomous.tasks.DriveToPoseTask;
 import frc.robot.autonomous.tasks.ElevatorTask;
@@ -12,7 +11,6 @@
 import frc.robot.autonomous.tasks.IntakeTask;
 import frc.robot.autonomous.tasks.ParallelTask;
 import frc.robot.autonomous.tasks.SequentialTask;
-import frc.robot.autonomous.tasks.SkippableTask;
 import frc.robot.autonomous.tasks.Task;
 import frc.robot.autonomous.tasks.WaitTask;
 import frc.robot.autonomous.tasks.WaitTask.WaitCondition;
@@ -69,7 +67,7 @@ public static ArrayList<Task> getDeAlgaeTasks() {
 
     tasks.add(new ParallelTask(
         new SequentialTask(
-            new WaitTask(0.5),
+            new WaitTask(0.2),
             new DriveToPoseTask(Branch.ALGAE_REVERSE)),
         new ArmTask(ArmState.STOW)));
 
@@ -81,6 +79,9 @@ public static ArrayList<Task> getDeAlgaeTasks() {
   public static ArrayList<Task> getAutoScoreTasks(ElevatorState elevatorState, Branch branch) {
     ArrayList<Task> tasks = new ArrayList<>();
     ArmState armTarget;
+
+    boolean CURRENTLY_USING_SAFE_POSE = false;
+
     boolean useSafePose;
     DriveToPoseTask firstDriveTask;
 
@@ -91,7 +92,12 @@ public static ArrayList<Task> getAutoScoreTasks(ElevatorState elevatorState, Bra
     } else {
       armTarget = ArmState.STOW;
       useSafePose = false;
-      firstDriveTask = new DriveToPoseTask(Branch.NONE);
+
+      if (CURRENTLY_USING_SAFE_POSE) {
+        firstDriveTask = new DriveToPoseTask(Branch.NONE);
+      } else {
+        firstDriveTask = new DriveToPoseTask(branch);
+      }
     }
 
     tasks.add(new ParallelTask(
@@ -103,19 +109,24 @@ public static ArrayList<Task> getAutoScoreTasks(ElevatorState elevatorState, Bra
                 new ArmTask(armTarget)))));
 
     // Drive to score
-    // if (useSafePose) {
+    if (useSafePose || CURRENTLY_USING_SAFE_POSE) {
       tasks.add(new DriveToPoseTask(branch));
-    // }
+    }
 
     // Score
-    tasks.add(new EndEffectorTask(EndEffectorState.SCORE_BRANCHES));
+    if (elevatorState == ElevatorState.L1) {
+      tasks.add(new EndEffectorTask(EndEffectorState.SCORE_TROUGH));
+    } else {
+      tasks.add(new EndEffectorTask(EndEffectorState.SCORE_BRANCHES));
+    }
 
     return tasks;
   }
 
   public void autoScore(ElevatorState elevatorState, Branch branch, int feederStation) {
     score(elevatorState, branch, new DriveToPoseTask(feederStation));
-    // score(elevatorState, branch, new SkippableTask(new DriveToPoseTask(feederStation), 2.2, new DoNothingTask()));
+    // score(elevatorState, branch, new SkippableTask(new
+    // DriveToPoseTask(feederStation), 2.2, new DoNothingTask()));
   }
 
   public void autoScore(ElevatorState elevatorState, Branch branch) {
diff --git a/src/main/java/frc/robot/autonomous/modes/LeftMode.java b/src/main/java/frc/robot/autonomous/modes/LeftMode.java
index 81320e6..df08f4b 100644
--- a/src/main/java/frc/robot/autonomous/modes/LeftMode.java
+++ b/src/main/java/frc/robot/autonomous/modes/LeftMode.java
@@ -9,6 +9,6 @@ public class LeftMode extends AutoModeBase {
   public void queueTasks() {
     autoScore(ElevatorState.L4, Branch.RIGHT, FeederStation.LEFT);
     autoScore(ElevatorState.L4, Branch.RIGHT, FeederStation.LEFT);
-    autoScore(ElevatorState.L3, Branch.LEFT, FeederStation.LEFT);
+    autoScore(ElevatorState.L4, Branch.LEFT, FeederStation.LEFT);
   }
 }
diff --git a/src/main/java/frc/robot/autonomous/modes/RightMode.java b/src/main/java/frc/robot/autonomous/modes/RightMode.java
index 9d59d48..8b4d519 100644
--- a/src/main/java/frc/robot/autonomous/modes/RightMode.java
+++ b/src/main/java/frc/robot/autonomous/modes/RightMode.java
@@ -9,6 +9,6 @@ public class RightMode extends AutoModeBase {
   public void queueTasks() {
     autoScore(ElevatorState.L4, Branch.LEFT, FeederStation.RIGHT);
     autoScore(ElevatorState.L4, Branch.LEFT, FeederStation.RIGHT);
-    autoScore(ElevatorState.L3, Branch.RIGHT, FeederStation.RIGHT);
+    autoScore(ElevatorState.L4, Branch.RIGHT, FeederStation.RIGHT);
   }
 }
diff --git a/src/main/java/frc/robot/autonomous/tasks/IntakeTask.java b/src/main/java/frc/robot/autonomous/tasks/IntakeTask.java
index a96d8f8..f467610 100644
--- a/src/main/java/frc/robot/autonomous/tasks/IntakeTask.java
+++ b/src/main/java/frc/robot/autonomous/tasks/IntakeTask.java
@@ -1,5 +1,6 @@
 package frc.robot.autonomous.tasks;
 
+import frc.robot.subsystems.EndEffector;
 import frc.robot.subsystems.intakes.Intake.IntakeState;
 import frc.robot.subsystems.intakes.Intakes;
 import frc.robot.subsystems.intakes.Intakes.IntakeVariant;
@@ -17,6 +18,13 @@ public IntakeTask(IntakeVariant intakeVariant, IntakeState intakeState) {
 
   @Override
   public void prepare() {
+    if (m_intakeState == IntakeState.STOW) {
+      // EndEffector.getInstance().shouldBeIndexingCoral = true;
+      EndEffector.getInstance().setShouldBeIndexingCoral(true);
+    } else {
+      EndEffector.getInstance().setShouldBeIndexingCoral(false);
+    }
+
     m_intakes.setIntakeState(m_intakeVariant, m_intakeState);
   }
 
diff --git a/src/main/java/frc/robot/constants/Constants.java b/src/main/java/frc/robot/constants/Constants.java
index 9c42b12..fb2f1a4 100644
--- a/src/main/java/frc/robot/constants/Constants.java
+++ b/src/main/java/frc/robot/constants/Constants.java
@@ -65,7 +65,7 @@ public static class SwerveDriveConstants {
     public final double k_wheelBaseRadius = Math.hypot(k_xCenterDistance, k_yCenterDistance);
 
     // Max speeds
-    public final double k_maxDriverSpeed = 2.0; // Meters per second
+    public final double k_maxDriverSpeed = 1.0; // Meters per second
     public final double k_maxDriverBoostSpeed = 4.5; // Meters per second
     public final double k_maxPossibleSpeed = 4.574; // Meters per second
 
@@ -77,7 +77,8 @@ public static class SwerveDriveConstants {
     public final double k_maxAngularAcceleration = 46.304; // Radians per second^2, pulled from Choreo
 
     public final double k_slowScaler = 0.5; // % reduction in speed
-    public final double k_boostScaler = 4.5 / 2.0; // % increase in speed (k_maxDriverBoostSpeed/k_maxDriverSpeed)
+    public final double k_boostScaler = k_maxDriverBoostSpeed / k_maxDriverSpeed; // % increase in speed
+                                                                                  // (k_maxDriverBoostSpeed/k_maxDriverSpeed)
 
     public final double k_wheelRadiusIn = 2.0; // inches
     public final double k_wheelCircumference = Units.inchesToMeters(k_wheelRadiusIn * 2.0 * Math.PI); // meters
@@ -199,11 +200,11 @@ public static class ElevatorConstants {
 
     public final double k_stowHeight = 0.0;
     public final double k_L1Height = 16.0;
-    public final double k_L2Height = 25.64;
+    public final double k_L2Height = 26.0;
     public final double k_L3Height = 42.7;
     public final double k_L4Height = 60.5; // 59.6;
     public final double k_maxHeight = 60.5;
-    public final double k_feederHeight = 28.36;
+    public final double k_feederHeight = 29.57;
     // public final double k_groundAlgaeHeight = 0.0;
     public final double k_lowAlgaeHeight = 27.643;
     public final double k_highAlgaeHeight = 44.571;
@@ -246,15 +247,13 @@ public static class EndEffectorConstants {
 
     public final double k_rollerGearRatio = 1.0 / 10.0;
 
-    public final double k_rollerP = 0.0010;
+    public final double k_rollerP = 0.0004;
     public final double k_rollerI = 0.000;
     public final double k_rollerD = 0.0;
     public final double k_rollerFF = 0.001;
 
     public final int k_maxCurrent = 20;
 
-    public final double k_speedScaleFactor = 4.5 / 2.0; // Right diameter / left diameter
-
     // public final double[] k_stopSpeeds = new double[] { 0.0, 0.0 };
     // public final double[] k_forwardIndexFastSpeeds = new double[] { 0.20, 0.20 };
     // public final double[] k_forwardIndexSlowSpeeds = new double[] { 0.10, 0.10 };
@@ -263,20 +262,21 @@ public static class EndEffectorConstants {
     // public final double[] k_troughSpeeds = new double[] { 0.3, 0.5 };
 
     public final double k_stopSpeed = 0.0;
-    public final double k_forwardIndexFastSpeed = 200.0;
+    public final double k_forwardIndexFastSpeed = 275.0;
     public final double k_forwardIndexSlowSpeed = 80.0;
-    public final double k_reverseIndexSpeed = -40.0;
-    public final double k_branchSpeed = 200.0;
-    public final double k_troughSpeed = 200.0;
+    public final double k_reverseIndexSpeed = -60.0;
+    public final double k_branchSpeed = 300.0;
+    public final double k_troughSpeed = 500.0;
   }
 
   public static class LaserCanConstants {
-    public final int k_indexId = 33;
+    public final int k_middleId = 33;
     public final int k_entranceId = 34;
     public final int k_exitId = 35;
 
     public final double k_entranceThreshold = 110.0;
     public final double k_exitThreshold = 40.0;
+    public final double k_middleThreshold = 40.0;
   }
 
   public static class IntakeConstants {
@@ -291,7 +291,7 @@ public static class IntakeConstants {
 
     public final double k_rollerGearRatio = (1.0 / 4.0);
 
-    public final double k_pivotMotorP = 4.0; // 15.0;
+    public final double k_pivotMotorP = 6.0; // 15.0;
     public final double k_pivotMotorI = 0.0;
     public final double k_pivotMotorD = 0.0;
 
@@ -309,7 +309,8 @@ public static class IntakeConstants {
     public final double k_maxAcceleration = 4.0;
     public final double k_maxVelocity = 2.0;
 
-    public final double k_maxIntakeSpeed = 300.0;
+    public final double k_maxIntakeSpeed = 350.0; // 300.0;
+    public final double k_stowingIntakeSpeed = k_maxIntakeSpeed * 0.75;
 
     public final double k_allowedPivotError = 0.01;
     public final double k_lowestRollerSpeed = -1000.0; // TODO: Make this work
@@ -324,6 +325,7 @@ public class LeftConstants {
       public double k_ejectPosition;
       public double k_horizontalPosition;
       public double k_stuckPosition;
+      public double k_algaePosition;
     }
 
     public class RightConstants {
@@ -332,6 +334,7 @@ public class RightConstants {
       public double k_ejectPosition;
       public double k_horizontalPosition;
       public double k_stuckPosition;
+      public double k_algaePosition;
     }
   }
 
@@ -349,14 +352,18 @@ public static class AutoAlignConstants {
     public final double k_minSafeElevatorDistance = 2.0;
 
     // Scoring offsets
-    // public final double k_l4ScoringDistance = 0.384;
+    // TODO: This is a possible new L4 scoring pose
+    // public final double k_l4ScoringDistance = 0.100;
+
     public final double k_l4ScoringDistance = 0.254;
     public final double k_otherScoringOffset = 0.0;
     public final double k_scoringHorizontalOffset = 0.175;
+    public final double k_scoringTroughHorizontalOffset = 0.46;
     public final double k_algaeHorizontalOffset = 0.0;
     public final double k_algaeReverseExtraDistance = -0.5;
 
     public final double k_maxApproachSpeed = 5.0;
+    public final double k_maxIndexApproachSpeed = 2.0;
     public final double k_fallOffDistance = 1.5;
 
     // Base feeder scoring pose
diff --git a/src/main/java/frc/robot/constants/MonarchConstants.java b/src/main/java/frc/robot/constants/MonarchConstants.java
index 3807bd6..57cbaf1 100644
--- a/src/main/java/frc/robot/constants/MonarchConstants.java
+++ b/src/main/java/frc/robot/constants/MonarchConstants.java
@@ -27,18 +27,21 @@ public MonarchConstants() {
     SwerveDrive.Turn.k_IZone = 0.0;
 
     Arm.k_stowAngle = 0.26152366399765015;
-    Arm.k_L4Angle = 0.5249119997024536; //0.5834578275680542;
+    Arm.k_L4Angle = 0.5249119997024536;
+    // TODO: This is a possible new L4 scoring pose
+    // Arm.k_L4Angle = 0.5834578275680542;
     Arm.k_horizontalAngle = 0.5074164867401123;
-    Arm.k_sourceAngle = 0.37661659717559814;
+    Arm.k_sourceAngle = 0.35026;
 
     Intake.Left.k_stowPosition = 0.551323413848877;
-    Intake.Left.k_groundPosition = 0.19023025035858154;
+    Intake.Left.k_groundPosition = 0.18343025035858154;
     Intake.Left.k_ejectPosition = 0.31802046298980713;
     Intake.Left.k_horizontalPosition = 0.31802046298980713;
     Intake.Left.k_stuckPosition = 0.5542285442352295;
+    Intake.Left.k_algaePosition = 0.4322316646575928;
 
     Intake.Right.k_stowPosition = 0.4848584532737732;
-    Intake.Right.k_groundPosition = 0.850831151008606; //0.8430086374282837;
+    Intake.Right.k_groundPosition = 0.854831151008606;
     Intake.Right.k_ejectPosition = 0.7199869155883789;
     Intake.Right.k_horizontalPosition = 0.7199869155883789;
     Intake.Right.k_stuckPosition = 0.7980979681015015;
diff --git a/src/main/java/frc/robot/controls/controllers/DriverController.java b/src/main/java/frc/robot/controls/controllers/DriverController.java
index affab68..b38ef10 100644
--- a/src/main/java/frc/robot/controls/controllers/DriverController.java
+++ b/src/main/java/frc/robot/controls/controllers/DriverController.java
@@ -42,8 +42,7 @@ public double testNegative() {
   }
 
   public boolean getWantsAutoPosition() {
-    return getRawButton(Button.X) ||
-        getRawButton(Button.LEFT_BUMPER) ||
+    return getRawButton(Button.LEFT_BUMPER) ||
         getRawButton(Button.RIGHT_BUMPER);
   }
 
@@ -56,8 +55,8 @@ public Branch getWantsAutoPositionBranch() {
     return Branch.NONE;
   }
 
-  public boolean getWantsAutoPositionPressed() {
-    return this.getRawButtonPressed(Button.X);
+  public boolean getWantsAutoScorePlusAlgae() {
+    return this.getRawButton(Button.X);
   }
 
   public boolean getWantsAutoPositionFeederStation() {
diff --git a/src/main/java/frc/robot/controls/controllers/OperatorController.java b/src/main/java/frc/robot/controls/controllers/OperatorController.java
index 071d630..b301a09 100644
--- a/src/main/java/frc/robot/controls/controllers/OperatorController.java
+++ b/src/main/java/frc/robot/controls/controllers/OperatorController.java
@@ -25,6 +25,14 @@ public boolean getWantsLeftIntakeStow() {
     return getRawButtonReleased(k_leftIntakeButton);
   }
 
+  public boolean getWantsIntakeAlgae() {
+    return getFilteredAxis(Axis.LEFT_TRIGGER) >= 0.5;
+  }
+
+  public boolean getWantsScoreAlgae() {
+    return getFilteredAxis(Axis.RIGHT_TRIGGER) >= 0.5;
+  }
+
   private final int k_rightIntakeButton = Button.RIGHT_BUMPER;
 
   public boolean getWantsRightIntakeGround() {
@@ -45,14 +53,23 @@ public boolean getWantsIntakeEjectStopped() {
     return getRawButtonReleased(k_ejectButton);
   }
 
+  // Notification notification = new Notification(Elastic.Notification.NotificationLevel.INFO,
+  // "Queued Elevator State Changed",
+  // "Queued state: L4",
+  // 1500);
+
   public ElevatorState getDesiredElevatorState() {
     if (getHatPressed(Direction.DOWN)) {
+      // Elastic.sendNotification(notification.withDescription("Queued state: L1"));
       m_desiredElevatorState = ElevatorState.L1;
     } else if (getHatPressed(Direction.RIGHT)) {
+      // Elastic.sendNotification(notification.withDescription("Queued state: L2"));
       m_desiredElevatorState = ElevatorState.L2;
     } else if (getHatPressed(Direction.LEFT)) {
+      // Elastic.sendNotification(notification.withDescription("Queued state: L3"));
       m_desiredElevatorState = ElevatorState.L3;
     } else if (getHatPressed(Direction.UP)) {
+      // Elastic.sendNotification(notification.withDescription("Queued state: L4"));
       m_desiredElevatorState = ElevatorState.L4;
     }
 
@@ -70,7 +87,7 @@ public boolean getWantsStow() {
   }
 
   public boolean getWantsResetElevator() {
-    return this.getRawButtonPressed(Button.START);
+    return this.getRawButtonPressed(Button.LEFT_JOYSTICK) || this.getRawButtonPressed(Button.RIGHT_JOYSTICK);
   }
 
   private final int k_scoreButton = Button.X;
diff --git a/src/main/java/frc/robot/subsystems/Arm.java b/src/main/java/frc/robot/subsystems/Arm.java
index 9026f09..3a905f5 100644
--- a/src/main/java/frc/robot/subsystems/Arm.java
+++ b/src/main/java/frc/robot/subsystems/Arm.java
@@ -135,7 +135,7 @@ public void writePeriodicOutputs() {
   }
 
   public enum ArmState {
-    STOW, EXTEND
+    STOW, EXTEND, EXTEND_FEEDER_STATION
   }
 
   @Override
@@ -168,6 +168,9 @@ public double getArmTarget() {
       case EXTEND -> {
         return RobotConstants.robotConfig.Arm.k_L4Angle;
       }
+      case EXTEND_FEEDER_STATION -> {
+        return RobotConstants.robotConfig.Arm.k_sourceAngle;
+      }
       default -> {
         return RobotConstants.robotConfig.Arm.k_stowAngle;
       }
diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java
index befa377..19d0462 100644
--- a/src/main/java/frc/robot/subsystems/Elevator.java
+++ b/src/main/java/frc/robot/subsystems/Elevator.java
@@ -100,7 +100,8 @@ public enum ElevatorState {
     L3,
     L4,
     ALGAE_LOW,
-    ALGAE_HIGH
+    ALGAE_HIGH,
+    FEEDER_STATION
   }
 
   private static class PeriodicIO {
@@ -145,6 +146,8 @@ public void reset() {
   }
 
   public void setState(ElevatorState state) {
+    PoseAligner.getInstance().setDesiredElevatorState(state);
+
     m_periodicIO.target_state = state;
   }
 
@@ -209,6 +212,9 @@ private double getElevatorTarget() {
       case ALGAE_LOW -> {
         return RobotConstants.robotConfig.Elevator.k_lowAlgaeHeight;
       }
+      case FEEDER_STATION -> {
+        return RobotConstants.robotConfig.Elevator.k_feederHeight;
+      }
       default -> {
         return RobotConstants.robotConfig.Elevator.k_stowHeight;
       }
diff --git a/src/main/java/frc/robot/subsystems/EndEffector.java b/src/main/java/frc/robot/subsystems/EndEffector.java
index 7e91c44..59c7cd5 100644
--- a/src/main/java/frc/robot/subsystems/EndEffector.java
+++ b/src/main/java/frc/robot/subsystems/EndEffector.java
@@ -34,7 +34,7 @@ public class EndEffector extends Subsystem {
   private LaserCanHandler m_laserCan;
   private Arm m_arm;
   private Elevator m_elevator;
-  private LEDs m_leds;
+  // private LEDs m_leds;
 
   public static EndEffector getInstance() {
     if (m_instance == null) {
@@ -57,7 +57,7 @@ private EndEffector() {
     m_laserCan = LaserCanHandler.getInstance();
     m_arm = Arm.getInstance();
     m_elevator = Elevator.getInstance();
-    m_leds = LEDs.getInstance();
+    // m_leds = LEDs.getInstance();
 
     SparkBaseConfig rollerConfig = new SparkFlexConfig();
 
@@ -86,33 +86,48 @@ private EndEffector() {
 
   private static class PeriodicIO {
     EndEffectorState state = EndEffectorState.INDEXED;
+    boolean shouldBeIndexingCoral = false;
+  }
+
+  public boolean getShouldBeIndexingCoral() {
+    return m_periodicIO.shouldBeIndexingCoral;
+  }
+
+  public void setShouldBeIndexingCoral(boolean shouldBeIndexingCoral) {
+    m_periodicIO.shouldBeIndexingCoral = shouldBeIndexingCoral;
   }
 
   public enum EndEffectorState {
+    // Indexing
     OFF,
     FORWARD_INDEX_FAST,
     FORWARD_INDEX_SLOW,
     REVERSE_INDEX,
-    SCORE_BRANCHES,
-    SCORE_TROUGH,
     INDEXED,
-    L4_REVERSE
+
+    // 💩
+    REINDEX,
+    L4_REVERSE,
+
+    // Scoring
+    SCORE_BRANCHES,
+    SCORE_TROUGH
   }
 
   public void setState(EndEffectorState state) {
     m_periodicIO.state = state;
 
-    switch (state) {
-      case OFF -> {
-        m_leds.setAllColor(Color.kRed);
-      }
-      case FORWARD_INDEX_FAST -> {
-        m_leds.setAllColor(Color.kYellow);
-      }
-      case INDEXED -> {
-        m_leds.setAllColor(Color.kGreen);
-      }
-    }
+    // switch (state) {
+    //   case OFF -> {
+    //     m_leds.setAllColor(Color.kRed);
+    //   }
+    //   case FORWARD_INDEX_FAST -> {
+    //     m_leds.setAllColor(Color.kYellow);
+    //   }
+    //   case INDEXED -> {
+    //     m_leds.setAllColor(Color.kGreen);
+    //   }
+    // }
   }
 
   private void off() {
@@ -146,10 +161,15 @@ public void writePeriodicOutputs() {
 
     m_rightRollerPIDController.setReference(desiredSpeed, ControlType.kVelocity);
     m_leftRollerPIDController.setReference(
-        desiredSpeed * RobotConstants.robotConfig.EndEffector.k_speedScaleFactor,
+        desiredSpeed,
         ControlType.kVelocity);
   }
 
+  public boolean shouldDriveSlow() {
+    return m_periodicIO.shouldBeIndexingCoral
+        && (m_periodicIO.state == EndEffectorState.OFF || m_periodicIO.state == EndEffectorState.FORWARD_INDEX_FAST);
+  }
+
   @AutoLogOutput(key = "EndEffector/RightMotorVoltage")
   public double getRightMotorVoltage() {
     return Helpers.getVoltage(m_rightMotor);
@@ -225,6 +245,10 @@ private double getDesiredRollerSpeed() {
         return -RobotConstants.robotConfig.EndEffector.k_reverseIndexSpeed;
       }
 
+      case REINDEX -> {
+        return RobotConstants.robotConfig.EndEffector.k_reverseIndexSpeed;
+      }
+
       default -> {
         return RobotConstants.robotConfig.EndEffector.k_stopSpeed;
       }
@@ -237,6 +261,12 @@ public void stop() {
   }
 
   private void updateState() {
+    if (!m_laserCan.getExitSeesCoral() && getEndEffectorState() != EndEffectorState.FORWARD_INDEX_FAST
+        && getEndEffectorState() != EndEffectorState.OFF) {
+      setState(EndEffectorState.OFF);
+      return;
+    }
+
     switch (m_periodicIO.state) {
       case FORWARD_INDEX_FAST -> {
         if (m_laserCan.getExitSeesCoral()) {
@@ -246,8 +276,8 @@ private void updateState() {
 
       case FORWARD_INDEX_SLOW -> {
         if (!m_laserCan.getEntranceSeesCoral()) {
-          // setState(EndEffectorState.INDEXED);
-          setState(EndEffectorState.REVERSE_INDEX);
+          setState(EndEffectorState.INDEXED);
+          // setState(EndEffectorState.REVERSE_INDEX);
         }
       }
 
@@ -261,13 +291,19 @@ private void updateState() {
         if (!m_laserCan.getExitSeesCoral()) {
           setState(EndEffectorState.OFF);
         }
+
+        if (!m_laserCan.getMiddleSeesCoral()) {
+          setState(EndEffectorState.REINDEX);
+        }
+      }
+
+      case REINDEX -> {
+        if (m_laserCan.getMiddleSeesCoral()) {
+          setState(EndEffectorState.INDEXED);
+        }
       }
 
       case OFF -> {
-        // if (!(m_laserCan.getEntranceSeesCoral() || m_laserCan.getIndexSeesCoral())
-        // && m_laserCan.getExitSeesCoral()) {
-        // reverse();
-        // } else
         if (m_laserCan.getEntranceSeesCoral()) {
           indexFast();
         }
diff --git a/src/main/java/frc/robot/subsystems/PoseAligner.java b/src/main/java/frc/robot/subsystems/PoseAligner.java
index a8ae5d6..45e83e3 100644
--- a/src/main/java/frc/robot/subsystems/PoseAligner.java
+++ b/src/main/java/frc/robot/subsystems/PoseAligner.java
@@ -33,6 +33,7 @@ public static PoseAligner getInstance() {
   private static class PeriodicIO {
     Pose2d safePose = new Pose2d();
     Pose2d scoringPose = new Pose2d();
+    ElevatorState elevatorState = ElevatorState.STOW;
   }
 
   @Override
@@ -128,6 +129,10 @@ public void stop() {
   public void reset() {
   }
 
+  public void setDesiredElevatorState(ElevatorState elevatorState) {
+    m_periodicIO.elevatorState = elevatorState;
+  }
+
   public ElevatorState getDeAlgaeElevatorState() {
     if (Helpers.isBlueAlliance()) {
       return m_reefSide % 2 != 0 ? ElevatorState.ALGAE_HIGH : ElevatorState.ALGAE_LOW;
@@ -143,12 +148,22 @@ public Pose2d getReefScoringPose(Pose2d safePose, int reefSide, Branch branch) {
     // y-translation -> along the shorter side of the field
     double offset = 0.0;
 
-    if (branch == Branch.RIGHT) {
-      offset = -RobotConstants.robotConfig.AutoAlign.k_scoringHorizontalOffset;
-    } else if (branch == Branch.LEFT) {
-      offset = RobotConstants.robotConfig.AutoAlign.k_scoringHorizontalOffset;
-    } else if (branch == Branch.ALGAE) {
+    if (branch == Branch.ALGAE) {
       offset = RobotConstants.robotConfig.AutoAlign.k_algaeHorizontalOffset;
+    } else {
+      if (m_periodicIO.elevatorState == ElevatorState.L1) {
+        if (branch == Branch.RIGHT) {
+          offset = -RobotConstants.robotConfig.AutoAlign.k_scoringTroughHorizontalOffset;
+        } else if (branch == Branch.LEFT) {
+          offset = RobotConstants.robotConfig.AutoAlign.k_scoringTroughHorizontalOffset;
+        }
+      } else {
+        if (branch == Branch.RIGHT) {
+          offset = -RobotConstants.robotConfig.AutoAlign.k_scoringHorizontalOffset;
+        } else if (branch == Branch.LEFT) {
+          offset = RobotConstants.robotConfig.AutoAlign.k_scoringHorizontalOffset;
+        }
+      }
     }
 
     Translation2d translation = new Translation2d(scoringDistance, offset);
diff --git a/src/main/java/frc/robot/subsystems/drivetrain/RARHolonomicDriveController.java b/src/main/java/frc/robot/subsystems/drivetrain/RARHolonomicDriveController.java
index 0f31534..7321f98 100644
--- a/src/main/java/frc/robot/subsystems/drivetrain/RARHolonomicDriveController.java
+++ b/src/main/java/frc/robot/subsystems/drivetrain/RARHolonomicDriveController.java
@@ -11,6 +11,7 @@
 import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
 import edu.wpi.first.math.trajectory.TrapezoidProfile.State;
 import frc.robot.constants.RobotConstants;
+import frc.robot.subsystems.EndEffector;
 import frc.robot.wrappers.PIDConstants;
 import frc.robot.wrappers.ProfiledPIDConstants;
 
@@ -19,9 +20,11 @@ public class RARHolonomicDriveController {
   private final PIDController xController;
   private final PIDController yController;
   private final ProfiledPIDController rotationController;
+  private EndEffector m_endEffector;
 
   private boolean m_isEnabled = true;
-  // private double mpsToRps = 1.0 / RobotConstants.robotConfig.SwerveDrive.k_wheelBaseRadius;
+  // private double mpsToRps = 1.0 /
+  // RobotConstants.robotConfig.SwerveDrive.k_wheelBaseRadius;
 
   // private boolean firstTimeForEverything = false;
 
@@ -51,6 +54,8 @@ public RARHolonomicDriveController(
         period);
     this.rotationController.setIntegratorRange(-rotationConstants.k_iZone, rotationConstants.k_iZone);
     this.rotationController.enableContinuousInput(-Math.PI, Math.PI);
+
+    this.m_endEffector = EndEffector.getInstance();
   }
 
   /**
@@ -62,6 +67,8 @@ public RARHolonomicDriveController(
   public RARHolonomicDriveController(
       PIDConstants translationConstants, ProfiledPIDConstants rotationConstants) {
     this(translationConstants, rotationConstants, 0.02);
+
+    this.m_endEffector = EndEffector.getInstance();
   }
 
   /**
@@ -91,14 +98,12 @@ public void reset(Pose2d currentPose, ChassisSpeeds currentSpeeds) {
   /**
    * Calculates the next output of the path following controller
    *
-   * @param currentPose      The current robot pose
-   * @param targetState      The desired trajectory state
-   * @param goalPose         The pose to end at
-   * @param maxApproachSpeed the speed in m/s to run at
+   * @param currentPose The current robot pose
+   * @param targetState The desired trajectory state
+   * @param goalPose    The pose to end at
    * @return The next robot relative output of the path following controller
    */
-  public ChassisSpeeds calculatePoseSpeeds(Pose2d currentPose, Pose2d goalPose, double maxApproachSpeed) {
-
+  public ChassisSpeeds calculatePoseSpeeds(Pose2d currentPose, Pose2d goalPose) {
     Logger.recordOutput("Odometry/GoalPose", goalPose);
 
     if (!m_isEnabled) {
@@ -109,6 +114,13 @@ public ChassisSpeeds calculatePoseSpeeds(Pose2d currentPose, Pose2d goalPose, do
     double translationError = currentPose.getTranslation().getDistance(goalPose.getTranslation());
     double rotationError = Math.abs(currentPose.getRotation().minus(goalPose.getRotation()).getDegrees());
 
+    double maxApproachSpeed = RobotConstants.robotConfig.AutoAlign.k_maxApproachSpeed;
+
+    // If we haven't indexed yet, we should be running at a slower speed
+    if (m_endEffector.shouldDriveSlow()) {
+      maxApproachSpeed = RobotConstants.robotConfig.AutoAlign.k_maxIndexApproachSpeed;
+    }
+
     // As we get closer to the target, we should be ramping down our x/y FF
     double targetSpeed = maxApproachSpeed;
     double falloffDistance = RobotConstants.robotConfig.AutoAlign.k_fallOffDistance;
diff --git a/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java b/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java
index bfbb6d7..746604e 100644
--- a/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java
+++ b/src/main/java/frc/robot/subsystems/drivetrain/SwerveDrive.java
@@ -146,19 +146,13 @@ public void drive(double xSpeed, double ySpeed, double rot, boolean fieldRelativ
     // TODO be able to call RAROdometry from Swerve Drive (maybe pass modules as
     // parameters)
 
-    ChassisSpeeds targetPoseChassisSpeeds = m_driveController.calculatePoseSpeeds(
-        currentPose,
-        targetPose,
-        RobotConstants.robotConfig.AutoAlign.k_maxApproachSpeed);
+    ChassisSpeeds targetPoseChassisSpeeds = m_driveController.calculatePoseSpeeds(currentPose, targetPose);
 
     drive(driverChassisSpeeds.plus(targetPoseChassisSpeeds));
   }
 
   public void drive(Pose2d currentPose, Pose2d goalPose) {
-    ChassisSpeeds targetPoseChassisSpeeds = m_driveController.calculatePoseSpeeds(
-        currentPose,
-        goalPose,
-        RobotConstants.robotConfig.AutoAlign.k_maxApproachSpeed);
+    ChassisSpeeds targetPoseChassisSpeeds = m_driveController.calculatePoseSpeeds(currentPose, goalPose);
 
     drive(targetPoseChassisSpeeds);
   }
diff --git a/src/main/java/frc/robot/subsystems/intakes/Intake.java b/src/main/java/frc/robot/subsystems/intakes/Intake.java
index 25a6153..0a76dce 100644
--- a/src/main/java/frc/robot/subsystems/intakes/Intake.java
+++ b/src/main/java/frc/robot/subsystems/intakes/Intake.java
@@ -87,7 +87,7 @@ private static class PeriodicIO {
     // pivotConfig.absoluteEncoder.zeroOffset(RobotConstants.robotConfig.Intake.k_rightPivotOffset);
     // }
 
-    rollerConfig.smartCurrentLimit(RobotConstants.robotConfig.Intake.k_rollerCurrentLimit);
+    // rollerConfig.smartCurrentLimit(RobotConstants.robotConfig.Intake.k_rollerCurrentLimit);
     pivotConfig.smartCurrentLimit(RobotConstants.robotConfig.Intake.k_pivotCurrentLimit);
 
     pivotConfig.inverted(true);
@@ -174,11 +174,18 @@ public void writePeriodicOutputs() {
         ff,
         ArbFFUnits.kVoltage);
 
+        
     if (getDesiredRollerSpeed() != 0.0) {
-      m_rollerPIDController.setReference(getDesiredRollerSpeed(), ControlType.kVelocity);
+      if(m_periodicIO.desiredIntakeState == IntakeState.ALGAE || m_periodicIO.desiredIntakeState == IntakeState.SCORE_ALGAE) {
+        m_rollerMotor.setVoltage(getDesiredRollerSpeed());
+      } else {
+        m_rollerPIDController.setReference(getDesiredRollerSpeed(), ControlType.kVelocity);
+      }
     } else {
       if (isAtState()) {
         m_rollerPIDController.setReference(0.0, ControlType.kVoltage);
+      } else {
+        m_rollerPIDController.setReference(RobotConstants.robotConfig.Intake.k_stowingIntakeSpeed, ControlType.kVelocity);
       }
     }
   }
@@ -224,6 +231,15 @@ public double getDesiredRollerSpeed() {
       case EJECT -> {
         return -RobotConstants.robotConfig.Intake.k_maxIntakeSpeed;
       }
+      case END_EJECT -> {
+        return 0.0;
+      }
+      case ALGAE -> {
+        return -3.5; //Volts
+      }
+      case SCORE_ALGAE -> {
+        return 3.5; // Volts
+      }
       default -> {
         return 0.0;
       }
@@ -247,10 +263,23 @@ public double getTargetPivotAngle(IntakeState state) {
         }
       }
       case EJECT -> {
+        return getPivotAngle();
+      }
+      case END_EJECT -> {
+        return getPivotAngle();
+      }
+      case ALGAE -> {
+        if (m_intakeName.equalsIgnoreCase("Left")) {
+          return RobotConstants.robotConfig.Intake.Left.k_algaePosition;
+        } else {
+          return RobotConstants.robotConfig.Intake.Right.k_algaePosition;
+        }
+      }
+      case SCORE_ALGAE -> {
         if (m_intakeName.equalsIgnoreCase("Left")) {
-          return getPivotAngle();
+          return RobotConstants.robotConfig.Intake.Left.k_algaePosition;
         } else {
-          return getPivotAngle();
+          return RobotConstants.robotConfig.Intake.Right.k_algaePosition;
         }
       }
       case STUCK -> {
@@ -325,6 +354,9 @@ public enum IntakeState {
     STOW,
     INTAKE,
     STUCK,
-    EJECT
+    EJECT,
+    END_EJECT,
+    ALGAE,
+    SCORE_ALGAE
   }
 }
diff --git a/src/main/java/frc/robot/subsystems/leds/LEDs.java b/src/main/java/frc/robot/subsystems/leds/LEDs.java
index 6cf80a3..5967bba 100644
--- a/src/main/java/frc/robot/subsystems/leds/LEDs.java
+++ b/src/main/java/frc/robot/subsystems/leds/LEDs.java
@@ -97,7 +97,7 @@ public void setRightColors(
 
   public void setColorFromElevatorState(ElevatorState state) {
     switch (state) {
-      case L2:
+      case L2 ->
         setAllColorModes(
             LEDModes.setColor(Color.kRed),
             LEDModes.setColor(Color.kRed),
@@ -107,8 +107,7 @@ public void setColorFromElevatorState(ElevatorState state) {
             LEDModes.setColor(Color.kBlack),
             LEDModes.setColor(Color.kRed),
             LEDModes.setColor(Color.kRed));
-        break;
-      case L3:
+      case L3 ->
         setAllColorModes(
             LEDModes.setColor(Color.kRed),
             LEDModes.setColor(Color.kRed),
@@ -118,8 +117,7 @@ public void setColorFromElevatorState(ElevatorState state) {
             LEDModes.setColor(Color.kRed),
             LEDModes.setColor(Color.kRed),
             LEDModes.setColor(Color.kRed));
-        break;
-      case L4:
+      case L4 ->
         setAllColorModes(
             LEDModes.setColor(Color.kRed),
             LEDModes.setColor(Color.kRed),
@@ -129,7 +127,8 @@ public void setColorFromElevatorState(ElevatorState state) {
             LEDModes.setColor(Color.kRed),
             LEDModes.setColor(Color.kRed),
             LEDModes.setColor(Color.kRed));
-        break;
+      default -> {
+      }
     }
   }
 
diff --git a/vendordeps/AdvantageKit.json b/vendordeps/AdvantageKit.json
index bb5276a..bef4a15 100644
--- a/vendordeps/AdvantageKit.json
+++ b/vendordeps/AdvantageKit.json
@@ -1,35 +1,35 @@
 {
     "fileName": "AdvantageKit.json",
     "name": "AdvantageKit",
-    "version": "4.1.1",
+    "version": "4.1.2",
     "uuid": "d820cc26-74e3-11ec-90d6-0242ac120003",
     "frcYear": "2025",
     "mavenUrls": [
-      "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/"
+        "https://frcmaven.wpi.edu/artifactory/littletonrobotics-mvn-release/"
     ],
     "jsonUrl": "https://github.com/Mechanical-Advantage/AdvantageKit/releases/latest/download/AdvantageKit.json",
     "javaDependencies": [
-      {
-        "groupId": "org.littletonrobotics.akit",
-        "artifactId": "akit-java",
-        "version": "4.1.1"
-      }
+        {
+            "groupId": "org.littletonrobotics.akit",
+            "artifactId": "akit-java",
+            "version": "4.1.2"
+        }
     ],
     "jniDependencies": [
-      {
-        "groupId": "org.littletonrobotics.akit",
-        "artifactId": "akit-wpilibio",
-        "version": "4.1.1",
-        "skipInvalidPlatforms": false,
-        "isJar": false,
-        "validPlatforms": [
-          "linuxathena",
-          "linuxx86-64",
-          "linuxarm64",
-          "osxuniversal",
-          "windowsx86-64"
-        ]
-      }
+        {
+            "groupId": "org.littletonrobotics.akit",
+            "artifactId": "akit-wpilibio",
+            "version": "4.1.2",
+            "skipInvalidPlatforms": false,
+            "isJar": false,
+            "validPlatforms": [
+                "linuxathena",
+                "linuxx86-64",
+                "linuxarm64",
+                "osxuniversal",
+                "windowsx86-64"
+            ]
+        }
     ],
     "cppDependencies": []
-  }
\ No newline at end of file
+}
\ No newline at end of file
diff --git a/vendordeps/Phoenix6-25.3.1.json b/vendordeps/Phoenix6-25.3.2.json
similarity index 92%
rename from vendordeps/Phoenix6-25.3.1.json
rename to vendordeps/Phoenix6-25.3.2.json
index a216d97..29187a8 100644
--- a/vendordeps/Phoenix6-25.3.1.json
+++ b/vendordeps/Phoenix6-25.3.2.json
@@ -1,7 +1,7 @@
 {
-    "fileName": "Phoenix6-25.3.1.json",
+    "fileName": "Phoenix6-25.3.2.json",
     "name": "CTRE-Phoenix (v6)",
-    "version": "25.3.1",
+    "version": "25.3.2",
     "frcYear": "2025",
     "uuid": "e995de00-2c64-4df5-8831-c1441420ff19",
     "mavenUrls": [
@@ -19,14 +19,14 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-java",
-            "version": "25.3.1"
+            "version": "25.3.2"
         }
     ],
     "jniDependencies": [
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "api-cpp",
-            "version": "25.3.1",
+            "version": "25.3.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -40,7 +40,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "25.3.1",
+            "version": "25.3.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -54,7 +54,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "api-cpp-sim",
-            "version": "25.3.1",
+            "version": "25.3.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -68,7 +68,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "25.3.1",
+            "version": "25.3.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -82,7 +82,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "25.3.1",
+            "version": "25.3.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -96,7 +96,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "25.3.1",
+            "version": "25.3.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -110,7 +110,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "25.3.1",
+            "version": "25.3.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -124,7 +124,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "25.3.1",
+            "version": "25.3.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -138,7 +138,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "25.3.1",
+            "version": "25.3.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -152,7 +152,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFXS",
-            "version": "25.3.1",
+            "version": "25.3.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -166,7 +166,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "25.3.1",
+            "version": "25.3.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -180,7 +180,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "25.3.1",
+            "version": "25.3.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -194,7 +194,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANrange",
-            "version": "25.3.1",
+            "version": "25.3.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -208,7 +208,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANdi",
-            "version": "25.3.1",
+            "version": "25.3.2",
             "isJar": false,
             "skipInvalidPlatforms": true,
             "validPlatforms": [
@@ -224,7 +224,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "wpiapi-cpp",
-            "version": "25.3.1",
+            "version": "25.3.2",
             "libName": "CTRE_Phoenix6_WPI",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -240,7 +240,7 @@
         {
             "groupId": "com.ctre.phoenix6",
             "artifactId": "tools",
-            "version": "25.3.1",
+            "version": "25.3.2",
             "libName": "CTRE_PhoenixTools",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -256,7 +256,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "wpiapi-cpp-sim",
-            "version": "25.3.1",
+            "version": "25.3.2",
             "libName": "CTRE_Phoenix6_WPISim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -272,7 +272,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "tools-sim",
-            "version": "25.3.1",
+            "version": "25.3.2",
             "libName": "CTRE_PhoenixTools_Sim",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -288,7 +288,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simTalonSRX",
-            "version": "25.3.1",
+            "version": "25.3.2",
             "libName": "CTRE_SimTalonSRX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -304,7 +304,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simVictorSPX",
-            "version": "25.3.1",
+            "version": "25.3.2",
             "libName": "CTRE_SimVictorSPX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -320,7 +320,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simPigeonIMU",
-            "version": "25.3.1",
+            "version": "25.3.2",
             "libName": "CTRE_SimPigeonIMU",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -336,7 +336,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simCANCoder",
-            "version": "25.3.1",
+            "version": "25.3.2",
             "libName": "CTRE_SimCANCoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -352,7 +352,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFX",
-            "version": "25.3.1",
+            "version": "25.3.2",
             "libName": "CTRE_SimProTalonFX",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -368,7 +368,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProTalonFXS",
-            "version": "25.3.1",
+            "version": "25.3.2",
             "libName": "CTRE_SimProTalonFXS",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -384,7 +384,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANcoder",
-            "version": "25.3.1",
+            "version": "25.3.2",
             "libName": "CTRE_SimProCANcoder",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -400,7 +400,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProPigeon2",
-            "version": "25.3.1",
+            "version": "25.3.2",
             "libName": "CTRE_SimProPigeon2",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -416,7 +416,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANrange",
-            "version": "25.3.1",
+            "version": "25.3.2",
             "libName": "CTRE_SimProCANrange",
             "headerClassifier": "headers",
             "sharedLibrary": true,
@@ -432,7 +432,7 @@
         {
             "groupId": "com.ctre.phoenix6.sim",
             "artifactId": "simProCANdi",
-            "version": "25.3.1",
+            "version": "25.3.2",
             "libName": "CTRE_SimProCANdi",
             "headerClassifier": "headers",
             "sharedLibrary": true,


Reviews:
user: login: GingerJakeDaBoi
body: Elastic is here, isn't that bad?
state: CHANGES_REQUESTED
author_association: CONTRIBUTOR
submitted_at: 2025-04-06T22:43:10Z
user: login: GingerJakeDaBoi
state: APPROVED
author_association: CONTRIBUTOR
submitted_at: 2025-04-08T22:53:51Z
user: login: websitedeb
body: 👌
state: APPROVED
author_association: CONTRIBUTOR
submitted_at: 2025-04-08T22:57:50Z
user: login: NotMePipe
body: Many changes, looks meh
state: CHANGES_REQUESTED
author_association: CONTRIBUTOR
submitted_at: 2025-04-08T23:01:21Z
user: login: NotMePipe
state: APPROVED
author_association: CONTRIBUTOR
submitted_at: 2025-04-08T23:20:43Z

Review comments:
diff_hunk: @@ -196,6 +205,9 @@ public void autonomousPeriodic() {
 
   @Override
   public void teleopInit() {
+    // Elastic.selectTab("Teleoperated");
+    m_endEffector.shouldBeIndexingCoral = false;
path: src/main/java/frc/robot/Robot.java
user: login: NotMePipe
body: Why do we not have a setter method? This seems very unsafe
created_at: 2025-04-08T22:55:34Z
author_association: CONTRIBUTOR
diff_hunk: @@ -17,6 +18,12 @@ public IntakeTask(IntakeVariant intakeVariant, IntakeState intakeState) {
 
   @Override
   public void prepare() {
+    if (m_intakeState == IntakeState.STOW) {
path: src/main/java/frc/robot/autonomous/tasks/IntakeTask.java
user: login: NotMePipe
body: Again, no setter method, bad
created_at: 2025-04-08T22:57:01Z
author_association: CONTRIBUTOR
diff_hunk: @@ -56,8 +55,8 @@ public Branch getWantsAutoPositionBranch() {
     return Branch.NONE;
   }
 
-  public boolean getWantsAutoPositionPressed() {
-    return this.getRawButtonPressed(Button.X);
+  public boolean getWantsAutoScorePlusAlgaePressed() {
path: src/main/java/frc/robot/controls/controllers/DriverController.java
user: login: NotMePipe
body: AHH Bad naming scheme, should be `getWantsAutoScorePlusAlgae()`
created_at: 2025-04-08T22:58:26Z
author_association: CONTRIBUTOR
diff_hunk: @@ -87,7 +87,7 @@ private static class PeriodicIO {
     // pivotConfig.absoluteEncoder.zeroOffset(RobotConstants.robotConfig.Intake.k_rightPivotOffset);
     // }
 
-    rollerConfig.smartCurrentLimit(RobotConstants.robotConfig.Intake.k_rollerCurrentLimit);
+    // rollerConfig.smartCurrentLimit(RobotConstants.robotConfig.Intake.k_rollerCurrentLimit);
path: src/main/java/frc/robot/subsystems/intakes/Intake.java
user: login: NotMePipe
body: This worked but kinda scares me
created_at: 2025-04-08T23:00:27Z
author_association: CONTRIBUTOR
path: src/main/java/frc/robot/subsystems/leds/LEDs.java
user: login: NotMePipe
body: If we choose to remove Elastic.java, I don't see much of an argument to keep the LEDs classes 🤷 
created_at: 2025-04-08T23:01:05Z
author_association: CONTRIBUTOR

PR comments:
user: login: NotMePipe
created_at: 2025-04-06T22:51:41Z
body: > Elastic is here, isn't that bad?

The Elastic.java isn't referenced anywhere and is just the library
author_association: CONTRIBUTOR
user: login: GingerJakeDaBoi
created_at: 2025-04-07T00:07:06Z
body: > > Elastic is here, isn't that bad?
> 
> The Elastic.java isn't referenced anywhere and is just the library

In that case, it should be removed before pushing to master.
author_association: CONTRIBUTOR
user: login: kryllyxofficial01
created_at: 2025-04-08T22:53:52Z
body: includes some of the processor stuff but its fiiiiine
author_association: CONTRIBUTOR