diff --git a/.SysId/sysid.json b/.SysId/sysid.json
new file mode 100644
index 0000000..0967ef4
--- /dev/null
+++ b/.SysId/sysid.json
@@ -0,0 +1 @@
+{}
diff --git a/.vscode/settings.json b/.vscode/settings.json
index 660d196..eae3fe5 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -18,7 +18,9 @@
     {
       "name": "WPIlibUnitTests",
       "workingDirectory": "${workspaceFolder}/build/jni/release",
-      "vmargs": ["-Djava.library.path=${workspaceFolder}/build/jni/release"],
+      "vmargs": [
+        "-Djava.library.path=${workspaceFolder}/build/jni/release"
+      ],
       "env": {
         "LD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release",
         "DYLD_LIBRARY_PATH": "${workspaceFolder}/build/jni/release"
diff --git a/AdvantageScope Swerve Calibration.json b/AdvantageScope Swerve Calibration.json
index f412ed2..513c508 100644
--- a/AdvantageScope Swerve Calibration.json	
+++ b/AdvantageScope Swerve Calibration.json	
@@ -1,10 +1,10 @@
 {
   "hubs": [
     {
-      "x": 139,
-      "y": 95,
-      "width": 1100,
-      "height": 650,
+      "x": -8,
+      "y": -8,
+      "width": 1936,
+      "height": 1048,
       "state": {
         "sidebar": {
           "width": 300,
@@ -13,11 +13,19 @@
             "/RealOutputs",
             "/RealOutputs/SwerveStates",
             "/RealOutputs/SwerveChassisSpeeds",
-            "/RealOutputs/Odometry"
+            "/RealOutputs/Odometry",
+            "/AdvantageKit/RealOutputs",
+            "/AdvantageKit/RealOutputs/Odometry/Robot",
+            "/AdvantageKit/RealOutputs/Vision/Camera1",
+            "/SmartDashboard",
+            "/AdvantageKit/RealOutputs/Superstructure",
+            "/AdvantageKit",
+            "/AdvantageKit/Intake",
+            "/AdvantageKit/Superstructure"
           ]
         },
         "tabs": {
-          "selected": 1,
+          "selected": 2,
           "tabs": [
             {
               "type": 0,
@@ -76,6 +84,51 @@
                     "logType": "Rotation2d",
                     "visible": true,
                     "options": {}
+                  },
+                  {
+                    "type": "states",
+                    "logKey": "NT:/AdvantageKit/RealOutputs/SwerveStates/Measured",
+                    "logType": "SwerveModuleState[]",
+                    "visible": true,
+                    "options": {
+                      "color": "#0000ff",
+                      "arrangement": "0,1,2,3"
+                    }
+                  },
+                  {
+                    "type": "states",
+                    "logKey": "NT:/AdvantageKit/RealOutputs/SwerveStates/SetpointsOptimized",
+                    "logType": "SwerveModuleState[]",
+                    "visible": true,
+                    "options": {
+                      "color": "#00ff00",
+                      "arrangement": "0,1,2,3"
+                    }
+                  },
+                  {
+                    "type": "chassisSpeeds",
+                    "logKey": "NT:/AdvantageKit/RealOutputs/SwerveChassisSpeeds/Measured",
+                    "logType": "ChassisSpeeds",
+                    "visible": true,
+                    "options": {
+                      "color": "#ffff00"
+                    }
+                  },
+                  {
+                    "type": "chassisSpeeds",
+                    "logKey": "NT:/AdvantageKit/RealOutputs/SwerveChassisSpeeds/Setpoints",
+                    "logType": "ChassisSpeeds",
+                    "visible": true,
+                    "options": {
+                      "color": "#ff8c00"
+                    }
+                  },
+                  {
+                    "type": "rotation",
+                    "logKey": "NT:/AdvantageKit/RealOutputs/Odometry/Robot/rotation",
+                    "logType": "Rotation2d",
+                    "visible": true,
+                    "options": {}
                   }
                 ],
                 "maxSpeed": 5,
@@ -120,9 +173,45 @@
                       "model": "Crab Bot",
                       "color": "#ff8c00"
                     }
+                  },
+                  {
+                    "type": "robot",
+                    "logKey": "NT:/AdvantageKit/RealOutputs/Odometry/Robot",
+                    "logType": "Pose2d",
+                    "visible": true,
+                    "options": {
+                      "model": "2024 KitBot"
+                    }
+                  },
+                  {
+                    "type": "vision",
+                    "logKey": "NT:/AdvantageKit/RealOutputs/Vision/Camera0/TagPoses",
+                    "logType": "Pose3d[]",
+                    "visible": true,
+                    "options": {
+                      "color": "#00ff00",
+                      "size": "normal"
+                    }
+                  },
+                  {
+                    "type": "vision",
+                    "logKey": "NT:/AdvantageKit/RealOutputs/Vision/Camera1/TagPoses",
+                    "logType": "Pose3d[]",
+                    "visible": true,
+                    "options": {
+                      "color": "#00ff00",
+                      "size": "normal"
+                    }
+                  },
+                  {
+                    "type": "mechanism",
+                    "logKey": "NT:/SmartDashboard/Intake/Mechanism",
+                    "logType": "Mechanism2d",
+                    "visible": true,
+                    "options": {}
                   }
                 ],
-                "game": "2025 Field",
+                "game": "2025 Field (Welded)",
                 "origin": "blue"
               },
               "controllerUUID": "psf0y633oclnjyocus23hcnq1d4tpyte",
@@ -130,17 +219,32 @@
                 "cameraIndex": -1,
                 "orbitFov": 50,
                 "cameraPosition": [
-                  1.4695761589768238e-15,
-                  5.999999999999999,
-                  -12
+                  5.19113927039247,
+                  5.88432583191309,
+                  -3.632652004468072
                 ],
                 "cameraTarget": [
-                  0,
-                  0.5,
-                  0
+                  5.830818917232525,
+                  0.5503848177969854,
+                  0.6221527170045741
                 ]
               },
-              "controlsHeight": 200
+              "controlsHeight": 100
+            },
+            {
+              "type": 8,
+              "title": "Joysticks",
+              "controller": [
+                "Xbox Controller (White)",
+                "None",
+                "None",
+                "None",
+                "None",
+                "None"
+              ],
+              "controllerUUID": "txhox0ps7gozefkq4lb225n2hyhj2s3h",
+              "renderer": null,
+              "controlsHeight": 85
             },
             {
               "type": 1,
@@ -459,5 +563,5 @@
     }
   ],
   "satellites": [],
-  "version": "4.0.0-beta-3"
+  "version": "4.1.5"
 }
diff --git a/Current AdvantageScope.json b/Current AdvantageScope.json
new file mode 100644
index 0000000..b1de72e
--- /dev/null
+++ b/Current AdvantageScope.json	
@@ -0,0 +1,550 @@
+{
+  "hubs": [
+    {
+      "x": -8,
+      "y": -8,
+      "width": 1936,
+      "height": 1048,
+      "state": {
+        "sidebar": {
+          "width": 300,
+          "expanded": [
+            "/Drive",
+            "/RealOutputs",
+            "/RealOutputs/SwerveStates",
+            "/RealOutputs/SwerveChassisSpeeds",
+            "/RealOutputs/Odometry",
+            "/AdvantageKit/RealOutputs",
+            "/AdvantageKit/RealOutputs/Odometry/Robot",
+            "/AdvantageKit/RealOutputs/Vision",
+            "/AdvantageKit/RealOutputs/Vision/Camera1",
+            "/SmartDashboard",
+            "/SmartDashboard/Intake"
+          ]
+        },
+        "tabs": {
+          "selected": 2,
+          "tabs": [
+            {
+              "type": 0,
+              "title": "",
+              "controller": null,
+              "controllerUUID": "grimvlgu9aluj8btvwotco2wpj0pto2h",
+              "renderer": "#/",
+              "controlsHeight": 0
+            },
+            {
+              "type": 9,
+              "title": "Drive Overview",
+              "controller": {
+                "sources": [
+                  {
+                    "type": "states",
+                    "logKey": "/RealOutputs/SwerveStates/Measured",
+                    "logType": "SwerveModuleState[]",
+                    "visible": true,
+                    "options": {
+                      "color": "#ff0000",
+                      "arrangement": "0,1,2,3"
+                    }
+                  },
+                  {
+                    "type": "states",
+                    "logKey": "/RealOutputs/SwerveStates/SetpointsOptimized",
+                    "logType": "SwerveModuleState[]",
+                    "visible": true,
+                    "options": {
+                      "color": "#00ffff",
+                      "arrangement": "0,1,2,3"
+                    }
+                  },
+                  {
+                    "type": "chassisSpeeds",
+                    "logKey": "/RealOutputs/SwerveChassisSpeeds/Measured",
+                    "logType": "ChassisSpeeds",
+                    "visible": true,
+                    "options": {
+                      "color": "#ff0000"
+                    }
+                  },
+                  {
+                    "type": "chassisSpeeds",
+                    "logKey": "/RealOutputs/SwerveChassisSpeeds/Setpoints",
+                    "logType": "ChassisSpeeds",
+                    "visible": true,
+                    "options": {
+                      "color": "#00ffff"
+                    }
+                  },
+                  {
+                    "type": "rotation",
+                    "logKey": "/RealOutputs/Odometry/Robot/rotation",
+                    "logType": "Rotation2d",
+                    "visible": true,
+                    "options": {}
+                  },
+                  {
+                    "type": "states",
+                    "logKey": "NT:/AdvantageKit/RealOutputs/SwerveStates/Measured",
+                    "logType": "SwerveModuleState[]",
+                    "visible": true,
+                    "options": {
+                      "color": "#0000ff",
+                      "arrangement": "0,1,2,3"
+                    }
+                  },
+                  {
+                    "type": "states",
+                    "logKey": "NT:/AdvantageKit/RealOutputs/SwerveStates/SetpointsOptimized",
+                    "logType": "SwerveModuleState[]",
+                    "visible": true,
+                    "options": {
+                      "color": "#00ff00",
+                      "arrangement": "0,1,2,3"
+                    }
+                  },
+                  {
+                    "type": "chassisSpeeds",
+                    "logKey": "NT:/AdvantageKit/RealOutputs/SwerveChassisSpeeds/Measured",
+                    "logType": "ChassisSpeeds",
+                    "visible": true,
+                    "options": {
+                      "color": "#ffff00"
+                    }
+                  },
+                  {
+                    "type": "chassisSpeeds",
+                    "logKey": "NT:/AdvantageKit/RealOutputs/SwerveChassisSpeeds/Setpoints",
+                    "logType": "ChassisSpeeds",
+                    "visible": true,
+                    "options": {
+                      "color": "#ff8c00"
+                    }
+                  },
+                  {
+                    "type": "rotation",
+                    "logKey": "NT:/AdvantageKit/RealOutputs/Odometry/Robot/rotation",
+                    "logType": "Rotation2d",
+                    "visible": true,
+                    "options": {}
+                  }
+                ],
+                "maxSpeed": 5,
+                "sizeX": 0.65,
+                "sizeY": 0.65,
+                "orientation": 1
+              },
+              "controllerUUID": "prdx7t2eedan6n46dxrjfu7eisf16f9o",
+              "renderer": null,
+              "controlsHeight": 200
+            },
+            {
+              "type": 3,
+              "title": "Field",
+              "controller": {
+                "sources": [
+                  {
+                    "type": "robot",
+                    "logKey": "/RealOutputs/Odometry/Robot",
+                    "logType": "Pose2d",
+                    "visible": true,
+                    "options": {
+                      "model": "Crab Bot"
+                    }
+                  },
+                  {
+                    "type": "trajectory",
+                    "logKey": "/RealOutputs/Odometry/Trajectory",
+                    "logType": "Pose2d[]",
+                    "visible": true,
+                    "options": {
+                      "color": "#ff8c00",
+                      "size": "normal"
+                    }
+                  },
+                  {
+                    "type": "ghost",
+                    "logKey": "/RealOutputs/Odometry/TrajectorySetpoint",
+                    "logType": "Pose2d",
+                    "visible": true,
+                    "options": {
+                      "model": "Crab Bot",
+                      "color": "#ff8c00"
+                    }
+                  },
+                  {
+                    "type": "robot",
+                    "logKey": "NT:/AdvantageKit/RealOutputs/Odometry/Robot",
+                    "logType": "Pose2d",
+                    "visible": true,
+                    "options": {
+                      "model": "2024 KitBot"
+                    }
+                  },
+                  {
+                    "type": "vision",
+                    "logKey": "NT:/AdvantageKit/RealOutputs/Vision/Camera0/TagPoses",
+                    "logType": "Pose3d[]",
+                    "visible": true,
+                    "options": {
+                      "color": "#00ff00",
+                      "size": "normal"
+                    }
+                  },
+                  {
+                    "type": "vision",
+                    "logKey": "NT:/AdvantageKit/RealOutputs/Vision/Camera1/TagPoses",
+                    "logType": "Pose3d[]",
+                    "visible": true,
+                    "options": {
+                      "color": "#00ff00",
+                      "size": "normal"
+                    }
+                  },
+                  {
+                    "type": "mechanism",
+                    "logKey": "NT:/SmartDashboard/Intake/Mechanism",
+                    "logType": "Mechanism2d",
+                    "visible": true,
+                    "options": {}
+                  }
+                ],
+                "game": "2025 Field (Welded)",
+                "origin": "blue"
+              },
+              "controllerUUID": "psf0y633oclnjyocus23hcnq1d4tpyte",
+              "renderer": {
+                "cameraIndex": -1,
+                "orbitFov": 50,
+                "cameraPosition": [
+                  4.889792955692206,
+                  1.4477331817350256,
+                  -1.5030132683871433
+                ],
+                "cameraTarget": [
+                  5.9462424992067175,
+                  0.5503848177969854,
+                  0.6337024678673298
+                ]
+              },
+              "controlsHeight": 200
+            },
+            {
+              "type": 1,
+              "title": "Turn Calibration",
+              "controller": {
+                "leftSources": [
+                  {
+                    "type": "stepped",
+                    "logKey": "/Drive/Module0/TurnPosition/value",
+                    "logType": "Number",
+                    "visible": true,
+                    "options": {
+                      "color": "#e5b31b",
+                      "size": "normal"
+                    }
+                  },
+                  {
+                    "type": "stepped",
+                    "logKey": "/Drive/Module1/TurnPosition/value",
+                    "logType": "Number",
+                    "visible": true,
+                    "options": {
+                      "color": "#af2437",
+                      "size": "normal"
+                    }
+                  },
+                  {
+                    "type": "stepped",
+                    "logKey": "/Drive/Module2/TurnPosition/value",
+                    "logType": "Number",
+                    "visible": true,
+                    "options": {
+                      "color": "#80588e",
+                      "size": "normal"
+                    }
+                  },
+                  {
+                    "type": "stepped",
+                    "logKey": "/Drive/Module3/TurnPosition/value",
+                    "logType": "Number",
+                    "visible": true,
+                    "options": {
+                      "color": "#e48b32",
+                      "size": "normal"
+                    }
+                  }
+                ],
+                "rightSources": [],
+                "discreteSources": [],
+                "leftLockedRange": null,
+                "rightLockedRange": null,
+                "leftUnitConversion": {
+                  "type": null,
+                  "factor": 1
+                },
+                "rightUnitConversion": {
+                  "type": null,
+                  "factor": 1
+                },
+                "leftFilter": 0,
+                "rightFilter": 0
+              },
+              "controllerUUID": "ol8lk80m83ma3aegae849d6k1d29sp7d",
+              "renderer": null,
+              "controlsHeight": 200
+            },
+            {
+              "type": 1,
+              "title": "Drive Calibration",
+              "controller": {
+                "leftSources": [
+                  {
+                    "type": "stepped",
+                    "logKey": "/Drive/Module0/DrivePositionRad",
+                    "logType": "Number",
+                    "visible": true,
+                    "options": {
+                      "color": "#e5b31b",
+                      "size": "normal"
+                    }
+                  },
+                  {
+                    "type": "stepped",
+                    "logKey": "/Drive/Module1/DrivePositionRad",
+                    "logType": "Number",
+                    "visible": true,
+                    "options": {
+                      "color": "#af2437",
+                      "size": "normal"
+                    }
+                  },
+                  {
+                    "type": "stepped",
+                    "logKey": "/Drive/Module2/DrivePositionRad",
+                    "logType": "Number",
+                    "visible": true,
+                    "options": {
+                      "color": "#80588e",
+                      "size": "normal"
+                    }
+                  },
+                  {
+                    "type": "stepped",
+                    "logKey": "/Drive/Module3/DrivePositionRad",
+                    "logType": "Number",
+                    "visible": true,
+                    "options": {
+                      "color": "#e48b32",
+                      "size": "normal"
+                    }
+                  }
+                ],
+                "rightSources": [],
+                "discreteSources": [],
+                "leftLockedRange": null,
+                "rightLockedRange": null,
+                "leftUnitConversion": {
+                  "type": null,
+                  "factor": 1
+                },
+                "rightUnitConversion": {
+                  "type": null,
+                  "factor": 1
+                },
+                "leftFilter": 0,
+                "rightFilter": 0
+              },
+              "controllerUUID": "7jj1f83nsv59dd2ou11ls3ccz5tq1293",
+              "renderer": null,
+              "controlsHeight": 200
+            },
+            {
+              "type": 1,
+              "title": "Turn PID Tuning",
+              "controller": {
+                "leftSources": [
+                  {
+                    "type": "stepped",
+                    "logKey": "/RealOutputs/SwerveStates/Measured/0/angle/value",
+                    "logType": "Number",
+                    "visible": true,
+                    "options": {
+                      "color": "#2b66a2",
+                      "size": "normal"
+                    }
+                  },
+                  {
+                    "type": "stepped",
+                    "logKey": "/RealOutputs/SwerveStates/SetpointsOptimized/0/angle/value",
+                    "logType": "Number",
+                    "visible": true,
+                    "options": {
+                      "color": "#e5b31b",
+                      "size": "normal"
+                    }
+                  }
+                ],
+                "rightSources": [],
+                "discreteSources": [],
+                "leftLockedRange": null,
+                "rightLockedRange": null,
+                "leftUnitConversion": {
+                  "type": null,
+                  "factor": 1
+                },
+                "rightUnitConversion": {
+                  "type": null,
+                  "factor": 1
+                },
+                "leftFilter": 0,
+                "rightFilter": 0
+              },
+              "controllerUUID": "kj6tf6sxm5g04wnjrhed6mx2wpccmqtn",
+              "renderer": null,
+              "controlsHeight": 200
+            },
+            {
+              "type": 1,
+              "title": "Drive PID Tuning",
+              "controller": {
+                "leftSources": [
+                  {
+                    "type": "stepped",
+                    "logKey": "/RealOutputs/SwerveStates/Measured/0/speed",
+                    "logType": "Number",
+                    "visible": true,
+                    "options": {
+                      "color": "#2b66a2",
+                      "size": "normal"
+                    }
+                  },
+                  {
+                    "type": "stepped",
+                    "logKey": "/RealOutputs/SwerveStates/SetpointsOptimized/0/speed",
+                    "logType": "Number",
+                    "visible": true,
+                    "options": {
+                      "color": "#e5b31b",
+                      "size": "normal"
+                    }
+                  }
+                ],
+                "rightSources": [],
+                "discreteSources": [],
+                "leftLockedRange": null,
+                "rightLockedRange": null,
+                "leftUnitConversion": {
+                  "type": null,
+                  "factor": 1
+                },
+                "rightUnitConversion": {
+                  "type": null,
+                  "factor": 1
+                },
+                "leftFilter": 0,
+                "rightFilter": 0
+              },
+              "controllerUUID": "bctxg9pmwpo31m9bv9ofr92oqqtbpqzp",
+              "renderer": null,
+              "controlsHeight": 200
+            },
+            {
+              "type": 1,
+              "title": "Max Speed Measurement",
+              "controller": {
+                "leftSources": [
+                  {
+                    "type": "stepped",
+                    "logKey": "/RealOutputs/SwerveChassisSpeeds/Measured/vx",
+                    "logType": "Number",
+                    "visible": true,
+                    "options": {
+                      "color": "#2b66a2",
+                      "size": "normal"
+                    }
+                  },
+                  {
+                    "type": "stepped",
+                    "logKey": "/RealOutputs/SwerveChassisSpeeds/Measured/vy",
+                    "logType": "Number",
+                    "visible": true,
+                    "options": {
+                      "color": "#e5b31b",
+                      "size": "normal"
+                    }
+                  }
+                ],
+                "rightSources": [],
+                "discreteSources": [],
+                "leftLockedRange": null,
+                "rightLockedRange": null,
+                "leftUnitConversion": {
+                  "type": null,
+                  "factor": 1
+                },
+                "rightUnitConversion": {
+                  "type": null,
+                  "factor": 1
+                },
+                "leftFilter": 0,
+                "rightFilter": 0
+              },
+              "controllerUUID": "oy3vmjuhr7g36evikwnmjhfacl30mzn5",
+              "renderer": null,
+              "controlsHeight": 200
+            },
+            {
+              "type": 1,
+              "title": "Slip Current Measurement",
+              "controller": {
+                "leftSources": [
+                  {
+                    "type": "stepped",
+                    "logKey": "/Drive/Module0/DriveCurrentAmps",
+                    "logType": "Number",
+                    "visible": true,
+                    "options": {
+                      "color": "#2b66a2",
+                      "size": "normal"
+                    }
+                  }
+                ],
+                "rightSources": [
+                  {
+                    "type": "stepped",
+                    "logKey": "/Drive/Module0/DriveVelocityRadPerSec",
+                    "logType": "Number",
+                    "visible": true,
+                    "options": {
+                      "color": "#e5b31b",
+                      "size": "normal"
+                    }
+                  }
+                ],
+                "discreteSources": [],
+                "leftLockedRange": null,
+                "rightLockedRange": null,
+                "leftUnitConversion": {
+                  "type": null,
+                  "factor": 1
+                },
+                "rightUnitConversion": {
+                  "type": null,
+                  "factor": 1
+                },
+                "leftFilter": 0,
+                "rightFilter": 0
+              },
+              "controllerUUID": "efudylbvebbv1ga2kosknov0uegmtj7h",
+              "renderer": null,
+              "controlsHeight": 200
+            }
+          ]
+        }
+      }
+    }
+  ],
+  "satellites": [],
+  "version": "4.1.5"
+}
diff --git a/README.md b/README.md
index c3a6fe9..7025d0b 100644
--- a/README.md
+++ b/README.md
@@ -56,4 +56,4 @@ double rotations = positionSignal.getValue().in(Units.Rotations);
 
 // Incorrect (won't compile)
 StatusSignal<Double> positionSignal = motor.getPosition(); // Type mismatch!
-```
\ No newline at end of file
+```
diff --git a/elastic-layout.json b/elastic-layout.json
new file mode 100644
index 0000000..7da7976
--- /dev/null
+++ b/elastic-layout.json
@@ -0,0 +1,279 @@
+{
+  "version": 1.0,
+  "grid_size": 128,
+  "tabs": [
+    {
+      "name": "Main",
+      "grid_layout": {
+        "layouts": [
+          {
+            "title": "Robot Status",
+            "x": 768.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 384.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "Match Time",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Shuffleboard/Main/Robot Status/Match Time",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "Battery Voltage",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Voltage View",
+                "properties": {
+                  "topic": "/Shuffleboard/Main/Robot Status/Battery Voltage",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "min_value": 4.0,
+                  "max_value": 13.0,
+                  "divisions": 5,
+                  "inverted": false,
+                  "orientation": "horizontal"
+                }
+              },
+              {
+                "title": "Alliance",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Shuffleboard/Main/Robot Status/Alliance",
+                  "period": 0.06,
+                  "data_type": "string",
+                  "show_submit_button": false
+                }
+              }
+            ]
+          },
+          {
+            "title": "Superstructure",
+            "x": 1024.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 384.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "Current State",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Shuffleboard/Main/Superstructure/Current State",
+                  "period": 0.06,
+                  "data_type": "string",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "Desired State",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Shuffleboard/Main/Superstructure/Desired State",
+                  "period": 0.06,
+                  "data_type": "string",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "At Goal",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Boolean Box",
+                "properties": {
+                  "topic": "/Shuffleboard/Main/Superstructure/At Goal",
+                  "period": 0.06,
+                  "data_type": "boolean",
+                  "true_color": 4283215696,
+                  "false_color": 4294198070,
+                  "true_icon": "None",
+                  "false_icon": "None"
+                }
+              }
+            ]
+          },
+          {
+            "title": "Drive Telemetry",
+            "x": 1280.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 512.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": [
+              {
+                "title": "X Position (m)",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Shuffleboard/Main/Drive Telemetry/X Position (m)",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "Y Position (m)",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Shuffleboard/Main/Drive Telemetry/Y Position (m)",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "Rotation (deg)",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Text Display",
+                "properties": {
+                  "topic": "/Shuffleboard/Main/Drive Telemetry/Rotation (deg)",
+                  "period": 0.06,
+                  "data_type": "double",
+                  "show_submit_button": false
+                }
+              },
+              {
+                "title": "Gyro Yaw (deg)",
+                "x": 0.0,
+                "y": 0.0,
+                "width": 128.0,
+                "height": 128.0,
+                "type": "Gyro",
+                "properties": {
+                  "topic": "/Shuffleboard/Main/Drive Telemetry/Gyro Yaw (deg)",
+                  "period": 0.06,
+                  "counter_clockwise_positive": false
+                }
+              }
+            ]
+          }
+        ],
+        "containers": [
+          {
+            "title": "Field",
+            "x": 0.0,
+            "y": 0.0,
+            "width": 768.0,
+            "height": 512.0,
+            "type": "Field",
+            "properties": {
+              "topic": "/Shuffleboard/Main/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": "FMSInfo",
+            "x": 0.0,
+            "y": 512.0,
+            "width": 384.0,
+            "height": 128.0,
+            "type": "FMSInfo",
+            "properties": {
+              "topic": "/FMSInfo",
+              "period": 0.06
+            }
+          }
+        ]
+      }
+    },
+    {
+      "name": "Vision",
+      "grid_layout": {
+        "layouts": [
+          {
+            "title": "Vision Status",
+            "x": 1280.0,
+            "y": 0.0,
+            "width": 256.0,
+            "height": 512.0,
+            "type": "List Layout",
+            "properties": {
+              "label_position": "TOP"
+            },
+            "children": []
+          }
+        ],
+        "containers": [
+          {
+            "title": "Camera 0",
+            "x": 0.0,
+            "y": 0.0,
+            "width": 640.0,
+            "height": 512.0,
+            "type": "Camera Stream",
+            "properties": {
+              "topic": "/Shuffleboard/Vision/Camera 0",
+              "period": 0.06,
+              "rotation_turns": 0
+            }
+          },
+          {
+            "title": "Camera 1",
+            "x": 640.0,
+            "y": 0.0,
+            "width": 640.0,
+            "height": 512.0,
+            "type": "Camera Stream",
+            "properties": {
+              "topic": "/Shuffleboard/Vision/Camera 1",
+              "period": 0.06,
+              "rotation_turns": 0
+            }
+          }
+        ]
+      }
+    }
+  ]
+}
diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path
index 303dbb2..9a7e5fb 100644
--- a/src/main/deploy/pathplanner/paths/Example Path.path	
+++ b/src/main/deploy/pathplanner/paths/Example Path.path	
@@ -8,36 +8,20 @@
       },
       "prevControl": null,
       "nextControl": {
-        "x": 3.013282910175676,
-        "y": 6.5274984191074985
+        "x": 3.133203374840485,
+        "y": 6.9703859889647495
       },
       "isLocked": false,
       "linkedName": null
     },
     {
       "anchor": {
-        "x": 5.166769560390973,
-        "y": 5.0964860911203305
+        "x": 4.0255714246368255,
+        "y": 6.940640387304872
       },
       "prevControl": {
-        "x": 4.166769560390973,
-        "y": 6.0964860911203305
-      },
-      "nextControl": {
-        "x": 6.166769560390973,
-        "y": 4.0964860911203305
-      },
-      "isLocked": false,
-      "linkedName": null
-    },
-    {
-      "anchor": {
-        "x": 7.0,
-        "y": 1.0
-      },
-      "prevControl": {
-        "x": 6.75,
-        "y": 2.5
+        "x": 2.00287051176512,
+        "y": 7.000131590624627
       },
       "nextControl": null,
       "isLocked": false,
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 8b62b38..8867cbe 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -94,6 +94,9 @@ public void robotPeriodic() {
         // the Command-based framework to work.
         CommandScheduler.getInstance().run();
 
+        // Update Elastic Dashboard
+        robotContainer.updateDashboard();
+
         // Return to normal thread priority
         Threads.setCurrentThreadPriority(false, 10);
     }
@@ -126,10 +129,6 @@ public void autonomousPeriodic() {}
     /** This function is called once when teleop is enabled. */
     @Override
     public void teleopInit() {
-        // This makes sure that the autonomous stops running when
-        // teleop starts running. If you want the autonomous to
-        // continue until interrupted by another command, remove
-        // this line or comment it out.
         if (autonomousCommand != null) {
             autonomousCommand.cancel();
         }
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index 9d8ffb1..a88e488 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -14,14 +14,11 @@
 package frc.robot;
 
 import static edu.wpi.first.units.Units.*;
-import static edu.wpi.first.units.Units.Degrees;
 import static frc.robot.subsystems.vision.VisionConstants.*;
-import static frc.robot.subsystems.vision.VisionConstants.robotToCamera1;
 
 import com.pathplanner.lib.auto.AutoBuilder;
 import edu.wpi.first.math.geometry.Pose2d;
 import edu.wpi.first.math.geometry.Rotation2d;
-import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.wpilibj.GenericHID;
 import edu.wpi.first.wpilibj.XboxController;
 import edu.wpi.first.wpilibj2.command.Command;
@@ -29,12 +26,17 @@
 import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
 import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
 import frc.robot.commands.DriveCommands;
+import frc.robot.commands.SuperstructureCommands;
 import frc.robot.generated.TunerConstants;
 import frc.robot.subsystems.drive.*;
+import frc.robot.subsystems.intake.*;
+import frc.robot.subsystems.superstructure.Superstructure;
+import frc.robot.subsystems.superstructure.SuperstructureIO;
+import frc.robot.subsystems.superstructure.SuperstructureIOReal;
 import frc.robot.subsystems.vision.*;
+import frc.robot.util.ElasticDashboard;
 import org.ironmaple.simulation.SimulatedArena;
 import org.ironmaple.simulation.drivesims.SwerveDriveSimulation;
-import org.ironmaple.simulation.seasonspecific.reefscape2025.ReefscapeCoralOnFly;
 import org.littletonrobotics.junction.Logger;
 import org.littletonrobotics.junction.networktables.LoggedDashboardChooser;
 
@@ -47,11 +49,17 @@ public class RobotContainer {
     // Subsystems
     private final Drive drive;
     private final Vision vision;
+    private final Intake intake;
+    private final Superstructure superstructure;
+
+    // Elastic Dashboard
+    private final ElasticDashboard elasticDashboard;
 
     private SwerveDriveSimulation driveSimulation = null;
 
     // Controller
     private final CommandXboxController controller = new CommandXboxController(0);
+    private final CommandXboxController operator = new CommandXboxController(1);
 
     // Dashboard inputs
     private final LoggedDashboardChooser<Command> autoChooser;
@@ -72,7 +80,8 @@ public RobotContainer() {
                         drive,
                         new VisionIOLimelight(VisionConstants.camera0Name, drive::getRotation),
                         new VisionIOLimelight(VisionConstants.camera1Name, drive::getRotation));
-
+                intake = new Intake(new IntakeIOTalonFXReal());
+                superstructure = new Superstructure(intake, new SuperstructureIOReal() {});
                 break;
             case SIM:
                 // Sim robot, instantiate physics sim IO implementations
@@ -96,7 +105,8 @@ public RobotContainer() {
                                 camera0Name, robotToCamera0, driveSimulation::getSimulatedDriveTrainPose),
                         new VisionIOPhotonVisionSim(
                                 camera1Name, robotToCamera1, driveSimulation::getSimulatedDriveTrainPose));
-
+                intake = new Intake(new IntakeIOTalonFXSim());
+                superstructure = new Superstructure(intake, new SuperstructureIOReal() {});
                 break;
 
             default:
@@ -109,10 +119,14 @@ public RobotContainer() {
                         new ModuleIO() {},
                         (pose) -> {});
                 vision = new Vision(drive, new VisionIO() {}, new VisionIO() {});
-
+                intake = new Intake(new IntakeIO() {});
+                superstructure = new Superstructure(intake, new SuperstructureIO() {});
                 break;
         }
 
+        // Initialize Elastic Dashboard
+        elasticDashboard = new ElasticDashboard();
+
         // Set up auto routines
         autoChooser = new LoggedDashboardChooser<>("Auto Choices", AutoBuilder.buildAutoChooser());
 
@@ -128,6 +142,13 @@ public RobotContainer() {
 
         // Configure the button bindings
         configureButtonBindings();
+
+        // Configure camera streams (adjust URLs if needed)
+        if (Constants.currentMode == Constants.Mode.REAL) {
+            elasticDashboard.setCameraStreams(
+                    "mjpg:http://limelight-" + camera0Name + ".local:5800",
+                    "mjpg:http://limelight-" + camera1Name + ".local:5800");
+        }
     }
 
     /**
@@ -136,51 +157,25 @@ public RobotContainer() {
      * and then passing it to a {@link edu.wpi.first.wpilibj2.command.button.JoystickButton}.
      */
     private void configureButtonBindings() {
+        // ============DRIVER CONTROLLER BINDINGS (DRIVE)============//
         // Default command, normal field-relative drive
         drive.setDefaultCommand(DriveCommands.joystickDrive(
                 drive, () -> -controller.getLeftY(), () -> -controller.getLeftX(), () -> -controller.getRightX()));
 
-        // Lock to 0° when A button is held
-        controller
-                .a()
-                .whileTrue(DriveCommands.joystickDriveAtAngle(
-                        drive, () -> -controller.getLeftY(), () -> -controller.getLeftX(), () -> new Rotation2d()));
-
-        // Switch to X pattern when X button is pressed
-        controller.x().onTrue(Commands.runOnce(drive::stopWithX, drive));
-
         // Reset gyro / odometry
         final Runnable resetGyro = Constants.currentMode == Constants.Mode.SIM
                 ? () -> drive.setPose(
                         driveSimulation.getSimulatedDriveTrainPose()) // reset odometry to actual robot pose during
                 // simulation
                 : () -> drive.setPose(new Pose2d(drive.getPose().getTranslation(), new Rotation2d())); // zero gyro
-        controller.start().onTrue(Commands.runOnce(resetGyro, drive).ignoringDisable(true));
-
-        // Example Coral Placement Code
-        // TODO: delete these code for your own project
-        if (Constants.currentMode == Constants.Mode.SIM) {
-            // L4 placement
-            controller.y().onTrue(Commands.runOnce(() -> SimulatedArena.getInstance()
-                    .addGamePieceProjectile(new ReefscapeCoralOnFly(
-                            driveSimulation.getSimulatedDriveTrainPose().getTranslation(),
-                            new Translation2d(0.4, 0),
-                            driveSimulation.getDriveTrainSimulatedChassisSpeedsFieldRelative(),
-                            driveSimulation.getSimulatedDriveTrainPose().getRotation(),
-                            Meters.of(2),
-                            MetersPerSecond.of(1.5),
-                            Degrees.of(-80)))));
-            // L3 placement
-            controller.b().onTrue(Commands.runOnce(() -> SimulatedArena.getInstance()
-                    .addGamePieceProjectile(new ReefscapeCoralOnFly(
-                            driveSimulation.getSimulatedDriveTrainPose().getTranslation(),
-                            new Translation2d(0.4, 0),
-                            driveSimulation.getDriveTrainSimulatedChassisSpeedsFieldRelative(),
-                            driveSimulation.getSimulatedDriveTrainPose().getRotation(),
-                            Meters.of(1.35),
-                            MetersPerSecond.of(1.5),
-                            Degrees.of(-60)))));
-        }
+        controller.povDown().onTrue(Commands.runOnce(resetGyro, drive).ignoringDisable(true));
+
+        // ============DRIVER CONTROLLER BINDINGS (SYSTEM)============//
+        // Intake
+        controller.leftBumper().whileTrue(SuperstructureCommands.intake(superstructure)); // left bumper
+
+        // Eject
+        controller.rightBumper().whileTrue(SuperstructureCommands.outtake(superstructure)); // right bumper
     }
 
     /**
@@ -209,4 +204,12 @@ public void updateSimulation() {
         Logger.recordOutput(
                 "FieldSimulation/Algae", SimulatedArena.getInstance().getGamePiecesArrayByType("Algae"));
     }
+
+    public void updateDashboard() {
+        elasticDashboard.update(drive, superstructure, intake);
+    }
+
+    public ElasticDashboard getElasticDashboard() {
+        return elasticDashboard;
+    }
 }
diff --git a/src/main/java/frc/robot/commands/SuperstructureCommands.java b/src/main/java/frc/robot/commands/SuperstructureCommands.java
new file mode 100644
index 0000000..caffe7c
--- /dev/null
+++ b/src/main/java/frc/robot/commands/SuperstructureCommands.java
@@ -0,0 +1,36 @@
+package frc.robot.commands;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import frc.robot.subsystems.superstructure.Superstructure;
+import frc.robot.subsystems.superstructure.Superstructure.Goal;
+
+public class SuperstructureCommands {
+
+    // Intake command that intakes while held, and then returns to idle when released
+    public static Command intake(Superstructure superstructure) {
+        return Commands.startEnd(
+                        () -> superstructure.setGoal(Goal.INTAKING),
+                        () -> superstructure.setGoal(Goal.IDLE),
+                        superstructure)
+                .withName("Intake");
+    }
+
+    // Outtake command that intakes while held, and then returns to idle when released
+    public static Command outtake(Superstructure superstructure) {
+        return Commands.startEnd(
+                        () -> superstructure.setGoal(Goal.OUTTAKING),
+                        () -> superstructure.setGoal(Goal.IDLE),
+                        superstructure)
+                .withName("Outtake");
+    }
+
+    // Intake command for autos, waits until the intake has reached its goal
+    public static Command intakeUntilReady(Superstructure superstructure) {
+        return Commands.sequence(
+                        Commands.runOnce(() -> superstructure.setGoal(Goal.INTAKING)),
+                        Commands.waitUntil(superstructure::atGoal))
+                .finallyDo(() -> superstructure.setGoal(Goal.IDLE))
+                .withName("IntakeUntilReady");
+    }
+}
diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java
index 9edd7c1..3ceb45b 100644
--- a/src/main/java/frc/robot/generated/TunerConstants.java
+++ b/src/main/java/frc/robot/generated/TunerConstants.java
@@ -25,7 +25,7 @@ public class TunerConstants {
             .withKI(0)
             .withKD(0.5)
             .withKS(0.1)
-            .withKV(1.91)
+            .withKV(1.79)
             .withKA(0)
             .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
     // When using closed-loop control, the drive motor uses the control
@@ -46,20 +46,19 @@ public class TunerConstants {
     private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated;
 
     // The remote sensor feedback type to use for the steer motors;
-    // When not Pro-licensed, FusedCANcoder/SyncCANcoder automatically fall back to RemoteCANcoder
+    // When not Pro-licensed, Fused*/Sync* automatically fall back to Remote*
     private static final SteerFeedbackType kSteerFeedbackType = SteerFeedbackType.FusedCANcoder;
 
     // The stator current at which the wheels start to slip;
     // This needs to be tuned to your individual robot
-    private static final Current kSlipCurrent = Amps.of(120.0);
+    private static final Current kSlipCurrent = Amps.of(104.0);
 
     // Initial configs for the drive and steer motors and the azimuth encoder; these cannot be null.
     // Some configs will be overwritten; check the `with*InitialConfigs()` API documentation.
     private static final TalonFXConfiguration driveInitialConfigs = new TalonFXConfiguration();
     private static final TalonFXConfiguration steerInitialConfigs = new TalonFXConfiguration()
             .withCurrentLimits(new CurrentLimitsConfigs()
-                    // Swerve azimuth does not require much torque output, so we can set a relatively
-                    // low
+                    // Swerve azimuth does not require much torque output, so we can set a relatively low
                     // stator current limit to help avoid brownouts without impacting performance.
                     .withStatorCurrentLimit(Amps.of(60))
                     .withStatorCurrentLimitEnable(true));
@@ -69,28 +68,28 @@ public class TunerConstants {
 
     // CAN bus that the devices are located on;
     // All swerve devices must share the same CAN bus
-    public static final CANBus kCANBus = new CANBus("canivore", "./logs/example.hoot");
+    public static final CANBus kCANBus = new CANBus("swerve", "./logs/example.hoot");
 
     // Theoretical free speed (m/s) at 12 V applied output;
     // This needs to be tuned to your individual robot
-    public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(4.69);
+    public static final LinearVelocity kSpeedAt12Volts = MetersPerSecond.of(6.14);
 
     // Every 1 rotation of the azimuth results in kCoupleRatio drive motor turns;
     // This may need to be tuned to your individual robot
-    private static final double kCoupleRatio = 3.8181818181818183;
+    private static final double kCoupleRatio = 3.375;
 
-    private static final double kDriveGearRatio = 7.363636363636365;
-    private static final double kSteerGearRatio = 15.42857142857143;
-    private static final Distance kWheelRadius = Inches.of(2.167);
+    private static final double kDriveGearRatio = 675.0 / 128.0;
+    private static final double kSteerGearRatio = 26.09;
+    private static final Distance kWheelRadius = Inches.of(1.841);
 
     private static final boolean kInvertLeftSide = false;
     private static final boolean kInvertRightSide = true;
 
-    private static final int kPigeonId = 1;
+    private static final int kPigeonId = 13;
 
     // These are only used for simulation
-    private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.004);
-    private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.025);
+    private static final MomentOfInertia kSteerInertia = KilogramSquareMeters.of(0.01);
+    private static final MomentOfInertia kDriveInertia = KilogramSquareMeters.of(0.01);
     // Simulated voltage necessary to overcome friction
     private static final Voltage kSteerFrictionVoltage = Volts.of(0.2);
     private static final Voltage kDriveFrictionVoltage = Volts.of(0.2);
@@ -125,48 +124,48 @@ public class TunerConstants {
                     .withDriveFrictionVoltage(kDriveFrictionVoltage);
 
     // Front Left
-    private static final int kFrontLeftDriveMotorId = 3;
-    private static final int kFrontLeftSteerMotorId = 2;
-    private static final int kFrontLeftEncoderId = 1;
-    private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.15234375);
-    private static final boolean kFrontLeftSteerMotorInverted = true;
+    private static final int kFrontLeftDriveMotorId = 8;
+    private static final int kFrontLeftSteerMotorId = 4;
+    private static final int kFrontLeftEncoderId = 12;
+    private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.198486328125);
+    private static final boolean kFrontLeftSteerMotorInverted = false;
     private static final boolean kFrontLeftEncoderInverted = false;
 
-    private static final Distance kFrontLeftXPos = Inches.of(10);
-    private static final Distance kFrontLeftYPos = Inches.of(10);
+    private static final Distance kFrontLeftXPos = Inches.of(10.375);
+    private static final Distance kFrontLeftYPos = Inches.of(10.375);
 
     // Front Right
-    private static final int kFrontRightDriveMotorId = 1;
-    private static final int kFrontRightSteerMotorId = 0;
-    private static final int kFrontRightEncoderId = 0;
-    private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.4873046875);
-    private static final boolean kFrontRightSteerMotorInverted = true;
+    private static final int kFrontRightDriveMotorId = 5;
+    private static final int kFrontRightSteerMotorId = 2;
+    private static final int kFrontRightEncoderId = 11;
+    private static final Angle kFrontRightEncoderOffset = Rotations.of(0.04296875);
+    private static final boolean kFrontRightSteerMotorInverted = false;
     private static final boolean kFrontRightEncoderInverted = false;
 
-    private static final Distance kFrontRightXPos = Inches.of(10);
-    private static final Distance kFrontRightYPos = Inches.of(-10);
+    private static final Distance kFrontRightXPos = Inches.of(10.375);
+    private static final Distance kFrontRightYPos = Inches.of(-10.375);
 
     // Back Left
     private static final int kBackLeftDriveMotorId = 7;
-    private static final int kBackLeftSteerMotorId = 6;
-    private static final int kBackLeftEncoderId = 3;
-    private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.219482421875);
-    private static final boolean kBackLeftSteerMotorInverted = true;
+    private static final int kBackLeftSteerMotorId = 1;
+    private static final int kBackLeftEncoderId = 10;
+    private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.488525390625);
+    private static final boolean kBackLeftSteerMotorInverted = false;
     private static final boolean kBackLeftEncoderInverted = false;
 
-    private static final Distance kBackLeftXPos = Inches.of(-10);
-    private static final Distance kBackLeftYPos = Inches.of(10);
+    private static final Distance kBackLeftXPos = Inches.of(-10.375);
+    private static final Distance kBackLeftYPos = Inches.of(10.375);
 
     // Back Right
-    private static final int kBackRightDriveMotorId = 5;
-    private static final int kBackRightSteerMotorId = 4;
-    private static final int kBackRightEncoderId = 2;
-    private static final Angle kBackRightEncoderOffset = Rotations.of(0.17236328125);
-    private static final boolean kBackRightSteerMotorInverted = true;
+    private static final int kBackRightDriveMotorId = 6;
+    private static final int kBackRightSteerMotorId = 3;
+    private static final int kBackRightEncoderId = 13;
+    private static final Angle kBackRightEncoderOffset = Rotations.of(-0.10400390625);
+    private static final boolean kBackRightSteerMotorInverted = false;
     private static final boolean kBackRightEncoderInverted = false;
 
-    private static final Distance kBackRightXPos = Inches.of(-10);
-    private static final Distance kBackRightYPos = Inches.of(-10);
+    private static final Distance kBackRightXPos = Inches.of(-10.375);
+    private static final Distance kBackRightYPos = Inches.of(-10.375);
 
     public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>
             FrontLeft = ConstantCreator.createModuleConstants(
@@ -213,12 +212,6 @@ public class TunerConstants {
                     kBackRightSteerMotorInverted,
                     kBackRightEncoderInverted);
 
-    /** Creates a CommandSwerveDrivetrain instance. This should only be called once in your robot program,. */
-    //   public static CommandSwerveDrivetrain createDrivetrain() {
-    //     return new CommandSwerveDrivetrain(
-    //         DrivetrainConstants, FrontLeft, FrontRight, BackLeft, BackRight);
-    //   }
-
     /** Swerve Drive class utilizing CTR Electronics' Phoenix 6 API with the selected device types. */
     public static class TunerSwerveDrivetrain extends SwerveDrivetrain<TalonFX, TalonFX, CANcoder> {
         /**
diff --git a/src/main/java/frc/robot/subsystems/drive/Drive.java b/src/main/java/frc/robot/subsystems/drive/Drive.java
index 02bd1d6..eb3ff70 100644
--- a/src/main/java/frc/robot/subsystems/drive/Drive.java
+++ b/src/main/java/frc/robot/subsystems/drive/Drive.java
@@ -22,7 +22,10 @@
 import com.pathplanner.lib.config.RobotConfig;
 import com.pathplanner.lib.controllers.PPHolonomicDriveController;
 import com.pathplanner.lib.pathfinding.Pathfinding;
+import com.pathplanner.lib.util.DriveFeedforwards;
 import com.pathplanner.lib.util.PathPlannerLogging;
+import com.pathplanner.lib.util.swerve.SwerveSetpoint;
+import com.pathplanner.lib.util.swerve.SwerveSetpointGenerator;
 import edu.wpi.first.hal.FRCNetComm.tInstances;
 import edu.wpi.first.hal.FRCNetComm.tResourceType;
 import edu.wpi.first.hal.HAL;
@@ -76,6 +79,7 @@ public class Drive extends SubsystemBase implements Vision.VisionConsumer {
     private static final double ROBOT_MASS_KG = 74.088;
     private static final double ROBOT_MOI = 6.883;
     private static final double WHEEL_COF = 1.2;
+    private static final double MAX_STEER_VELOCITY_RAD_PER_SEC = 13.0; // ~2 rotations per second
     private static final RobotConfig PP_CONFIG = new RobotConfig(
             ROBOT_MASS_KG,
             ROBOT_MOI,
@@ -125,6 +129,10 @@ public class Drive extends SubsystemBase implements Vision.VisionConsumer {
 
     private final Consumer<Pose2d> resetSimulationPoseCallBack;
 
+    // Swerve setpoint generator (Team 254 style anti-slip)
+    private final SwerveSetpointGenerator setpointGenerator;
+    private SwerveSetpoint previousSetpoint;
+
     public Drive(
             GyroIO gyroIO,
             ModuleIO flModuleIO,
@@ -168,6 +176,10 @@ public Drive(
                 new SysIdRoutine.Config(
                         null, null, null, (state) -> Logger.recordOutput("Drive/SysIdState", state.toString())),
                 new SysIdRoutine.Mechanism((voltage) -> runCharacterization(voltage.in(Volts)), null, this));
+
+        // Configure swerve setpoint generator (Team 254 style)
+        setpointGenerator = new SwerveSetpointGenerator(PP_CONFIG, MAX_STEER_VELOCITY_RAD_PER_SEC);
+        previousSetpoint = new SwerveSetpoint(getChassisSpeeds(), getModuleStates(), DriveFeedforwards.zeros(4));
     }
 
     @Override
@@ -232,10 +244,10 @@ public void periodic() {
      * @param speeds Speeds in meters/sec
      */
     public void runVelocity(ChassisSpeeds speeds) {
-        // Calculate module setpoints
-        speeds = ChassisSpeeds.discretize(speeds, 0.02);
-        SwerveModuleState[] setpointStates = kinematics.toSwerveModuleStates(speeds);
-        SwerveDriveKinematics.desaturateWheelSpeeds(setpointStates, TunerConstants.kSpeedAt12Volts);
+        // Use Team 254's setpoint generator for carefully controlled module states
+        // Note: Do NOT discretize speeds - the setpoint generator handles this internally
+        previousSetpoint = setpointGenerator.generateSetpoint(previousSetpoint, speeds, 0.02);
+        SwerveModuleState[] setpointStates = previousSetpoint.moduleStates();
 
         // Log unoptimized setpoints and setpoint speeds
         Logger.recordOutput("SwerveStates/Setpoints", setpointStates);
diff --git a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java
index f10767f..0734f23 100644
--- a/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java
+++ b/src/main/java/frc/robot/subsystems/drive/GyroIOPigeon2.java
@@ -41,7 +41,7 @@ public GyroIOPigeon2() {
         yawVelocity.setUpdateFrequency(50.0);
         pigeon.optimizeBusUtilization();
         yawTimestampQueue = PhoenixOdometryThread.getInstance().makeTimestampQueue();
-        yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(pigeon.getYaw());
+        yawPositionQueue = PhoenixOdometryThread.getInstance().registerSignal(yaw.clone());
     }
 
     @Override
diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java
index cfa3ff8..fda2c65 100644
--- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java
+++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFX.java
@@ -70,6 +70,7 @@ protected ModuleIOTalonFX(
         var driveConfig = constants.DriveMotorInitialConfigs;
         driveConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
         driveConfig.Slot0 = constants.DriveMotorGains;
+        driveConfig.Feedback.SensorToMechanismRatio = constants.DriveMotorGearRatio;
         driveConfig.TorqueCurrent.PeakForwardTorqueCurrent = constants.SlipCurrent;
         driveConfig.TorqueCurrent.PeakReverseTorqueCurrent = -constants.SlipCurrent;
         driveConfig.CurrentLimits.StatorCurrentLimit = constants.SlipCurrent;
@@ -142,10 +143,8 @@ public void updateInputs(ModuleIOInputs inputs) {
 
         // Update drive inputs
         inputs.driveConnected = driveConnectedDebounce.calculate(driveStatus.isOK());
-        inputs.drivePositionRad =
-                Units.rotationsToRadians(drivePosition.getValueAsDouble()) / constants.DriveMotorGearRatio;
-        inputs.driveVelocityRadPerSec =
-                Units.rotationsToRadians(driveVelocity.getValueAsDouble()) / constants.DriveMotorGearRatio;
+        inputs.drivePositionRad = Units.rotationsToRadians(drivePosition.getValueAsDouble());
+        inputs.driveVelocityRadPerSec = Units.rotationsToRadians(driveVelocity.getValueAsDouble());
         inputs.driveAppliedVolts = driveAppliedVolts.getValueAsDouble();
         inputs.driveCurrentAmps = driveCurrent.getValueAsDouble();
 
@@ -177,13 +176,19 @@ public void setTurnOpenLoop(double output) {
     }
 
     @Override
-    public void setDriveVelocity(double wheelVelocityRadPerSec) {
-        double motorVelocityRotPerSec =
-                Units.radiansToRotations(wheelVelocityRadPerSec) * constants.DriveMotorGearRatio;
+    public void setDriveVelocity(double velocityRadPerSec) {
+        double velocityRotPerSec = Units.radiansToRotations(velocityRadPerSec);
+
+        // Apply coupling compensation (254 style): add back the drive velocity caused by azimuth rotation
+        // so the wheel achieves the actual requested velocity
+        double azimuthVelocityRps = turnVelocity.getValueAsDouble() / constants.SteerMotorGearRatio;
+        double driveBackoutRps = azimuthVelocityRps * constants.CouplingGearRatio / constants.DriveMotorGearRatio;
+        velocityRotPerSec += driveBackoutRps;
+
         driveTalon.setControl(
                 switch (constants.DriveMotorClosedLoopOutput) {
-                    case Voltage -> velocityVoltageRequest.withVelocity(motorVelocityRotPerSec);
-                    case TorqueCurrentFOC -> velocityTorqueCurrentRequest.withVelocity(motorVelocityRotPerSec);
+                    case Voltage -> velocityVoltageRequest.withVelocity(velocityRotPerSec);
+                    case TorqueCurrentFOC -> velocityTorqueCurrentRequest.withVelocity(velocityRotPerSec);
                 });
     }
 
diff --git a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXReal.java b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXReal.java
index 7e27a56..0da142a 100644
--- a/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXReal.java
+++ b/src/main/java/frc/robot/subsystems/drive/ModuleIOTalonFXReal.java
@@ -42,14 +42,34 @@ public ModuleIOTalonFXReal(SwerveModuleConstants constants) {
     public void updateInputs(ModuleIOInputs inputs) {
         super.updateInputs(inputs);
 
+        // Apply coupling compensation to main drive position
+        // When azimuth rotates, the drive encoder sees phantom movement
+        double couplingRatio = constants.CouplingGearRatio;
+        double driveGearRatio = constants.DriveMotorGearRatio;
+        double turnRotations = inputs.turnAbsolutePosition.getRotations();
+        double couplingCorrectionRad = Units.rotationsToRadians(turnRotations * couplingRatio / driveGearRatio);
+        inputs.drivePositionRad -= couplingCorrectionRad;
+
         // Update odometry inputs
         inputs.odometryTimestamps =
                 timestampQueue.stream().mapToDouble((Double value) -> value).toArray();
-        inputs.odometryDrivePositionsRad = drivePositionQueue.stream()
-                .mapToDouble(Units::rotationsToRadians)
-                .toArray();
-        inputs.odometryTurnPositions =
-                turnPositionQueue.stream().map(Rotation2d::fromRotations).toArray(Rotation2d[]::new);
+
+        // Get raw positions from queues
+        double[] rawDrivePositions = drivePositionQueue.stream().mapToDouble(Double::doubleValue).toArray();
+        double[] rawTurnPositions = turnPositionQueue.stream().mapToDouble(Double::doubleValue).toArray();
+
+        // Apply coupling compensation to odometry samples
+        inputs.odometryDrivePositionsRad = new double[rawDrivePositions.length];
+        for (int i = 0; i < rawDrivePositions.length; i++) {
+            double couplingCorrection = rawTurnPositions[i] * couplingRatio / driveGearRatio;
+            inputs.odometryDrivePositionsRad[i] = Units.rotationsToRadians(rawDrivePositions[i] - couplingCorrection);
+        }
+
+        inputs.odometryTurnPositions = new Rotation2d[rawTurnPositions.length];
+        for (int i = 0; i < rawTurnPositions.length; i++) {
+            inputs.odometryTurnPositions[i] = Rotation2d.fromRotations(rawTurnPositions[i]);
+        }
+
         timestampQueue.clear();
         drivePositionQueue.clear();
         turnPositionQueue.clear();
diff --git a/src/main/java/frc/robot/subsystems/intake/Intake.java b/src/main/java/frc/robot/subsystems/intake/Intake.java
new file mode 100644
index 0000000..968e234
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/Intake.java
@@ -0,0 +1,165 @@
+package frc.robot.subsystems.intake;
+
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import org.littletonrobotics.junction.Logger;
+
+public class Intake extends SubsystemBase {
+    // Arm position constants (radians)
+    public static final double ARM_STOWED_POSITION = 0.0;
+    public static final double ARM_DEPLOYED_POSITION = -8; // -1.62
+
+    private final IntakeIO io;
+    private final IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged();
+
+    // Mechanism2d for visualization
+    private final Mechanism2d mechanism;
+    private final MechanismLigament2d armLigament;
+    private final MechanismLigament2d rollerLigament;
+
+    public Intake(IntakeIO io) {
+        this.io = io;
+
+        // Create Mechanism2d (width, height in meters)
+        mechanism = new Mechanism2d(3, 3);
+
+        // Create root at robot center (x, y in meters)
+        MechanismRoot2d root = mechanism.getRoot("IntakeRoot", 1.5, 0.5);
+
+        // Create arm ligament (name, length in meters, angle in degrees, line width, color)
+        armLigament = root.append(new MechanismLigament2d(
+                "Arm",
+                0.5, // 0.5 meter arm length
+                Math.toDegrees(ARM_STOWED_POSITION),
+                6,
+                new Color8Bit(Color.kOrange)));
+
+        // Create roller at end of arm
+        rollerLigament = armLigament.append(new MechanismLigament2d(
+                "Roller",
+                0.15, // 0.15 meter roller visualization
+                90, // Perpendicular to arm
+                4,
+                new Color8Bit(Color.kGreen)));
+
+        SmartDashboard.putData("Intake/Mechanism", mechanism);
+    }
+
+    @Override
+    public void periodic() {
+        io.updateInputs(inputs);
+        Logger.processInputs("Intake", inputs);
+
+        // Update mechanism visualization
+        armLigament.setAngle(Math.toDegrees(inputs.armPosition));
+        rollerLigament.setColor(new Color8Bit(Color.kGreen));
+    }
+
+    // Set roller voltage (-12 to +12 volts)
+    public void setRollerDutyCycle(double value) {
+        io.setRollerDutyCycle(value);
+    }
+
+    // Set arm voltage (-12 to +12 volts) for manual control
+    public void setArmVoltage(double voltage) {
+        io.setArmVoltage(voltage);
+    }
+
+    // Set arm to a specific position in radians
+    public void setArmPosition(double positionRad) {
+        io.setArmPosition(positionRad);
+    }
+
+    // Deploy the intake arm to deployed position
+    public void deployArm() {
+        io.setArmPosition(ARM_DEPLOYED_POSITION);
+    }
+
+    // Stow the intake arm to stowed position
+    public void stowArm() {
+        io.setArmPosition(ARM_STOWED_POSITION);
+    }
+
+    // Reset arm encoder position to zero
+    public void resetArmPosition() {
+        io.resetArmPosition();
+    }
+
+    // GETTERS
+
+    // Roller telemetry
+    public double getRollerPositionRad() {
+        return inputs.rollerPosition;
+    }
+
+    public double getRollerVelocityRadPerSec() {
+        return inputs.rollerVelocity;
+    }
+
+    public double getRollerCurrent() {
+        return inputs.rollerCurrent;
+    }
+
+    public double getRollerVoltage() {
+        return inputs.rollerVoltage;
+    }
+
+    // Arm telemetry
+    public double getArmPositionRad() {
+        return inputs.armPosition;
+    }
+
+    public double getArmVelocityRadPerSec() {
+        return inputs.armVelocity;
+    }
+
+    public double getArmCurrent() {
+        return inputs.armCurrent;
+    }
+
+    public double getArmVoltage() {
+        return inputs.armVoltage;
+    }
+
+    // Indexer telemetry
+    public double getIndexerPositionRad() {
+        return inputs.indexerPosition;
+    }
+
+    public double getIndexerVelocityRadPerSec() {
+        return inputs.indexerVelocity;
+    }
+
+    public double getIndexerCurrent() {
+        return inputs.indexerCurrent;
+    }
+
+    public double getIndexerVoltage() {
+        return inputs.indexerVoltage;
+    }
+
+    // Check if arm is at target position (within tolerance)
+    public boolean isArmAtPosition(double targetRad, double toleranceRad) {
+        return Math.abs(inputs.armPosition - targetRad) < toleranceRad;
+    }
+
+    // Get the Mechanism2d for dashboard display
+    public Mechanism2d getMechanism() {
+        return mechanism;
+    }
+
+    // Stop all motors
+    public void stop() {
+        io.stopRoller();
+    }
+
+    // Stop arm
+    public void stopArm() {
+        io.stopArmMotor();
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIO.java b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java
new file mode 100644
index 0000000..1c44e74
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java
@@ -0,0 +1,40 @@
+package frc.robot.subsystems.intake;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface IntakeIO {
+    @AutoLog
+    class IntakeIOInputs {
+        // Roller Motor
+        public double rollerCurrent = 0.0;
+        public double rollerVoltage = 0.0;
+        public double rollerPosition = 0.0;
+        public double rollerVelocity = 0.0;
+
+        // Arm Motor
+        public double armCurrent = 0.0;
+        public double armVoltage = 0.0;
+        public double armPosition = 0.0;
+        public double armVelocity = 0.0;
+
+        // Indexer Motor
+        public double indexerCurrent = 0.0;
+        public double indexerVoltage = 0.0;
+        public double indexerPosition = 0.0;
+        public double indexerVelocity = 0.0;
+    }
+
+    default void updateInputs(IntakeIOInputs inputs) {}
+
+    default void setRollerDutyCycle(double value) {}
+
+    default void setArmVoltage(double voltage) {}
+
+    default void setArmPosition(double positionRad) {}
+
+    default void resetArmPosition() {}
+
+    default void stopRoller() {}
+
+    default void stopArmMotor() {}
+}
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java
new file mode 100644
index 0000000..0d4b6f3
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXReal.java
@@ -0,0 +1,197 @@
+package frc.robot.subsystems.intake;
+
+import static frc.robot.util.PhoenixUtil.tryUntilOk;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.controls.DutyCycleOut;
+import com.ctre.phoenix6.controls.MotionMagicExpoTorqueCurrentFOC;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.units.measure.*;
+
+public class IntakeIOTalonFXReal implements IntakeIO {
+    // Hardware configuration constants
+    private static final int ROLLER_MOTOR_ID = 20;
+    private static final int ARM_MOTOR_ID = 19;
+    private static final int INDEXER_MOTOR_ID = 17;
+    private static final double ROLLER_GEAR_RATIO = 3.0;
+    private static final double ARM_GEAR_RATIO = 1; // 248/9
+    private static final double INDEXER_GEAR_RATIO = 3.0;
+    // Hardware
+    private final TalonFX rollerMotor;
+    private final TalonFX armMotor;
+    private final TalonFX indexerMotor;
+
+    // Control requests
+    private final DutyCycleOut voltageRequest = new DutyCycleOut(0.0);
+    private final MotionMagicExpoTorqueCurrentFOC positionRequest = new MotionMagicExpoTorqueCurrentFOC(0.0);
+
+    // Status signals - Roller
+    private final StatusSignal<Angle> rollerPosition;
+    private final StatusSignal<AngularVelocity> rollerVelocity;
+    private final StatusSignal<Voltage> rollerAppliedVolts;
+    private final StatusSignal<Current> rollerCurrent;
+
+    // Status signals - Arm
+    private final StatusSignal<Angle> armPosition;
+    private final StatusSignal<AngularVelocity> armVelocity;
+    private final StatusSignal<Voltage> armAppliedVolts;
+    private final StatusSignal<Current> armCurrent;
+
+    // Status Signals - Indexer
+    private final StatusSignal<Angle> indexerPosition;
+    private final StatusSignal<AngularVelocity> indexerVelocity;
+    private final StatusSignal<Voltage> indexerAppliedVolts;
+    private final StatusSignal<Current> indexerCurrent;
+
+    public IntakeIOTalonFXReal() {
+        // Initialize hardware
+        rollerMotor = new TalonFX(ROLLER_MOTOR_ID, "swerve"); // CHANGE TO SWERVE LATER
+        armMotor = new TalonFX(ARM_MOTOR_ID, "swerve"); // CHANGE TO SWERVE LATER
+        indexerMotor = new TalonFX(INDEXER_MOTOR_ID, "swerve"); // CHANGE LATER TO SWERVE
+
+        // Configure roller motor
+        var rollerConfig = new TalonFXConfiguration();
+        rollerConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast;
+        rollerConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+
+        // Current limits for roller
+        rollerConfig.CurrentLimits.StatorCurrentLimit = 60.0;
+        rollerConfig.CurrentLimits.StatorCurrentLimitEnable = true;
+        rollerConfig.CurrentLimits.SupplyCurrentLimit = 50.0;
+        rollerConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
+
+        tryUntilOk(5, () -> rollerMotor.getConfigurator().apply(rollerConfig, 0.25));
+
+        // Configure indexer motor
+        var indexerConfig = new TalonFXConfiguration();
+        indexerConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast;
+        indexerConfig.MotorOutput.Inverted = InvertedValue.Clockwise_Positive;
+
+        // Current limits for roller
+        indexerConfig.CurrentLimits.StatorCurrentLimit = 60.0;
+        indexerConfig.CurrentLimits.StatorCurrentLimitEnable = true;
+        indexerConfig.CurrentLimits.SupplyCurrentLimit = 50.0;
+        indexerConfig.CurrentLimits.SupplyCurrentLimitEnable = true;
+
+        tryUntilOk(5, () -> indexerMotor.getConfigurator().apply(indexerConfig, 0.25));
+
+        // Configure arm motor
+        var armConfig = new TalonFXConfiguration();
+        armConfig.MotorOutput.NeutralMode = NeutralModeValue.Brake;
+        armConfig.MotorOutput.Inverted = InvertedValue.CounterClockwise_Positive;
+
+        // Current limits for arm
+        armConfig.CurrentLimits.StatorCurrentLimit = 30.0;
+        armConfig.CurrentLimits.StatorCurrentLimitEnable = true;
+
+        // PID for arm position control
+        armConfig.Slot0.kP = 100.0;
+        armConfig.Slot0.kI = 0.0;
+        armConfig.Slot0.kD = 0.5;
+        armConfig.Slot0.kS = 0.0;
+        armConfig.Slot0.kV = 0.0;
+
+        tryUntilOk(5, () -> armMotor.getConfigurator().apply(armConfig, 0.25));
+
+        resetArmPosition();
+
+        // Create status signals - Roller
+        rollerPosition = rollerMotor.getPosition();
+        rollerVelocity = rollerMotor.getVelocity();
+        rollerAppliedVolts = rollerMotor.getMotorVoltage();
+        rollerCurrent = rollerMotor.getStatorCurrent();
+
+        // Create status signals - Indexer
+        indexerPosition = indexerMotor.getPosition();
+        indexerVelocity = indexerMotor.getVelocity();
+        indexerAppliedVolts = indexerMotor.getMotorVoltage();
+        indexerCurrent = indexerMotor.getStatorCurrent();
+
+        // Create status signals - Arm
+        armPosition = armMotor.getPosition();
+        armVelocity = armMotor.getVelocity();
+        armAppliedVolts = armMotor.getMotorVoltage();
+        armCurrent = armMotor.getStatorCurrent();
+
+        // Configure update frequencies
+        BaseStatusSignal.setUpdateFrequencyForAll(
+                50.0,
+                rollerPosition,
+                rollerVelocity,
+                rollerAppliedVolts,
+                rollerCurrent,
+                armPosition,
+                armVelocity,
+                armAppliedVolts,
+                armCurrent,
+                indexerVelocity,
+                indexerAppliedVolts,
+                indexerCurrent,
+                indexerPosition);
+        rollerMotor.optimizeBusUtilization();
+        armMotor.optimizeBusUtilization();
+        indexerMotor.optimizeBusUtilization();
+    }
+
+    @Override
+    public void updateInputs(IntakeIOInputs inputs) {
+        BaseStatusSignal.refreshAll(rollerPosition, rollerVelocity, rollerAppliedVolts, rollerCurrent);
+        BaseStatusSignal.refreshAll(armPosition, armVelocity, armAppliedVolts, armCurrent);
+        BaseStatusSignal.refreshAll(indexerVelocity, indexerAppliedVolts, indexerCurrent);
+
+        // Roller inputs
+        inputs.rollerPosition = Units.rotationsToRadians(rollerPosition.getValueAsDouble()) / ROLLER_GEAR_RATIO;
+        inputs.rollerVelocity = Units.rotationsToRadians(rollerVelocity.getValueAsDouble()) / ROLLER_GEAR_RATIO;
+        inputs.rollerVoltage = rollerAppliedVolts.getValueAsDouble();
+        inputs.rollerCurrent = rollerCurrent.getValueAsDouble();
+
+        // Arm inputs
+        inputs.armPosition = Units.rotationsToRadians(armPosition.getValueAsDouble()) / ARM_GEAR_RATIO;
+        inputs.armVelocity = Units.rotationsToRadians(armVelocity.getValueAsDouble()) / ARM_GEAR_RATIO;
+        inputs.armVoltage = armAppliedVolts.getValueAsDouble();
+        inputs.armCurrent = armCurrent.getValueAsDouble();
+
+        // Indexer inputs
+        inputs.indexerPosition = Units.rotationsToRadians(indexerPosition.getValueAsDouble()) / INDEXER_GEAR_RATIO;
+        inputs.indexerVelocity = Units.rotationsToRadians(indexerVelocity.getValueAsDouble()) / INDEXER_GEAR_RATIO;
+        inputs.indexerVoltage = indexerAppliedVolts.getValueAsDouble();
+        inputs.indexerCurrent = indexerCurrent.getValueAsDouble();
+    }
+
+    @Override
+    public void setRollerDutyCycle(double value) {
+        rollerMotor.setControl(voltageRequest.withOutput(value));
+        indexerMotor.setControl(voltageRequest.withOutput(value));
+    }
+
+    @Override
+    public void setArmVoltage(double voltage) {
+        armMotor.setControl(voltageRequest.withOutput(voltage));
+    }
+
+    @Override
+    public void setArmPosition(double positionRad) {
+        double motorRotations = Units.radiansToRotations(positionRad) * ARM_GEAR_RATIO;
+        armMotor.setControl(positionRequest.withPosition(motorRotations));
+    }
+
+    @Override
+    public void resetArmPosition() {
+        tryUntilOk(5, () -> armMotor.setPosition(0.0, 0.25));
+    }
+
+    @Override
+    public void stopRoller() {
+        rollerMotor.stopMotor();
+        indexerMotor.stopMotor();
+    }
+
+    public void stopArmMotor() {
+        armMotor.stopMotor();
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java
new file mode 100644
index 0000000..06f68cf
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOTalonFXSim.java
@@ -0,0 +1,132 @@
+package frc.robot.subsystems.intake;
+
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+import edu.wpi.first.wpilibj.simulation.DCMotorSim;
+
+public class IntakeIOTalonFXSim implements IntakeIO {
+    // Hardware configuration constants
+    private static final double ROLLER_GEAR_RATIO = 3.0;
+    private static final double ARM_GEAR_RATIO = 100.0;
+    private static final double INDEXER_GEAR_RATIO = 3.0;
+
+    // Simulation parameters
+    private static final double ROLLER_MOI = 0.001; // kg*m^2
+    private static final double INDEXER_MOI = 0.001; // kg*m^2
+    private static final double ARM_MOI = 0.1; // kg*m^2
+
+    // Arm position constants (radians)
+    private static final double ARM_MIN_ANGLE = 0.0;
+    private static final double ARM_MAX_ANGLE = Math.PI / 2;
+
+    // Motor simulations
+    private final DCMotorSim rollerSim;
+    private final DCMotorSim armSim;
+    private final DCMotorSim indexerSim;
+
+    // Applied voltages
+    private double rollerAppliedVolts = 0.0;
+    private double indexerAppliedVolts = 0.0;
+    private double armAppliedVolts = 0.0;
+
+    // Simulated game piece detection
+    private boolean simulatedGamePiece = false;
+
+    public IntakeIOTalonFXSim() {
+        // Kraken X60 for roller (1 motor, geared)
+        // Using LinearSystemId to create the plant
+        rollerSim = new DCMotorSim(
+                LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), ROLLER_MOI, ROLLER_GEAR_RATIO),
+                DCMotor.getKrakenX60(1));
+
+        // Kraken X60 for indexer (1 motor, geared)
+        indexerSim = new DCMotorSim(
+                LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), INDEXER_MOI, INDEXER_GEAR_RATIO),
+                DCMotor.getKrakenX60(1));
+
+        // Kraken X60 for arm (1 motor, highly geared)
+        armSim = new DCMotorSim(
+                LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), ARM_MOI, ARM_GEAR_RATIO),
+                DCMotor.getKrakenX60(1));
+    }
+
+    @Override
+    public void updateInputs(IntakeIOInputs inputs) {
+        // Update simulations
+        rollerSim.update(0.02);
+        armSim.update(0.02);
+        indexerSim.update(0.02);
+
+        // Clamp arm position to realistic bounds
+        if (armSim.getAngularPositionRad() < ARM_MIN_ANGLE) {
+            armSim.setState(ARM_MIN_ANGLE, 0.0);
+        } else if (armSim.getAngularPositionRad() > ARM_MAX_ANGLE) {
+            armSim.setState(ARM_MAX_ANGLE, 0.0);
+        }
+
+        // Update roller inputs
+        inputs.rollerPosition = rollerSim.getAngularPositionRad();
+        inputs.rollerVelocity = rollerSim.getAngularVelocityRadPerSec();
+        inputs.rollerVoltage = rollerAppliedVolts;
+        inputs.rollerCurrent = rollerSim.getCurrentDrawAmps();
+
+        // Update arm inputs
+        inputs.armPosition = armSim.getAngularPositionRad();
+        inputs.armVelocity = armSim.getAngularVelocityRadPerSec();
+        inputs.armVoltage = armAppliedVolts;
+        inputs.armCurrent = armSim.getCurrentDrawAmps();
+
+        // Update indexer inputs
+        inputs.indexerPosition = indexerSim.getAngularPositionRad();
+        inputs.indexerVelocity = indexerSim.getAngularVelocityRadPerSec();
+        inputs.indexerVoltage = indexerAppliedVolts;
+        inputs.indexerCurrent = indexerSim.getCurrentDrawAmps();
+
+        if (!simulatedGamePiece
+                && rollerAppliedVolts > 6.0
+                && Math.abs(rollerSim.getAngularVelocityRadPerSec()) > 10.0) {
+            simulatedGamePiece = true;
+        }
+
+        if (simulatedGamePiece && rollerAppliedVolts < -6.0) {
+            simulatedGamePiece = false;
+        }
+    }
+
+    @Override
+    public void setRollerDutyCycle(double voltage) {
+        rollerAppliedVolts = voltage;
+        rollerSim.setInputVoltage(voltage);
+        indexerAppliedVolts = voltage;
+        indexerSim.setInputVoltage(voltage);
+    }
+
+    @Override
+    public void setArmVoltage(double voltage) {
+        armAppliedVolts = voltage;
+        armSim.setInputVoltage(voltage);
+    }
+
+    @Override
+    public void setArmPosition(double positionRad) {
+        // Simple P controller for position control in sim
+        double kP = 10.0;
+        double error = positionRad - armSim.getAngularPositionRad();
+        double voltage = error * kP;
+
+        // Clamp voltage
+        voltage = Math.max(-12.0, Math.min(12.0, voltage));
+
+        setArmVoltage(voltage);
+    }
+
+    @Override
+    public void resetArmPosition() {
+        armSim.setState(ARM_MIN_ANGLE, 0.0);
+    }
+
+    @Override
+    public void stopRoller() {
+        setRollerDutyCycle(0.0);
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFXSim.java b/src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFXSim.java
new file mode 100644
index 0000000..5d0113a
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFXSim.java
@@ -0,0 +1,3 @@
+package frc.robot.subsystems.shooter;
+
+public class ModuleIOTalonFXSim {}
diff --git a/src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFx.java b/src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFx.java
new file mode 100644
index 0000000..9262655
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/shooter/ModuleIOTalonFx.java
@@ -0,0 +1,3 @@
+package frc.robot.subsystems.shooter;
+
+public class ModuleIOTalonFx {}
diff --git a/src/main/java/frc/robot/subsystems/shooter/Shooter.java b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
new file mode 100644
index 0000000..f718411
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/shooter/Shooter.java
@@ -0,0 +1,5 @@
+package frc.robot.subsystems.shooter;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public class Shooter extends SubsystemBase {}
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java
new file mode 100644
index 0000000..8b13789
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIO.java
@@ -0,0 +1 @@
+
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFXReal.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFXReal.java
new file mode 100644
index 0000000..4596445
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFXReal.java
@@ -0,0 +1,3 @@
+package frc.robot.subsystems.shooter;
+
+public class ShooterIOTalonFXReal {}
diff --git a/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFXSim.java b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFXSim.java
new file mode 100644
index 0000000..ebc978b
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/shooter/ShooterIOTalonFXSim.java
@@ -0,0 +1,3 @@
+package frc.robot.subsystems.shooter;
+
+public class ShooterIOTalonFXSim {}
diff --git a/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java b/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java
new file mode 100644
index 0000000..57a3f8d
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/superstructure/Superstructure.java
@@ -0,0 +1,91 @@
+package frc.robot.subsystems.superstructure;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.subsystems.intake.Intake;
+import org.littletonrobotics.junction.Logger;
+
+public class Superstructure extends SubsystemBase {
+
+    public enum Goal {
+        IDLE,
+        INTAKING,
+        OUTTAKING;
+    }
+
+    private final SuperstructureIOInputsAutoLogged inputs = new SuperstructureIOInputsAutoLogged();
+
+    private final Intake intake;
+    private final SuperstructureIO io;
+
+    private Goal currentGoal = Goal.IDLE;
+    private Goal desiredGoal = Goal.IDLE;
+
+    public Superstructure(Intake intake, SuperstructureIO io) {
+        this.intake = intake;
+        this.io = io;
+    }
+
+    @Override
+    public void periodic() {
+        // Process Inputs
+        io.updateInputs(inputs);
+        Logger.processInputs("Superstructure", inputs);
+
+        updateGoalInfo();
+
+        // Log current states
+        Logger.recordOutput("Superstructure/CurrentGoal", currentGoal.toString());
+        Logger.recordOutput("Superstructure/DesiredGoal", desiredGoal.toString());
+        Logger.recordOutput("Superstructure/AtGoal", atGoal());
+
+        currentGoal = desiredGoal;
+
+        switch (currentGoal) {
+            case IDLE:
+                intake.stowArm();
+                intake.setRollerDutyCycle(0.0);
+                break;
+            case INTAKING:
+                intake.deployArm();
+                intake.setRollerDutyCycle(0.5);
+                break;
+            case OUTTAKING:
+                intake.deployArm();
+                intake.setRollerDutyCycle(-0.5);
+                break;
+        }
+    }
+
+    private void updateGoalInfo() {
+        inputs.currentGoal = currentGoal;
+        inputs.desiredGoal = desiredGoal;
+        inputs.atGoal = atGoal();
+    }
+
+    public Intake getIntake() {
+        return this.intake;
+    }
+
+    public Goal getCurrentGoal() {
+        return currentGoal;
+    }
+
+    public Goal getDesiredGoal() {
+        return desiredGoal;
+    }
+
+    public void setGoal(Goal goal) {
+        desiredGoal = goal;
+    }
+
+    public boolean atGoal() {
+        if (currentGoal != desiredGoal) {
+            return false;
+        }
+
+        return switch (currentGoal) {
+            case IDLE -> intake.isArmAtPosition(Intake.ARM_STOWED_POSITION, 0.1);
+            case INTAKING, OUTTAKING -> intake.isArmAtPosition(Intake.ARM_DEPLOYED_POSITION, 0.1);
+        };
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIO.java b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIO.java
new file mode 100644
index 0000000..6e8f6a5
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIO.java
@@ -0,0 +1,14 @@
+package frc.robot.subsystems.superstructure;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface SuperstructureIO {
+    @AutoLog
+    class SuperstructureIOInputs {
+        public Superstructure.Goal currentGoal = Superstructure.Goal.IDLE;
+        public Superstructure.Goal desiredGoal = Superstructure.Goal.IDLE;
+        public boolean atGoal = true;
+    }
+
+    default void updateInputs(SuperstructureIOInputs inputs) {}
+}
diff --git a/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIOReal.java b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIOReal.java
new file mode 100644
index 0000000..af390fa
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/superstructure/SuperstructureIOReal.java
@@ -0,0 +1,6 @@
+package frc.robot.subsystems.superstructure;
+
+public class SuperstructureIOReal implements SuperstructureIO {
+    @Override
+    public void updateInputs(SuperstructureIOInputs inputs) {}
+}
diff --git a/src/main/java/frc/robot/util/ElasticDashboard.java b/src/main/java/frc/robot/util/ElasticDashboard.java
new file mode 100644
index 0000000..a13f11d
--- /dev/null
+++ b/src/main/java/frc/robot/util/ElasticDashboard.java
@@ -0,0 +1,365 @@
+package frc.robot.util;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.networktables.GenericEntry;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInLayouts;
+import edu.wpi.first.wpilibj.shuffleboard.BuiltInWidgets;
+import edu.wpi.first.wpilibj.shuffleboard.Shuffleboard;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardLayout;
+import edu.wpi.first.wpilibj.shuffleboard.ShuffleboardTab;
+import edu.wpi.first.wpilibj.smartdashboard.Field2d;
+import edu.wpi.first.wpilibj.smartdashboard.SendableChooser;
+import edu.wpi.first.wpilibj2.command.Command;
+import frc.robot.subsystems.drive.Drive;
+import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.superstructure.Superstructure;
+import java.util.Map;
+
+/**
+ * Utility class for managing Elastic Dashboard layouts and widgets. Provides a centralized interface for displaying
+ * robot telemetry.
+ */
+public class ElasticDashboard {
+    // Tabs
+    private final ShuffleboardTab mainTab;
+    private final ShuffleboardTab visionTab;
+    private final ShuffleboardTab mechanismTab;
+
+    // Field widget
+    private final Field2d field;
+
+    // Main tab widgets
+    private GenericEntry currentStateEntry;
+    private GenericEntry desiredStateEntry;
+    private GenericEntry atGoalEntry;
+    private GenericEntry matchTimeEntry;
+    private GenericEntry batteryVoltageEntry;
+    private GenericEntry allianceEntry;
+
+    // Drive tab widgets
+    private GenericEntry driveXEntry;
+    private GenericEntry driveYEntry;
+    private GenericEntry driveRotationEntry;
+    private GenericEntry gyroYawEntry;
+
+    // Mechanism tab widgets
+    private GenericEntry armCurrentEntry;
+    private GenericEntry armVoltageEntry;
+    private GenericEntry armPositionEntry;
+    private GenericEntry armVelocityEntry;
+    private GenericEntry rollerCurrentEntry;
+    private GenericEntry rollerVoltageEntry;
+    private GenericEntry rollerPositionEntry;
+    private GenericEntry rollerVelocityEntry;
+    private GenericEntry indexerCurrentEntry;
+    private GenericEntry indexerVoltageEntry;
+    private GenericEntry indexerPositionEntry;
+    private GenericEntry indexerVelocityEntry;
+
+    // Vision tab widgets (placeholders for camera streams)
+    private GenericEntry camera0StreamEntry;
+    private GenericEntry camera1StreamEntry;
+
+    // Auto chooser
+    private SendableChooser<Command> autoChooser;
+
+    public ElasticDashboard() {
+        // Create tabs
+        mainTab = Shuffleboard.getTab("Main");
+        visionTab = Shuffleboard.getTab("Vision");
+        mechanismTab = Shuffleboard.getTab("Mechanisms");
+
+        // Initialize field
+        field = new Field2d();
+
+        setupMainTab();
+        setupVisionTab();
+        setupMechTab();
+    }
+
+    /** Sets up the main competition tab with essential robot information. */
+    private void setupMainTab() {
+        // Field widget - large visualization
+        mainTab.add("Field", field)
+                .withWidget(BuiltInWidgets.kField)
+                .withSize(6, 3)
+                .withPosition(0, 0);
+
+        // Robot Status layout
+        ShuffleboardLayout robotStatus = mainTab.getLayout("Robot Status", BuiltInLayouts.kList)
+                .withSize(2, 3)
+                .withPosition(6, 0);
+
+        matchTimeEntry = robotStatus
+                .add("Match Time", 0.0)
+                .withWidget(BuiltInWidgets.kTextView)
+                .getEntry();
+
+        batteryVoltageEntry = robotStatus
+                .add("Battery Voltage", 0.0)
+                .withWidget(BuiltInWidgets.kVoltageView)
+                .withProperties(Map.of("min", 0, "max", 13))
+                .getEntry();
+
+        allianceEntry = robotStatus
+                .add("Alliance", "Unknown")
+                .withWidget(BuiltInWidgets.kTextView)
+                .getEntry();
+
+        // Superstructure Status layout
+        ShuffleboardLayout superstructureStatus = mainTab.getLayout("Superstructure", BuiltInLayouts.kList)
+                .withSize(2, 3)
+                .withPosition(8, 0);
+
+        currentStateEntry = superstructureStatus
+                .add("Current State", "IDLE")
+                .withWidget(BuiltInWidgets.kTextView)
+                .getEntry();
+
+        desiredStateEntry = superstructureStatus
+                .add("Desired State", "IDLE")
+                .withWidget(BuiltInWidgets.kTextView)
+                .getEntry();
+
+        atGoalEntry = superstructureStatus
+                .add("At Goal", false)
+                .withWidget(BuiltInWidgets.kBooleanBox)
+                .withProperties(Map.of("colorWhenTrue", "green", "colorWhenFalse", "red"))
+                .getEntry();
+
+        // Drive Telemetry layout
+        ShuffleboardLayout driveTelemetry = mainTab.getLayout("Drive Telemetry", BuiltInLayouts.kList)
+                .withSize(2, 4)
+                .withPosition(10, 0);
+
+        driveXEntry = driveTelemetry
+                .add("X Position (m)", 0.0)
+                .withWidget(BuiltInWidgets.kTextView)
+                .getEntry();
+
+        driveYEntry = driveTelemetry
+                .add("Y Position (m)", 0.0)
+                .withWidget(BuiltInWidgets.kTextView)
+                .getEntry();
+
+        driveRotationEntry = driveTelemetry
+                .add("Rotation (deg)", 0.0)
+                .withWidget(BuiltInWidgets.kTextView)
+                .getEntry();
+
+        gyroYawEntry = driveTelemetry
+                .add("Gyro Yaw (deg)", 0.0)
+                .withWidget(BuiltInWidgets.kGyro)
+                .getEntry();
+
+        // Intake value summary
+        ShuffleboardLayout intakeSummary = mainTab.getLayout("Intake Summary", BuiltInLayouts.kList)
+                .withSize(2, 3)
+                .withPosition(6, 3);
+
+        intakeSummary
+                .add("Arm Position", 0.0)
+                .withWidget(BuiltInWidgets.kNumberBar)
+                .withProperties(Map.of("min", -10, "max", 1));
+
+        intakeSummary
+                .add("Roller Speed", 0.0)
+                .withWidget(BuiltInWidgets.kNumberBar)
+                .withProperties(Map.of("min", -100, "max", 100));
+
+        // Swerve module visualization
+        ShuffleboardLayout moduleLayout = mainTab.getLayout("Swerve Modules", BuiltInLayouts.kGrid)
+                .withSize(4, 2)
+                .withPosition(6, 3)
+                .withProperties(Map.of("Number of columns", 2, "Number of rows", 2));
+    }
+
+    // Sets up the vision tab with camera streams
+    private void setupVisionTab() {
+        // Camera 0 stream
+        camera0StreamEntry = visionTab
+                .add("Camera 0", "mjpg:http://limelight-camera0.local:5800")
+                .withWidget(BuiltInWidgets.kCameraStream)
+                .withSize(5, 4)
+                .withPosition(0, 0)
+                .getEntry();
+
+        // Camera 1 stream
+        camera1StreamEntry = visionTab
+                .add("Camera 1", "mjpg:http://limelight-camera1.local:5800")
+                .withWidget(BuiltInWidgets.kCameraStream)
+                .withSize(5, 4)
+                .withPosition(5, 0)
+                .getEntry();
+
+        // Vision status
+        ShuffleboardLayout visionStatus = visionTab
+                .getLayout("Vision Status", BuiltInLayouts.kList)
+                .withSize(2, 4)
+                .withPosition(10, 0);
+    }
+
+    // Sets the auto chooser for the dashboard. Should be called from RobotContainer after AutoBuilder is configured.
+    public void setAutoChooser(SendableChooser<Command> chooser) {
+        this.autoChooser = chooser;
+        mainTab.add("Auto Chooser", chooser)
+                .withWidget(BuiltInWidgets.kComboBoxChooser)
+                .withSize(2, 1)
+                .withPosition(0, 4);
+    }
+
+    private void setupMechTab() {
+        ShuffleboardLayout IntakeArmValues = mechanismTab
+                .getLayout("Intake Arm", BuiltInLayouts.kList)
+                .withSize(2, 4)
+                .withPosition(0, 0);
+
+        armCurrentEntry = IntakeArmValues.add("Arm Current", 0.0)
+                .withWidget(BuiltInWidgets.kTextView)
+                .getEntry();
+
+        armVoltageEntry = IntakeArmValues.add("Arm Voltage", 0.0)
+                .withWidget(BuiltInWidgets.kVoltageView)
+                .getEntry();
+
+        armPositionEntry = IntakeArmValues.add("Arm Position (Rad)", 0.0)
+                .withWidget(BuiltInWidgets.kTextView)
+                .getEntry();
+
+        armVelocityEntry = IntakeArmValues.add("Arm Velocity (Rad/s)", 0.0)
+                .withWidget(BuiltInWidgets.kTextView)
+                .getEntry();
+
+        ShuffleboardLayout IntakeRollerValues = mechanismTab
+                .getLayout("Intake Roller", BuiltInLayouts.kList)
+                .withSize(2, 4)
+                .withPosition(2, 0);
+
+        rollerCurrentEntry = IntakeRollerValues.add("Roller Current", 0.0)
+                .withWidget(BuiltInWidgets.kTextView)
+                .getEntry();
+
+        rollerVoltageEntry = IntakeRollerValues.add("Roller Voltage", 0.0)
+                .withWidget(BuiltInWidgets.kVoltageView)
+                .getEntry();
+
+        rollerPositionEntry = IntakeRollerValues.add("Roller Position (Rad)", 0.0)
+                .withWidget(BuiltInWidgets.kTextView)
+                .getEntry();
+
+        rollerVelocityEntry = IntakeRollerValues.add("Roller Velocity (Rad/s)", 0.0)
+                .withWidget(BuiltInWidgets.kTextView)
+                .getEntry();
+
+        ShuffleboardLayout IndexerValues = mechanismTab
+                .getLayout("Indexer", BuiltInLayouts.kList)
+                .withSize(2, 4)
+                .withPosition(4, 0);
+
+        indexerCurrentEntry = IndexerValues.add("Indexer Current", 0.0)
+                .withWidget(BuiltInWidgets.kTextView)
+                .getEntry();
+
+        indexerVoltageEntry = IndexerValues.add("Indexer Voltage", 0.0)
+                .withWidget(BuiltInWidgets.kVoltageView)
+                .getEntry();
+
+        indexerPositionEntry = IndexerValues.add("Indexer Position (Rad)", 0.0)
+                .withWidget(BuiltInWidgets.kTextView)
+                .getEntry();
+
+        indexerVelocityEntry = IndexerValues.add("Indexer Velocity (Rad/s)", 0.0)
+                .withWidget(BuiltInWidgets.kTextView)
+                .getEntry();
+    }
+
+    // Updates all dashboard values
+    public void update(Drive drive, Superstructure superstructure, Intake intake) {
+        // Update match time
+        double matchTime = DriverStation.getMatchTime();
+        matchTimeEntry.setDouble(matchTime);
+
+        // Update battery voltage
+        double batteryVoltage = RobotController.getBatteryVoltage();
+        batteryVoltageEntry.setDouble(batteryVoltage);
+
+        // Update alliance
+        var alliance = DriverStation.getAlliance();
+        if (alliance.isPresent()) {
+            allianceEntry.setString(alliance.get().toString());
+        } else {
+            allianceEntry.setString("Unknown");
+        }
+
+        // Update superstructure state
+        updateSuperstructureState(superstructure);
+
+        // Update drive telemetry
+        updateDriveTelemetry(drive);
+
+        // Update field
+        updateField(drive);
+
+        // Update Mechanism values
+        updateMechs(intake);
+    }
+
+    // Updates superstructure state info
+    private void updateSuperstructureState(Superstructure superstructure) {
+        atGoalEntry.setBoolean(superstructure.atGoal());
+        currentStateEntry.setString(superstructure.getCurrentGoal().toString());
+        desiredStateEntry.setString(superstructure.getDesiredGoal().toString());
+    }
+
+    private void updateMechs(Intake intake) {
+
+        // Update Intake Arm values
+        armCurrentEntry.setDouble(intake.getArmCurrent());
+        armVoltageEntry.setDouble(intake.getArmVoltage());
+        armPositionEntry.setDouble(intake.getArmPositionRad());
+        armVelocityEntry.setDouble(intake.getArmVelocityRadPerSec());
+
+        // Update Intake Roller values
+        rollerCurrentEntry.setDouble(intake.getRollerCurrent());
+        rollerVoltageEntry.setDouble(intake.getRollerVoltage());
+        rollerPositionEntry.setDouble(intake.getRollerPositionRad());
+        rollerVelocityEntry.setDouble(intake.getRollerVelocityRadPerSec());
+
+        // Update Indexer values
+        indexerCurrentEntry.setDouble(intake.getIndexerCurrent());
+        indexerVoltageEntry.setDouble(intake.getIndexerVoltage());
+        indexerPositionEntry.setDouble(intake.getIndexerPositionRad());
+        indexerVelocityEntry.setDouble(intake.getIndexerVelocityRadPerSec());
+    }
+
+    // Updates drive info
+    private void updateDriveTelemetry(Drive drive) {
+        Pose2d pose = drive.getPose();
+        Rotation2d rotation = drive.getRotation();
+
+        // Update position
+        driveXEntry.setDouble(pose.getX());
+        driveYEntry.setDouble(pose.getY());
+        driveRotationEntry.setDouble(rotation.getDegrees());
+        gyroYawEntry.setDouble(rotation.getDegrees());
+    }
+
+    // Updates the field widget with current robot pose
+    private void updateField(Drive drive) {
+        field.setRobotPose(drive.getPose());
+    }
+
+    // gets field for external use
+    public Field2d getField() {
+        return field;
+    }
+
+    // Set camera stream URLS
+    public void setCameraStreams(String camera0Url, String camera1Url) {
+        camera0StreamEntry.setString(camera0Url);
+        camera1StreamEntry.setString(camera1Url);
+    }
+}