diff --git a/.vscode/settings.json b/.vscode/settings.json
index dccbc7c..c9c0948 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -56,5 +56,6 @@
     "edu.wpi.first.math.proto.*",
     "edu.wpi.first.math.**.proto.*",
     "edu.wpi.first.math.**.struct.*",
-  ]
+  ],
+  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx16G -Xms100m -Xlog:disable"
 }
diff --git a/simgui-ds.json b/simgui-ds.json
index 73cc713..b16ea5c 100644
--- a/simgui-ds.json
+++ b/simgui-ds.json
@@ -88,5 +88,14 @@
       "buttonCount": 0,
       "povCount": 0
     }
+  ],
+  "robotJoysticks": [
+    {
+      "useGamepad": true
+    },
+    {
+      "guid": "78696e70757401000000000000000000",
+      "useGamepad": true
+    }
   ]
 }
diff --git a/src/main/deploy/pathplanner/navgrid.json b/src/main/deploy/pathplanner/navgrid.json
new file mode 100644
index 0000000..23e0db9
--- /dev/null
+++ b/src/main/deploy/pathplanner/navgrid.json
@@ -0,0 +1 @@
+{"field_size":{"x":17.548,"y":8.052},"nodeSizeMeters":0.3,"grid":[[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true],[true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true],[true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true],[true,true,true,true,true,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,false,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true],[true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true,true]]}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/paths/Example Path.path b/src/main/deploy/pathplanner/paths/Example Path.path
new file mode 100644
index 0000000..6232c20
--- /dev/null
+++ b/src/main/deploy/pathplanner/paths/Example Path.path	
@@ -0,0 +1,54 @@
+{
+  "version": "2025.0",
+  "waypoints": [
+    {
+      "anchor": {
+        "x": 2.0,
+        "y": 7.0
+      },
+      "prevControl": null,
+      "nextControl": {
+        "x": 3.0,
+        "y": 7.0
+      },
+      "isLocked": false,
+      "linkedName": null
+    },
+    {
+      "anchor": {
+        "x": 4.0,
+        "y": 6.0
+      },
+      "prevControl": {
+        "x": 3.0,
+        "y": 6.0
+      },
+      "nextControl": null,
+      "isLocked": false,
+      "linkedName": null
+    }
+  ],
+  "rotationTargets": [],
+  "constraintZones": [],
+  "pointTowardsZones": [],
+  "eventMarkers": [],
+  "globalConstraints": {
+    "maxVelocity": 5.0,
+    "maxAcceleration": 5.6,
+    "maxAngularVelocity": 540.0,
+    "maxAngularAcceleration": 720.0,
+    "nominalVoltage": 12.0,
+    "unlimited": false
+  },
+  "goalEndState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "reversed": false,
+  "folder": null,
+  "idealStartingState": {
+    "velocity": 0,
+    "rotation": 0.0
+  },
+  "useDefaultConstraints": true
+}
\ No newline at end of file
diff --git a/src/main/deploy/pathplanner/settings.json b/src/main/deploy/pathplanner/settings.json
new file mode 100644
index 0000000..603f423
--- /dev/null
+++ b/src/main/deploy/pathplanner/settings.json
@@ -0,0 +1,32 @@
+{
+  "robotWidth": 0.9,
+  "robotLength": 0.9,
+  "holonomicMode": true,
+  "pathFolders": [],
+  "autoFolders": [],
+  "defaultMaxVel": 5.0,
+  "defaultMaxAccel": 5.6,
+  "defaultMaxAngVel": 540.0,
+  "defaultMaxAngAccel": 720.0,
+  "defaultNominalVoltage": 12.0,
+  "robotMass": 48.0,
+  "robotMOI": 6.883,
+  "robotTrackwidth": 0.546,
+  "driveWheelRadius": 0.048,
+  "driveGearing": 5.143,
+  "maxDriveSpeed": 5.45,
+  "driveMotorType": "krakenX60",
+  "driveCurrentLimit": 60.0,
+  "wheelCOF": 1.2,
+  "flModuleX": 0.273,
+  "flModuleY": 0.273,
+  "frModuleX": 0.273,
+  "frModuleY": -0.273,
+  "blModuleX": -0.273,
+  "blModuleY": 0.273,
+  "brModuleX": -0.273,
+  "brModuleY": -0.273,
+  "bumperOffsetX": 0.0,
+  "bumperOffsetY": 0.0,
+  "robotFeatures": []
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/Constants.java b/src/main/java/frc/robot/Constants.java
index eb7d9c5..f5dbaed 100644
--- a/src/main/java/frc/robot/Constants.java
+++ b/src/main/java/frc/robot/Constants.java
@@ -1,10 +1,31 @@
 package frc.robot;
 
+import edu.wpi.first.wpilibj2.command.button.CommandXboxController;
+
 public class Constants {
+    public static final boolean tuningMode = false;
+    public static final class IntakeSetpoints{
+      public static final double intake = -0.6;
+      public static final double slow = -0.3;
+      public static final double reverse = 0.7;
+    }
     public static Mode currentMode = Mode.REAL;
     public static enum Mode {
-        REAL,
         SIM,
+        REAL,
         REPLAY
     }
+
+    public static class OIConstants {
+        public static final CommandXboxController driverController = new CommandXboxController(0);
+        public static final CommandXboxController operatorController = new CommandXboxController(1);
+        public static final double KAxisDeadband = 0.1;  
+    }
+
+    public static enum Gamepiece {
+        ALGAE,
+        CORAL,
+        NONE,
+        SIM,
+    }
 }
diff --git a/src/main/java/frc/robot/Main.java b/src/main/java/frc/robot/Main.java
index fe215d7..5585907 100644
--- a/src/main/java/frc/robot/Main.java
+++ b/src/main/java/frc/robot/Main.java
@@ -4,12 +4,24 @@
 
 package frc.robot;
 
+import java.io.IOException;
+
 import edu.wpi.first.wpilibj.RobotBase;
 
-public final class Main {
+public final class Main  {
   private Main() {}
 
+  public String Bruh = "Bruh";
+
   public static void main(String... args) {
-    RobotBase.startRobot(Robot::new);
+    RobotBase.startRobot(() -> {
+      try {
+        return new Robot();
+      } catch (IOException | org.json.simple.parser.ParseException e) {
+        // TODO Auto-generated catch block
+        e.printStackTrace();
+      }
+            return null;
+    });
   }
 }
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 0f8efc1..0e01211 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -4,6 +4,9 @@
 
 package frc.robot;
 
+import java.io.IOException;
+
+import org.json.simple.parser.ParseException;
 import org.littletonrobotics.junction.LogFileUtil;
 import org.littletonrobotics.junction.LoggedRobot;
 import org.littletonrobotics.junction.Logger;
@@ -11,17 +14,18 @@
 import org.littletonrobotics.junction.wpilog.WPILOGReader;
 import org.littletonrobotics.junction.wpilog.WPILOGWriter;
 
-import edu.wpi.first.wpilibj.PowerDistribution;
-import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
+import com.pathplanner.lib.pathfinding.Pathfinding;
+
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
+import frc.robot.Util.LocalADStarAK;
 
 public class Robot extends LoggedRobot {
   private Command m_autonomousCommand;
 
   private final RobotContainer m_robotContainer;
 
-  public Robot() {
+  public Robot() throws IOException, ParseException {
     Logger.recordMetadata("ProjectName", "2025-Reefscape");
     switch (Constants.currentMode) {
       case REAL:
@@ -50,6 +54,11 @@ public void robotPeriodic() {
     CommandScheduler.getInstance().run();
   }
 
+  @Override
+  public void robotInit(){
+    Pathfinding.setPathfinder(new LocalADStarAK());
+  }
+
   @Override
   public void disabledInit() {}
 
diff --git a/src/main/java/frc/robot/RobotContainer.java b/src/main/java/frc/robot/RobotContainer.java
index de2c9d0..3f0bde1 100644
--- a/src/main/java/frc/robot/RobotContainer.java
+++ b/src/main/java/frc/robot/RobotContainer.java
@@ -4,17 +4,159 @@
 
 package frc.robot;
 
+import static edu.wpi.first.units.Units.*;
+
+import com.ctre.phoenix6.swerve.SwerveModule.DriveRequestType;
+import com.ctre.phoenix6.swerve.SwerveRequest;
+
+import java.io.IOException;
+
+import org.json.simple.parser.ParseException;
+
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.Commands;
 
+import frc.robot.generated.TunerConstants;
+import frc.robot.subsystems.CommandSwerveDrivetrain;
+import frc.robot.subsystems.elevator.Elevator;
+import frc.robot.subsystems.elevator.ElevatorConstants;
+import frc.robot.subsystems.elevator.ElevatorIOReal;
+import frc.robot.subsystems.elevator.ElevatorIOSim;
+import frc.robot.Constants.OIConstants;
+import frc.robot.subsystems.intake.Intake;
+import frc.robot.subsystems.intake.IntakeIORealVortex;
+import frc.robot.subsystems.intake.IntakeIOSim;
+import frc.robot.subsystems.arm.Arm;
+import frc.robot.subsystems.arm.ArmConstants;
+import frc.robot.subsystems.arm.ArmIOSim;
+import frc.robot.subsystems.arm.ArmIOReal;
+
 public class RobotContainer {
-  public RobotContainer() {
+  private double MaxSpeed = TunerConstants.kSpeedAt12Volts.in(MetersPerSecond);
+  private double MaxAngularRate = RotationsPerSecond.of(0.75).in(RadiansPerSecond);
+
+  /* Setting up bindings for necessary control of the swerve drive platform */
+  private final SwerveRequest.FieldCentric drive = new SwerveRequest.FieldCentric()
+      .withDeadband(MaxSpeed * 0.1).withRotationalDeadband(MaxAngularRate * 0.1) // Add a 10% deadband
+      .withDriveRequestType(DriveRequestType.OpenLoopVoltage); // Use open-loop control for drive motors
+  private final SwerveRequest.SwerveDriveBrake brake = new SwerveRequest.SwerveDriveBrake();
+  private final SwerveRequest.PointWheelsAt point = new SwerveRequest.PointWheelsAt();
+
+  private final Telemetry logger = new Telemetry(MaxSpeed);
+
+  public final CommandSwerveDrivetrain drivetrain = TunerConstants.createDrivetrain();
+
+  private Elevator elevator;
+  // private Drive drive;
+  private Intake intake;
+
+  public RobotContainer() throws IOException, ParseException {
+    switch (Constants.currentMode) {
+      case REAL:
+        elevator = Elevator.initialize(new ElevatorIOReal());
+        intake = Intake.initialize(new IntakeIORealVortex());
+        Arm.initialize(new ArmIOReal());
+        break;
+
+      case SIM:
+        elevator = Elevator.initialize(new ElevatorIOSim());
+        intake = Intake.initialize(new IntakeIOSim());
+        Arm.initialize(new ArmIOSim());
+        break;
+
+      default:
+        elevator = Elevator.initialize(new ElevatorIOSim());
+        intake = Intake.initialize(new IntakeIOSim());
+        break;
+
+    }
+    RobotVisualizer robotVisualizer = new RobotVisualizer();
     configureBindings();
   }
 
-  private void configureBindings() {}
+  private void configureBindings() {
+    // Note that X is defined as forward according to WPILib convention,
+    // and Y is defined as to the left according to WPILib convention.
+    drivetrain.setDefaultCommand(
+        // Drivetrain will execute this command periodically
+        drivetrain
+            .applyRequest(() -> drive.withVelocityX(-Constants.OIConstants.driverController.getLeftY() * MaxSpeed)
+                .withVelocityY(-Constants.OIConstants.driverController.getLeftX() * MaxSpeed)
+                .withRotationalRate(-Constants.OIConstants.driverController.getRightX() * MaxAngularRate)));
+
+    // Constants.OIConstants.driverController.back().and(Constants.OIConstants.driverController.y()).whileTrue(drivetrain.sysIdDynamic(Direction.kForward));
+    // Constants.OIConstants.driverController.back().and(Constants.OIConstants.driverController.x()).whileTrue(drivetrain.sysIdDynamic(Direction.kReverse));
+    // Constants.OIConstants.driverController.start().and(Constants.OIConstants.driverController.y()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kForward));
+    // Constants.OIConstants.driverController.start().and(Constants.OIConstants.driverController.x()).whileTrue(drivetrain.sysIdQuasistatic(Direction.kReverse));
+
+    Constants.OIConstants.driverController.a().onTrue(drivetrain.runOnce(() -> drivetrain.seedFieldCentric()));
+
+    drivetrain.registerTelemetry(logger::telemeterize);
+
+    OIConstants.operatorController.leftTrigger()
+        .whileTrue(
+            Commands.startEnd(() -> elevator.moveElevator(OIConstants.operatorController.getLeftTriggerAxis() / 1.5),
+                () -> elevator.moveElevator(0),
+                elevator));
+
+    OIConstants.operatorController.rightTrigger()
+        .whileTrue(Commands.startEnd(
+            () -> elevator.moveElevator(-OIConstants.operatorController.getRightTriggerAxis() / 1.5),
+            () -> elevator.moveElevator(0),
+            elevator));
+
+    // OIConstants.operatorController.leftBumper().whileTrue(
+    // Arm.getInstance().sysIDRoutine().quasistatic(SysIdRoutine.Direction.kForward)
+    // );
+
+    // OIConstants.operatorController.rightBumper().whileTrue(
+    // Arm.getInstance().sysIDRoutine().quasistatic(SysIdRoutine.Direction.kReverse)
+    // );
+
+    // OIConstants.operatorController.leftTrigger().whileTrue(
+    // Arm.getInstance().sysIDRoutine().dynamic(SysIdRoutine.Direction.kForward)
+    // );
+
+    // OIConstants.operatorController.rightTrigger().whileTrue(
+    // Arm.getInstance().sysIDRoutine().dynamic(SysIdRoutine.Direction.kReverse)
+    // );
+
+    Constants.OIConstants.operatorController.povRight().onTrue(
+        Commands.sequence(
+            Commands.parallel(
+                Commands.runOnce(() -> Arm.getInstance().setGoal(1.02), Arm.getInstance()),
+                Elevator.getInstance().setGoal(0.57))));
+
+    Constants.OIConstants.operatorController.povLeft().onTrue(
+        Commands.sequence(
+            Elevator.getInstance().setGoal(ElevatorConstants.minDistance),
+            Commands.runOnce(() -> Arm.getInstance().setGoal(-1.8), Arm.getInstance()),
+            Commands.waitSeconds(1.5)));
+
+    // Constants.OIConstants.operatorController.povUp()
+    // .whileTrue(
+    // Commands.sequence(
+    // intake.intake().until(() -> intake.GamePieceInitial()),
+    // intake.intakeSlow().until(() -> intake.GamePieceFinal()),
+    // Commands.run(() -> intake.setIntakeSpeed(0))
+    // )
+    // );
+
+    Constants.OIConstants.operatorController.x().whileTrue(Commands.startEnd(() -> intake.setIntakeSpeed(-1),
+        () -> intake.setIntakeSpeed(0), intake));
+
+    Constants.OIConstants.operatorController.b().whileTrue(Commands.startEnd(() -> intake.setIntakeSpeed(1),
+        () -> intake.setIntakeSpeed(0), intake));
+
+    // OIConstants.driverController.povUp().onTrue(Commands.runOnce(() ->
+    // Arm.getInstance().setGoal(ArmConstants.CORAL_L3)));
+    // OIConstants.driverController.leftBumper().onTrue(Commands.runOnce(() ->
+    // Arm.getInstance().setGoal(-1.8)));
+    // OIConstants.driverController.rightBumper().onTrue(Commands.runOnce(() ->
+    // Arm.getInstance().setGoal(ArmConstants.CORAL_L2)));
+  }
 
   public Command getAutonomousCommand() {
-    return Commands.print("No autonomous command configured");
+    return Commands.runOnce(() -> Arm.getInstance().setGoal(ArmConstants.CORAL_L4), Arm.getInstance());
   }
 }
diff --git a/src/main/java/frc/robot/RobotVisualizer.java b/src/main/java/frc/robot/RobotVisualizer.java
new file mode 100644
index 0000000..d96b44a
--- /dev/null
+++ b/src/main/java/frc/robot/RobotVisualizer.java
@@ -0,0 +1,45 @@
+package frc.robot;
+
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.mechanism.LoggedMechanism2d;
+import org.littletonrobotics.junction.mechanism.LoggedMechanismLigament2d;
+import org.littletonrobotics.junction.mechanism.LoggedMechanismRoot2d;
+
+import edu.wpi.first.apriltag.AprilTagFieldLayout;
+import edu.wpi.first.apriltag.AprilTagFields;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Transform2d;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.subsystems.arm.Arm;
+import frc.robot.subsystems.arm.ArmConstants;
+import frc.robot.subsystems.elevator.Elevator;
+import frc.robot.subsystems.elevator.ElevatorConstants;
+import frc.robot.subsystems.intake.Intake;
+
+public class RobotVisualizer extends SubsystemBase {
+    private LoggedMechanism2d robotVisualizer = new LoggedMechanism2d(2, 2.5);    
+    private LoggedMechanismRoot2d robotRoot = robotVisualizer.getRoot("Robot Visualizer", 1, 0.1);
+    
+    private LoggedMechanismLigament2d elevatorstage1Ligament = robotRoot.append(new LoggedMechanismLigament2d("Elevator Stage 1 Ligament", ElevatorConstants.maxDistance/.9, 90, 10, new Color8Bit(Color.kRed)));
+    private LoggedMechanismLigament2d elevatorstage2Ligament = robotRoot.append(new LoggedMechanismLigament2d("Elevator Stage 2 Ligament", ElevatorConstants.maxDistance, 90, 9, new Color8Bit(Color.kBlue)));
+    private LoggedMechanismLigament2d armLigament = elevatorstage2Ligament.append(new LoggedMechanismLigament2d("Arm Ligament", ArmConstants.Sim.LENGTH, -90, 8, new Color8Bit(Color.kGreen)));
+    private LoggedMechanismLigament2d intakeLigament = armLigament.append(new LoggedMechanismLigament2d("Intake Ligament", 0.1, -90, 7, new Color8Bit(Color.kYellow)));
+
+    public RobotVisualizer() {
+
+    }
+
+    @Override
+    public void periodic() {
+        intakeLigament.setAngle(Units.rotationsToDegrees(Intake.getInstance().getPosition()));
+        armLigament.setAngle(Units.radiansToDegrees(Arm.getInstance().getAngle()) - 90);
+        elevatorstage2Ligament.setLength(ElevatorConstants.maxDistance/.9 + Elevator.getInstance().getPosition());
+        Logger.recordOutput("RobotVisualizer", robotVisualizer);
+        Pose2d fakePose = AprilTagFieldLayout.loadField(AprilTagFields.k2025Reefscape).getTagPose(7).get().toPose2d().plus(new Transform2d(1, 0, new Rotation2d(Math.PI)));
+        Logger.recordOutput("Fake robot pose", fakePose);
+    }
+}
diff --git a/src/main/java/frc/robot/Telemetry.java b/src/main/java/frc/robot/Telemetry.java
new file mode 100644
index 0000000..edf1979
--- /dev/null
+++ b/src/main/java/frc/robot/Telemetry.java
@@ -0,0 +1,124 @@
+package frc.robot;
+
+import com.ctre.phoenix6.SignalLogger;
+import com.ctre.phoenix6.swerve.SwerveDrivetrain.SwerveDriveState;
+
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.kinematics.ChassisSpeeds;
+import edu.wpi.first.math.kinematics.SwerveModulePosition;
+import edu.wpi.first.math.kinematics.SwerveModuleState;
+import edu.wpi.first.networktables.DoubleArrayPublisher;
+import edu.wpi.first.networktables.DoublePublisher;
+import edu.wpi.first.networktables.NetworkTable;
+import edu.wpi.first.networktables.NetworkTableInstance;
+import edu.wpi.first.networktables.StringPublisher;
+import edu.wpi.first.networktables.StructArrayPublisher;
+import edu.wpi.first.networktables.StructPublisher;
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+
+public class Telemetry {
+    private final double MaxSpeed;
+
+    /**
+     * Construct a telemetry object, with the specified max speed of the robot
+     * 
+     * @param maxSpeed Maximum speed in meters per second
+     */
+    public Telemetry(double maxSpeed) {
+        MaxSpeed = maxSpeed;
+        SignalLogger.start();
+    }
+
+    /* What to publish over networktables for telemetry */
+    private final NetworkTableInstance inst = NetworkTableInstance.getDefault();
+
+    /* Robot swerve drive state */
+    private final NetworkTable driveStateTable = inst.getTable("DriveState");
+    private final StructPublisher<Pose2d> drivePose = driveStateTable.getStructTopic("Pose", Pose2d.struct).publish();
+    private final StructPublisher<ChassisSpeeds> driveSpeeds = driveStateTable.getStructTopic("Speeds", ChassisSpeeds.struct).publish();
+    private final StructArrayPublisher<SwerveModuleState> driveModuleStates = driveStateTable.getStructArrayTopic("ModuleStates", SwerveModuleState.struct).publish();
+    private final StructArrayPublisher<SwerveModuleState> driveModuleTargets = driveStateTable.getStructArrayTopic("ModuleTargets", SwerveModuleState.struct).publish();
+    private final StructArrayPublisher<SwerveModulePosition> driveModulePositions = driveStateTable.getStructArrayTopic("ModulePositions", SwerveModulePosition.struct).publish();
+    private final DoublePublisher driveTimestamp = driveStateTable.getDoubleTopic("Timestamp").publish();
+    private final DoublePublisher driveOdometryFrequency = driveStateTable.getDoubleTopic("OdometryFrequency").publish();
+
+    /* Robot pose for field positioning */
+    private final NetworkTable table = inst.getTable("Pose");
+    private final DoubleArrayPublisher fieldPub = table.getDoubleArrayTopic("robotPose").publish();
+    private final StringPublisher fieldTypePub = table.getStringTopic(".type").publish();
+
+    /* Mechanisms to represent the swerve module states */
+    private final Mechanism2d[] m_moduleMechanisms = new Mechanism2d[] {
+        new Mechanism2d(1, 1),
+        new Mechanism2d(1, 1),
+        new Mechanism2d(1, 1),
+        new Mechanism2d(1, 1),
+    };
+    /* A direction and length changing ligament for speed representation */
+    private final MechanismLigament2d[] m_moduleSpeeds = new MechanismLigament2d[] {
+        m_moduleMechanisms[0].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)),
+        m_moduleMechanisms[1].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)),
+        m_moduleMechanisms[2].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)),
+        m_moduleMechanisms[3].getRoot("RootSpeed", 0.5, 0.5).append(new MechanismLigament2d("Speed", 0.5, 0)),
+    };
+    /* A direction changing and length constant ligament for module direction */
+    private final MechanismLigament2d[] m_moduleDirections = new MechanismLigament2d[] {
+        m_moduleMechanisms[0].getRoot("RootDirection", 0.5, 0.5)
+            .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
+        m_moduleMechanisms[1].getRoot("RootDirection", 0.5, 0.5)
+            .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
+        m_moduleMechanisms[2].getRoot("RootDirection", 0.5, 0.5)
+            .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
+        m_moduleMechanisms[3].getRoot("RootDirection", 0.5, 0.5)
+            .append(new MechanismLigament2d("Direction", 0.1, 0, 0, new Color8Bit(Color.kWhite))),
+    };
+
+    private final double[] m_poseArray = new double[3];
+    private final double[] m_moduleStatesArray = new double[8];
+    private final double[] m_moduleTargetsArray = new double[8];
+
+    /** Accept the swerve drive state and telemeterize it to SmartDashboard and SignalLogger. */
+    public void telemeterize(SwerveDriveState state) {
+        /* Telemeterize the swerve drive state */
+        drivePose.set(state.Pose);
+        driveSpeeds.set(state.Speeds);
+        driveModuleStates.set(state.ModuleStates);
+        driveModuleTargets.set(state.ModuleTargets);
+        driveModulePositions.set(state.ModulePositions);
+        driveTimestamp.set(state.Timestamp);
+        driveOdometryFrequency.set(1.0 / state.OdometryPeriod);
+
+        /* Also write to log file */
+        m_poseArray[0] = state.Pose.getX();
+        m_poseArray[1] = state.Pose.getY();
+        m_poseArray[2] = state.Pose.getRotation().getDegrees();
+        for (int i = 0; i < 4; ++i) {
+            m_moduleStatesArray[i*2 + 0] = state.ModuleStates[i].angle.getRadians();
+            m_moduleStatesArray[i*2 + 1] = state.ModuleStates[i].speedMetersPerSecond;
+            m_moduleTargetsArray[i*2 + 0] = state.ModuleTargets[i].angle.getRadians();
+            m_moduleTargetsArray[i*2 + 1] = state.ModuleTargets[i].speedMetersPerSecond;
+        }
+
+        SignalLogger.writeDoubleArray("DriveState/Pose", m_poseArray);
+        SignalLogger.writeDoubleArray("DriveState/ModuleStates", m_moduleStatesArray);
+        SignalLogger.writeDoubleArray("DriveState/ModuleTargets", m_moduleTargetsArray);
+        SignalLogger.writeDouble("DriveState/OdometryPeriod", state.OdometryPeriod, "seconds");
+
+        /* Telemeterize the pose to a Field2d */
+        fieldTypePub.set("Field2d");
+        fieldPub.set(m_poseArray);
+
+        /* Telemeterize the module states to a Mechanism2d */
+        for (int i = 0; i < 4; ++i) {
+            m_moduleSpeeds[i].setAngle(state.ModuleStates[i].angle);
+            m_moduleDirections[i].setAngle(state.ModuleStates[i].angle);
+            m_moduleSpeeds[i].setLength(state.ModuleStates[i].speedMetersPerSecond / (2 * MaxSpeed));
+
+            SmartDashboard.putData("Module " + i, m_moduleMechanisms[i]);
+        }
+    }
+}
diff --git a/src/main/java/frc/robot/Util/LocalADStarAK.java b/src/main/java/frc/robot/Util/LocalADStarAK.java
new file mode 100644
index 0000000..3f4170b
--- /dev/null
+++ b/src/main/java/frc/robot/Util/LocalADStarAK.java
@@ -0,0 +1,149 @@
+package frc.robot.Util;
+
+import edu.wpi.first.math.Pair;
+import edu.wpi.first.math.geometry.Translation2d;
+import com.pathplanner.lib.path.GoalEndState;
+import com.pathplanner.lib.path.PathConstraints;
+import com.pathplanner.lib.path.PathPlannerPath;
+import com.pathplanner.lib.path.PathPoint;
+import com.pathplanner.lib.pathfinding.Pathfinder;
+import com.pathplanner.lib.pathfinding.LocalADStar;
+import java.util.ArrayList;
+import java.util.Collections;
+import java.util.List;
+import org.littletonrobotics.junction.LogTable;
+import org.littletonrobotics.junction.Logger;
+import org.littletonrobotics.junction.inputs.LoggableInputs;
+
+public class LocalADStarAK implements Pathfinder {
+  private final ADStarIO io = new ADStarIO();
+
+  /**
+   * Get if a new path has been calculated since the last time a path was retrieved
+   *
+   * @return True if a new path is available
+   */
+  @Override
+  public boolean isNewPathAvailable() {
+    if (!Logger.hasReplaySource()) {
+      io.updateIsNewPathAvailable();
+    }
+
+    Logger.processInputs("LocalADStarAK", io);
+
+    return io.isNewPathAvailable;
+  }
+
+  /**
+   * Get the most recently calculated path
+   *
+   * @param constraints The path constraints to use when creating the path
+   * @param goalEndState The goal end state to use when creating the path
+   * @return The PathPlannerPath created from the points calculated by the pathfinder
+   */
+  @Override
+  public PathPlannerPath getCurrentPath(PathConstraints constraints, GoalEndState goalEndState) {
+    if (!Logger.hasReplaySource()) {
+      io.updateCurrentPathPoints(constraints, goalEndState);
+    }
+
+    Logger.processInputs("LocalADStarAK", io);
+
+    if (io.currentPathPoints.isEmpty()) {
+      return null;
+    }
+
+    return PathPlannerPath.fromPathPoints(io.currentPathPoints, constraints, goalEndState);
+  }
+
+  /**
+   * Set the start position to pathfind from
+   *
+   * @param startPosition Start position on the field. If this is within an obstacle it will be
+   *     moved to the nearest non-obstacle node.
+   */
+  @Override
+  public void setStartPosition(Translation2d startPosition) {
+    if (!Logger.hasReplaySource()) {
+      io.adStar.setStartPosition(startPosition);
+    }
+  }
+
+  /**
+   * Set the goal position to pathfind to
+   *
+   * @param goalPosition Goal position on the field. f this is within an obstacle it will be moved
+   *     to the nearest non-obstacle node.
+   */
+  @Override
+  public void setGoalPosition(Translation2d goalPosition) {
+    if (!Logger.hasReplaySource()) {
+      io.adStar.setGoalPosition(goalPosition);
+    }
+  }
+
+  /**
+   * Set the dynamic obstacles that should be avoided while pathfinding.
+   *
+   * @param obs A List of Translation2d pairs representing obstacles. Each Translation2d represents
+   *     opposite corners of a bounding box.
+   * @param currentRobotPos The current position of the robot. This is needed to change the start
+   *     position of the path to properly avoid obstacles
+   */
+  @Override
+  public void setDynamicObstacles(
+      List<Pair<Translation2d, Translation2d>> obs, Translation2d currentRobotPos) {
+    if (!Logger.hasReplaySource()) {
+      io.adStar.setDynamicObstacles(obs, currentRobotPos);
+    }
+  }
+
+  private static class ADStarIO implements LoggableInputs {
+    public LocalADStar adStar = new LocalADStar();
+    public boolean isNewPathAvailable = false;
+    public List<PathPoint> currentPathPoints = Collections.emptyList();
+
+    @Override
+    public void toLog(LogTable table) {
+      table.put("IsNewPathAvailable", isNewPathAvailable);
+
+      double[] pointsLogged = new double[currentPathPoints.size() * 2];
+      int idx = 0;
+      for (PathPoint point : currentPathPoints) {
+        pointsLogged[idx] = point.position.getX();
+        pointsLogged[idx + 1] = point.position.getY();
+        idx += 2;
+      }
+
+      table.put("CurrentPathPoints", pointsLogged);
+    }
+
+    @Override
+    public void fromLog(LogTable table) {
+      isNewPathAvailable = table.get("IsNewPathAvailable", false);
+
+      double[] pointsLogged = table.get("CurrentPathPoints", new double[0]);
+
+      List<PathPoint> pathPoints = new ArrayList<>();
+      for (int i = 0; i < pointsLogged.length; i += 2) {
+        pathPoints.add(new PathPoint(new Translation2d(pointsLogged[i], pointsLogged[i + 1]), null));
+      }
+
+      currentPathPoints = pathPoints;
+    }
+
+    public void updateIsNewPathAvailable() {
+      isNewPathAvailable = adStar.isNewPathAvailable();
+    }
+
+    public void updateCurrentPathPoints(PathConstraints constraints, GoalEndState goalEndState) {
+      PathPlannerPath currentPath = adStar.getCurrentPath(constraints, goalEndState);
+
+      if (currentPath != null) {
+        currentPathPoints = currentPath.getAllPathPoints();
+      } else {
+        currentPathPoints = Collections.emptyList();
+      }
+    }
+  }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/Util/LoggedTunableNumber.java b/src/main/java/frc/robot/Util/LoggedTunableNumber.java
new file mode 100644
index 0000000..23ecf6d
--- /dev/null
+++ b/src/main/java/frc/robot/Util/LoggedTunableNumber.java
@@ -0,0 +1,122 @@
+// Copyright (c) 2025 FRC 6328
+// http://github.com/Mechanical-Advantage
+//
+// Use of this source code is governed by an MIT-style
+// license that can be found in the LICENSE file at
+// the root directory of this project.
+package frc.robot.Util;
+
+import frc.robot.Constants;
+import java.util.Arrays;
+import java.util.HashMap;
+import java.util.Map;
+import java.util.function.Consumer;
+import java.util.function.DoubleSupplier;
+import org.littletonrobotics.junction.networktables.LoggedNetworkNumber;
+
+/**
+ * Class for a tunable number. Gets value from dashboard in tuning mode, returns default if not or
+ * value not in dashboard.
+ */
+public class LoggedTunableNumber implements DoubleSupplier {
+  private static final String tableKey = "/Tuning";
+
+  private final String key;
+  private boolean hasDefault = false;
+  private double defaultValue;
+  private LoggedNetworkNumber dashboardNumber;
+  private Map<Integer, Double> lastHasChangedValues = new HashMap<>();
+
+  /**
+   * Create a new LoggedTunableNumber
+   *
+   * @param dashboardKey Key on dashboard
+   */
+  public LoggedTunableNumber(String dashboardKey) {
+    this.key = tableKey + "/" + dashboardKey;
+  }
+
+  /**
+   * Create a new LoggedTunableNumber with the default value
+   *
+   * @param dashboardKey Key on dashboard
+   * @param defaultValue Default value
+   */
+  public LoggedTunableNumber(String dashboardKey, double defaultValue) {
+    this(dashboardKey);
+    initDefault(defaultValue);
+  }
+
+  /**
+   * Set the default value of the number. The default value can only be set once.
+   *
+   * @param defaultValue The default value
+   */
+  public void initDefault(double defaultValue) {
+    if (!hasDefault) {
+      hasDefault = true;
+      this.defaultValue = defaultValue;
+      if (Constants.tuningMode) {
+        dashboardNumber = new LoggedNetworkNumber(key, defaultValue);
+      }
+    }
+  }
+
+  /**
+   * Get the current value, from dashboard if available and in tuning mode.
+   *
+   * @return The current value
+   */
+  public double get() {
+    if (!hasDefault) {
+      return 0.0;
+    } else {
+      return Constants.tuningMode ? dashboardNumber.get() : defaultValue;
+    }
+  }
+
+  /**
+   * Checks whether the number has changed since our last check
+   *
+   * @param id Unique identifier for the caller to avoid conflicts when shared between multiple
+   *     objects. Recommended approach is to pass the result of "hashCode()"
+   * @return True if the number has changed since the last time this method was called, false
+   *     otherwise.
+   */
+  public boolean hasChanged(int id) {
+    double currentValue = get();
+    Double lastValue = lastHasChangedValues.get(id);
+    if (lastValue == null || currentValue != lastValue) {
+      lastHasChangedValues.put(id, currentValue);
+      return true;
+    }
+
+    return false;
+  }
+
+  /**
+   * Runs action if any of the tunableNumbers have changed
+   *
+   * @param id Unique identifier for the caller to avoid conflicts when shared between multiple *
+   *     objects. Recommended approach is to pass the result of "hashCode()"
+   * @param action Callback to run when any of the tunable numbers have changed. Access tunable
+   *     numbers in order inputted in method
+   * @param tunableNumbers All tunable numbers to check
+   */
+  public static void ifChanged(
+      int id, Consumer<double[]> action, LoggedTunableNumber... tunableNumbers) {
+    if (Arrays.stream(tunableNumbers).anyMatch(tunableNumber -> tunableNumber.hasChanged(id))) {
+      action.accept(Arrays.stream(tunableNumbers).mapToDouble(LoggedTunableNumber::get).toArray());
+    }
+  }
+
+  /** Runs action if any of the tunableNumbers have changed */
+  public static void ifChanged(int id, Runnable action, LoggedTunableNumber... tunableNumbers) {
+    ifChanged(id, values -> action.run(), tunableNumbers);
+  }
+
+  @Override
+  public double getAsDouble() {
+    return get();
+  }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/generated/TunerConstants.java b/src/main/java/frc/robot/generated/TunerConstants.java
new file mode 100644
index 0000000..9e77c31
--- /dev/null
+++ b/src/main/java/frc/robot/generated/TunerConstants.java
@@ -0,0 +1,286 @@
+package frc.robot.generated;
+
+import static edu.wpi.first.units.Units.*;
+
+import com.ctre.phoenix6.CANBus;
+import com.ctre.phoenix6.configs.*;
+import com.ctre.phoenix6.hardware.*;
+import com.ctre.phoenix6.signals.*;
+import com.ctre.phoenix6.swerve.*;
+import com.ctre.phoenix6.swerve.SwerveModuleConstants.*;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.units.measure.*;
+
+import frc.robot.subsystems.CommandSwerveDrivetrain;
+
+// Generated by the Tuner X Swerve Project Generator
+// https://v6.docs.ctr-electronics.com/en/stable/docs/tuner/tuner-swerve/index.html
+public class TunerConstants {
+    // Both sets of gains need to be tuned to your individual robot.
+
+    // The steer motor uses any SwerveModule.SteerRequestType control request with the
+    // output type specified by SwerveModuleConstants.SteerMotorClosedLoopOutput
+    private static final Slot0Configs steerGains = new Slot0Configs()
+        .withKP(100).withKI(0).withKD(0.5)
+        .withKS(0.1).withKV(1.66).withKA(0)
+        .withStaticFeedforwardSign(StaticFeedforwardSignValue.UseClosedLoopSign);
+    // When using closed-loop control, the drive motor uses the control
+    // output type specified by SwerveModuleConstants.DriveMotorClosedLoopOutput
+    private static final Slot0Configs driveGains = new Slot0Configs()
+        .withKP(0.1).withKI(0).withKD(0)
+        .withKS(0).withKV(0.124);
+
+    // The closed-loop output type to use for the steer motors;
+    // This affects the PID/FF gains for the steer motors
+    private static final ClosedLoopOutputType kSteerClosedLoopOutput = ClosedLoopOutputType.Voltage;
+    // The closed-loop output type to use for the drive motors;
+    // This affects the PID/FF gains for the drive motors
+    private static final ClosedLoopOutputType kDriveClosedLoopOutput = ClosedLoopOutputType.Voltage;
+
+    // The type of motor used for the drive motor
+    private static final DriveMotorArrangement kDriveMotorType = DriveMotorArrangement.TalonFX_Integrated;
+    // The type of motor used for the drive motor
+    private static final SteerMotorArrangement kSteerMotorType = SteerMotorArrangement.TalonFX_Integrated;
+
+    // The remote sensor feedback type to use for the steer motors;
+    // 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);
+
+    // 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
+                // stator current limit to help avoid brownouts without impacting performance.
+                .withStatorCurrentLimit(Amps.of(60))
+                .withStatorCurrentLimitEnable(true)
+        );
+    private static final CANcoderConfiguration encoderInitialConfigs = new CANcoderConfiguration();
+    // Configs for the Pigeon 2; leave this null to skip applying Pigeon 2 configs
+    private static final Pigeon2Configuration pigeonConfigs = null;
+
+    // CAN bus that the devices are located on;
+    // All swerve devices must share the same CAN bus
+    public static final CANBus kCANBus = new CANBus("", "./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.75);
+
+    // 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 = 4.2;
+
+    private static final double kDriveGearRatio = 6.720000000000001;
+    private static final double kSteerGearRatio = 13.371428571428572;
+    private static final Distance kWheelRadius = Inches.of(2);
+
+    private static final boolean kInvertLeftSide = false;
+    private static final boolean kInvertRightSide = true;
+
+    private static final int kPigeonId = 20;
+
+    // These are only used for simulation
+    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);
+
+    public static final SwerveDrivetrainConstants DrivetrainConstants = new SwerveDrivetrainConstants()
+            .withCANBusName(kCANBus.getName())
+            .withPigeon2Id(kPigeonId)
+            .withPigeon2Configs(pigeonConfigs);
+
+    private static final SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> ConstantCreator =
+        new SwerveModuleConstantsFactory<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration>()
+            .withDriveMotorGearRatio(kDriveGearRatio)
+            .withSteerMotorGearRatio(kSteerGearRatio)
+            .withCouplingGearRatio(kCoupleRatio)
+            .withWheelRadius(kWheelRadius)
+            .withSteerMotorGains(steerGains)
+            .withDriveMotorGains(driveGains)
+            .withSteerMotorClosedLoopOutput(kSteerClosedLoopOutput)
+            .withDriveMotorClosedLoopOutput(kDriveClosedLoopOutput)
+            .withSlipCurrent(kSlipCurrent)
+            .withSpeedAt12Volts(kSpeedAt12Volts)
+            .withDriveMotorType(kDriveMotorType)
+            .withSteerMotorType(kSteerMotorType)
+            .withFeedbackSource(kSteerFeedbackType)
+            .withDriveMotorInitialConfigs(driveInitialConfigs)
+            .withSteerMotorInitialConfigs(steerInitialConfigs)
+            .withEncoderInitialConfigs(encoderInitialConfigs)
+            .withSteerInertia(kSteerInertia)
+            .withDriveInertia(kDriveInertia)
+            .withSteerFrictionVoltage(kSteerFrictionVoltage)
+            .withDriveFrictionVoltage(kDriveFrictionVoltage);
+
+
+    // Front Left
+    private static final int kFrontLeftDriveMotorId = 1;
+    private static final int kFrontLeftSteerMotorId = 2;
+    private static final int kFrontLeftEncoderId = 22;
+    private static final Angle kFrontLeftEncoderOffset = Rotations.of(0.164794921875);
+    private static final boolean kFrontLeftSteerMotorInverted = true;
+    private static final boolean kFrontLeftEncoderInverted = false;
+
+    private static final Distance kFrontLeftXPos = Inches.of(10.625);
+    private static final Distance kFrontLeftYPos = Inches.of(10.625);
+
+    // Front Right
+    private static final int kFrontRightDriveMotorId = 3;
+    private static final int kFrontRightSteerMotorId = 4;
+    private static final int kFrontRightEncoderId = 23;
+    private static final Angle kFrontRightEncoderOffset = Rotations.of(-0.0107421875);
+    private static final boolean kFrontRightSteerMotorInverted = true;
+    private static final boolean kFrontRightEncoderInverted = false;
+
+    private static final Distance kFrontRightXPos = Inches.of(10.625);
+    private static final Distance kFrontRightYPos = Inches.of(-10.625);
+
+    // Back Left
+    private static final int kBackLeftDriveMotorId = 8;
+    private static final int kBackLeftSteerMotorId = 7;
+    private static final int kBackLeftEncoderId = 21;
+    private static final Angle kBackLeftEncoderOffset = Rotations.of(-0.458984375);
+    private static final boolean kBackLeftSteerMotorInverted = true;
+    private static final boolean kBackLeftEncoderInverted = false;
+
+    private static final Distance kBackLeftXPos = Inches.of(-10.625);
+    private static final Distance kBackLeftYPos = Inches.of(10.625);
+
+    // Back Right
+    private static final int kBackRightDriveMotorId = 6;
+    private static final int kBackRightSteerMotorId = 5;
+    private static final int kBackRightEncoderId = 24;
+    private static final Angle kBackRightEncoderOffset = Rotations.of(0.07958984375);
+    private static final boolean kBackRightSteerMotorInverted = true;
+    private static final boolean kBackRightEncoderInverted = false;
+
+    private static final Distance kBackRightXPos = Inches.of(-10.625);
+    private static final Distance kBackRightYPos = Inches.of(-10.625);
+
+
+    public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FrontLeft =
+        ConstantCreator.createModuleConstants(
+            kFrontLeftSteerMotorId, kFrontLeftDriveMotorId, kFrontLeftEncoderId, kFrontLeftEncoderOffset,
+            kFrontLeftXPos, kFrontLeftYPos, kInvertLeftSide, kFrontLeftSteerMotorInverted, kFrontLeftEncoderInverted
+        );
+    public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> FrontRight =
+        ConstantCreator.createModuleConstants(
+            kFrontRightSteerMotorId, kFrontRightDriveMotorId, kFrontRightEncoderId, kFrontRightEncoderOffset,
+            kFrontRightXPos, kFrontRightYPos, kInvertRightSide, kFrontRightSteerMotorInverted, kFrontRightEncoderInverted
+        );
+    public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BackLeft =
+        ConstantCreator.createModuleConstants(
+            kBackLeftSteerMotorId, kBackLeftDriveMotorId, kBackLeftEncoderId, kBackLeftEncoderOffset,
+            kBackLeftXPos, kBackLeftYPos, kInvertLeftSide, kBackLeftSteerMotorInverted, kBackLeftEncoderInverted
+        );
+    public static final SwerveModuleConstants<TalonFXConfiguration, TalonFXConfiguration, CANcoderConfiguration> BackRight =
+        ConstantCreator.createModuleConstants(
+            kBackRightSteerMotorId, kBackRightDriveMotorId, kBackRightEncoderId, kBackRightEncoderOffset,
+            kBackRightXPos, kBackRightYPos, kInvertRightSide, 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> {
+        /**
+         * Constructs a CTRE SwerveDrivetrain using the specified constants.
+         * <p>
+         * This constructs the underlying hardware devices, so users should not construct
+         * the devices themselves. If they need the devices, they can access them through
+         * getters in the classes.
+         *
+         * @param drivetrainConstants   Drivetrain-wide constants for the swerve drive
+         * @param modules               Constants for each specific module
+         */
+        public TunerSwerveDrivetrain(
+            SwerveDrivetrainConstants drivetrainConstants,
+            SwerveModuleConstants<?, ?, ?>... modules
+        ) {
+            super(
+                TalonFX::new, TalonFX::new, CANcoder::new,
+                drivetrainConstants, modules
+            );
+        }
+
+        /**
+         * Constructs a CTRE SwerveDrivetrain using the specified constants.
+         * <p>
+         * This constructs the underlying hardware devices, so users should not construct
+         * the devices themselves. If they need the devices, they can access them through
+         * getters in the classes.
+         *
+         * @param drivetrainConstants     Drivetrain-wide constants for the swerve drive
+         * @param odometryUpdateFrequency The frequency to run the odometry loop. If
+         *                                unspecified or set to 0 Hz, this is 250 Hz on
+         *                                CAN FD, and 100 Hz on CAN 2.0.
+         * @param modules                 Constants for each specific module
+         */
+        public TunerSwerveDrivetrain(
+            SwerveDrivetrainConstants drivetrainConstants,
+            double odometryUpdateFrequency,
+            SwerveModuleConstants<?, ?, ?>... modules
+        ) {
+            super(
+                TalonFX::new, TalonFX::new, CANcoder::new,
+                drivetrainConstants, odometryUpdateFrequency, modules
+            );
+        }
+
+        /**
+         * Constructs a CTRE SwerveDrivetrain using the specified constants.
+         * <p>
+         * This constructs the underlying hardware devices, so users should not construct
+         * the devices themselves. If they need the devices, they can access them through
+         * getters in the classes.
+         *
+         * @param drivetrainConstants       Drivetrain-wide constants for the swerve drive
+         * @param odometryUpdateFrequency   The frequency to run the odometry loop. If
+         *                                  unspecified or set to 0 Hz, this is 250 Hz on
+         *                                  CAN FD, and 100 Hz on CAN 2.0.
+         * @param odometryStandardDeviation The standard deviation for odometry calculation
+         *                                  in the form [x, y, theta]ᵀ, with units in meters
+         *                                  and radians
+         * @param visionStandardDeviation   The standard deviation for vision calculation
+         *                                  in the form [x, y, theta]ᵀ, with units in meters
+         *                                  and radians
+         * @param modules                   Constants for each specific module
+         */
+        public TunerSwerveDrivetrain(
+            SwerveDrivetrainConstants drivetrainConstants,
+            double odometryUpdateFrequency,
+            Matrix<N3, N1> odometryStandardDeviation,
+            Matrix<N3, N1> visionStandardDeviation,
+            SwerveModuleConstants<?, ?, ?>... modules
+        ) {
+            super(
+                TalonFX::new, TalonFX::new, CANcoder::new,
+                drivetrainConstants, odometryUpdateFrequency,
+                odometryStandardDeviation, visionStandardDeviation, modules
+            );
+        }
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
new file mode 100644
index 0000000..d127487
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/CommandSwerveDrivetrain.java
@@ -0,0 +1,288 @@
+package frc.robot.subsystems;
+
+import static edu.wpi.first.units.Units.*;
+
+import java.util.function.Supplier;
+
+import com.ctre.phoenix6.SignalLogger;
+import com.ctre.phoenix6.Utils;
+import com.ctre.phoenix6.swerve.SwerveDrivetrainConstants;
+import com.ctre.phoenix6.swerve.SwerveModuleConstants;
+import com.ctre.phoenix6.swerve.SwerveRequest;
+
+import edu.wpi.first.math.Matrix;
+import edu.wpi.first.math.geometry.Pose2d;
+import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.numbers.N1;
+import edu.wpi.first.math.numbers.N3;
+import edu.wpi.first.wpilibj.DriverStation;
+import edu.wpi.first.wpilibj.DriverStation.Alliance;
+import edu.wpi.first.wpilibj.Notifier;
+import edu.wpi.first.wpilibj.RobotController;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Subsystem;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+
+import frc.robot.generated.TunerConstants.TunerSwerveDrivetrain;
+
+/**
+ * Class that extends the Phoenix 6 SwerveDrivetrain class and implements
+ * Subsystem so it can easily be used in command-based projects.
+ */
+public class CommandSwerveDrivetrain extends TunerSwerveDrivetrain implements Subsystem {
+    private static final double kSimLoopPeriod = 0.005; // 5 ms
+    private Notifier m_simNotifier = null;
+    private double m_lastSimTime;
+
+    /* Blue alliance sees forward as 0 degrees (toward red alliance wall) */
+    private static final Rotation2d kBlueAlliancePerspectiveRotation = Rotation2d.kZero;
+    /* Red alliance sees forward as 180 degrees (toward blue alliance wall) */
+    private static final Rotation2d kRedAlliancePerspectiveRotation = Rotation2d.k180deg;
+    /* Keep track if we've ever applied the operator perspective before or not */
+    private boolean m_hasAppliedOperatorPerspective = false;
+
+    /* Swerve requests to apply during SysId characterization */
+    private final SwerveRequest.SysIdSwerveTranslation m_translationCharacterization = new SwerveRequest.SysIdSwerveTranslation();
+    private final SwerveRequest.SysIdSwerveSteerGains m_steerCharacterization = new SwerveRequest.SysIdSwerveSteerGains();
+    private final SwerveRequest.SysIdSwerveRotation m_rotationCharacterization = new SwerveRequest.SysIdSwerveRotation();
+
+    /* SysId routine for characterizing translation. This is used to find PID gains for the drive motors. */
+    private final SysIdRoutine m_sysIdRoutineTranslation = new SysIdRoutine(
+        new SysIdRoutine.Config(
+            null,        // Use default ramp rate (1 V/s)
+            Volts.of(4), // Reduce dynamic step voltage to 4 V to prevent brownout
+            null,        // Use default timeout (10 s)
+            // Log state with SignalLogger class
+            state -> SignalLogger.writeString("SysIdTranslation_State", state.toString())
+        ),
+        new SysIdRoutine.Mechanism(
+            output -> setControl(m_translationCharacterization.withVolts(output)),
+            null,
+            this
+        )
+    );
+
+    /* SysId routine for characterizing steer. This is used to find PID gains for the steer motors. */
+    private final SysIdRoutine m_sysIdRoutineSteer = new SysIdRoutine(
+        new SysIdRoutine.Config(
+            null,        // Use default ramp rate (1 V/s)
+            Volts.of(7), // Use dynamic voltage of 7 V
+            null,        // Use default timeout (10 s)
+            // Log state with SignalLogger class
+            state -> SignalLogger.writeString("SysIdSteer_State", state.toString())
+        ),
+        new SysIdRoutine.Mechanism(
+            volts -> setControl(m_steerCharacterization.withVolts(volts)),
+            null,
+            this
+        )
+    );
+
+    /*
+     * SysId routine for characterizing rotation.
+     * This is used to find PID gains for the FieldCentricFacingAngle HeadingController.
+     * See the documentation of SwerveRequest.SysIdSwerveRotation for info on importing the log to SysId.
+     */
+    private final SysIdRoutine m_sysIdRoutineRotation = new SysIdRoutine(
+        new SysIdRoutine.Config(
+            /* This is in radians per second², but SysId only supports "volts per second" */
+            Volts.of(Math.PI / 6).per(Second),
+            /* This is in radians per second, but SysId only supports "volts" */
+            Volts.of(Math.PI),
+            null, // Use default timeout (10 s)
+            // Log state with SignalLogger class
+            state -> SignalLogger.writeString("SysIdRotation_State", state.toString())
+        ),
+        new SysIdRoutine.Mechanism(
+            output -> {
+                /* output is actually radians per second, but SysId only supports "volts" */
+                setControl(m_rotationCharacterization.withRotationalRate(output.in(Volts)));
+                /* also log the requested output for SysId */
+                SignalLogger.writeDouble("Rotational_Rate", output.in(Volts));
+            },
+            null,
+            this
+        )
+    );
+
+    /* The SysId routine to test */
+    private SysIdRoutine m_sysIdRoutineToApply = m_sysIdRoutineTranslation;
+
+    /**
+     * Constructs a CTRE SwerveDrivetrain using the specified constants.
+     * <p>
+     * This constructs the underlying hardware devices, so users should not construct
+     * the devices themselves. If they need the devices, they can access them through
+     * getters in the classes.
+     *
+     * @param drivetrainConstants   Drivetrain-wide constants for the swerve drive
+     * @param modules               Constants for each specific module
+     */
+    public CommandSwerveDrivetrain(
+        SwerveDrivetrainConstants drivetrainConstants,
+        SwerveModuleConstants<?, ?, ?>... modules
+    ) {
+        super(drivetrainConstants, modules);
+        if (Utils.isSimulation()) {
+            startSimThread();
+        }
+    }
+
+    /**
+     * Constructs a CTRE SwerveDrivetrain using the specified constants.
+     * <p>
+     * This constructs the underlying hardware devices, so users should not construct
+     * the devices themselves. If they need the devices, they can access them through
+     * getters in the classes.
+     *
+     * @param drivetrainConstants     Drivetrain-wide constants for the swerve drive
+     * @param odometryUpdateFrequency The frequency to run the odometry loop. If
+     *                                unspecified or set to 0 Hz, this is 250 Hz on
+     *                                CAN FD, and 100 Hz on CAN 2.0.
+     * @param modules                 Constants for each specific module
+     */
+    public CommandSwerveDrivetrain(
+        SwerveDrivetrainConstants drivetrainConstants,
+        double odometryUpdateFrequency,
+        SwerveModuleConstants<?, ?, ?>... modules
+    ) {
+        super(drivetrainConstants, odometryUpdateFrequency, modules);
+        if (Utils.isSimulation()) {
+            startSimThread();
+        }
+    }
+
+    /**
+     * Constructs a CTRE SwerveDrivetrain using the specified constants.
+     * <p>
+     * This constructs the underlying hardware devices, so users should not construct
+     * the devices themselves. If they need the devices, they can access them through
+     * getters in the classes.
+     *
+     * @param drivetrainConstants       Drivetrain-wide constants for the swerve drive
+     * @param odometryUpdateFrequency   The frequency to run the odometry loop. If
+     *                                  unspecified or set to 0 Hz, this is 250 Hz on
+     *                                  CAN FD, and 100 Hz on CAN 2.0.
+     * @param odometryStandardDeviation The standard deviation for odometry calculation
+     *                                  in the form [x, y, theta]ᵀ, with units in meters
+     *                                  and radians
+     * @param visionStandardDeviation   The standard deviation for vision calculation
+     *                                  in the form [x, y, theta]ᵀ, with units in meters
+     *                                  and radians
+     * @param modules                   Constants for each specific module
+     */
+    public CommandSwerveDrivetrain(
+        SwerveDrivetrainConstants drivetrainConstants,
+        double odometryUpdateFrequency,
+        Matrix<N3, N1> odometryStandardDeviation,
+        Matrix<N3, N1> visionStandardDeviation,
+        SwerveModuleConstants<?, ?, ?>... modules
+    ) {
+        super(drivetrainConstants, odometryUpdateFrequency, odometryStandardDeviation, visionStandardDeviation, modules);
+        if (Utils.isSimulation()) {
+            startSimThread();
+        }
+    }
+
+    /**
+     * Returns a command that applies the specified control request to this swerve drivetrain.
+     *
+     * @param request Function returning the request to apply
+     * @return Command to run
+     */
+    public Command applyRequest(Supplier<SwerveRequest> requestSupplier) {
+        return run(() -> this.setControl(requestSupplier.get()));
+    }
+
+    /**
+     * Runs the SysId Quasistatic test in the given direction for the routine
+     * specified by {@link #m_sysIdRoutineToApply}.
+     *
+     * @param direction Direction of the SysId Quasistatic test
+     * @return Command to run
+     */
+    public Command sysIdQuasistatic(SysIdRoutine.Direction direction) {
+        return m_sysIdRoutineToApply.quasistatic(direction);
+    }
+
+    /**
+     * Runs the SysId Dynamic test in the given direction for the routine
+     * specified by {@link #m_sysIdRoutineToApply}.
+     *
+     * @param direction Direction of the SysId Dynamic test
+     * @return Command to run
+     */
+    public Command sysIdDynamic(SysIdRoutine.Direction direction) {
+        return m_sysIdRoutineToApply.dynamic(direction);
+    }
+
+    @Override
+    public void periodic() {
+        /*
+         * Periodically try to apply the operator perspective.
+         * If we haven't applied the operator perspective before, then we should apply it regardless of DS state.
+         * This allows us to correct the perspective in case the robot code restarts mid-match.
+         * Otherwise, only check and apply the operator perspective if the DS is disabled.
+         * This ensures driving behavior doesn't change until an explicit disable event occurs during testing.
+         */
+        if (!m_hasAppliedOperatorPerspective || DriverStation.isDisabled()) {
+            DriverStation.getAlliance().ifPresent(allianceColor -> {
+                setOperatorPerspectiveForward(
+                    allianceColor == Alliance.Red
+                        ? kRedAlliancePerspectiveRotation
+                        : kBlueAlliancePerspectiveRotation
+                );
+                m_hasAppliedOperatorPerspective = true;
+            });
+        }
+    }
+
+    private void startSimThread() {
+        m_lastSimTime = Utils.getCurrentTimeSeconds();
+
+        /* Run simulation at a faster rate so PID gains behave more reasonably */
+        m_simNotifier = new Notifier(() -> {
+            final double currentTime = Utils.getCurrentTimeSeconds();
+            double deltaTime = currentTime - m_lastSimTime;
+            m_lastSimTime = currentTime;
+
+            /* use the measured time delta, get battery voltage from WPILib */
+            updateSimState(deltaTime, RobotController.getBatteryVoltage());
+        });
+        m_simNotifier.startPeriodic(kSimLoopPeriod);
+    }
+
+    /**
+     * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
+     * while still accounting for measurement noise.
+     *
+     * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
+     * @param timestampSeconds The timestamp of the vision measurement in seconds.
+     */
+    @Override
+    public void addVisionMeasurement(Pose2d visionRobotPoseMeters, double timestampSeconds) {
+        super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds));
+    }
+
+    /**
+     * Adds a vision measurement to the Kalman Filter. This will correct the odometry pose estimate
+     * while still accounting for measurement noise.
+     * <p>
+     * Note that the vision measurement standard deviations passed into this method
+     * will continue to apply to future measurements until a subsequent call to
+     * {@link #setVisionMeasurementStdDevs(Matrix)} or this method.
+     *
+     * @param visionRobotPoseMeters The pose of the robot as measured by the vision camera.
+     * @param timestampSeconds The timestamp of the vision measurement in seconds.
+     * @param visionMeasurementStdDevs Standard deviations of the vision pose measurement
+     *     in the form [x, y, theta]ᵀ, with units in meters and radians.
+     */
+    @Override
+    public void addVisionMeasurement(
+        Pose2d visionRobotPoseMeters,
+        double timestampSeconds,
+        Matrix<N3, N1> visionMeasurementStdDevs
+    ) {
+        super.addVisionMeasurement(visionRobotPoseMeters, Utils.fpgaToCurrentTime(timestampSeconds), visionMeasurementStdDevs);
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/arm/Arm.java b/src/main/java/frc/robot/subsystems/arm/Arm.java
new file mode 100644
index 0000000..aad06be
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/arm/Arm.java
@@ -0,0 +1,122 @@
+package frc.robot.subsystems.arm;
+
+import static edu.wpi.first.units.Units.Radians;
+import static edu.wpi.first.units.Units.RadiansPerSecond;
+import static edu.wpi.first.units.Units.Volts;
+
+import java.util.regex.MatchResult;
+
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.geometry.Pose3d;
+import edu.wpi.first.math.geometry.Rotation3d;
+import edu.wpi.first.math.geometry.Translation3d;
+import edu.wpi.first.units.measure.Angle;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Voltage;
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.wpilibj2.command.sysid.SysIdRoutine;
+import frc.robot.Constants;
+import frc.robot.Constants.Gamepiece;
+import frc.robot.Constants.Mode;
+import frc.robot.Constants.OIConstants;
+
+public class Arm extends SubsystemBase {
+    private ArmIOInputsAutoLogged inputs = new ArmIOInputsAutoLogged();
+    private ArmIO io;
+    private static Arm instance;
+
+    private SysIdRoutine sysIdRoutine = new SysIdRoutine(
+        new SysIdRoutine.Config(),
+        new SysIdRoutine.Mechanism(volts -> {
+            manualVoltage(volts.magnitude());
+        }, 
+        log -> {
+            log.motor("arm motor")
+                .voltage(Voltage.ofBaseUnits(inputs.voltage, Volts))
+                .angularPosition(Angle.ofBaseUnits(inputs.relativeEncoderPosition, Radians))
+                .angularVelocity(AngularVelocity.ofBaseUnits(inputs.relativeEncoderVelocity, RadiansPerSecond));
+        }, this)
+    );
+
+    private Arm(ArmIO io) {
+        this.io = io;
+        setDefaultCommand(
+            Commands.run(() -> {
+                double up = MathUtil.applyDeadband(OIConstants.driverController.getLeftTriggerAxis(), 0.1);
+                double down = MathUtil.applyDeadband(OIConstants.driverController.getRightTriggerAxis(), 0.1);
+                double change = (up - down) * 0.1;
+                // io.setGoal(inputs.goalAngle + change);
+            }, this)
+        );
+        setFFMode(Constants.currentMode == Mode.SIM ? Gamepiece.SIM: Gamepiece.NONE);
+    }
+
+    public double getAngle() {
+        return inputs.angularPosition;
+    }
+
+    public double getGoalAngle() {
+        return inputs.goalAngle;
+    }
+
+    public static void initialize(ArmIO io) {
+        instance = new Arm(io);
+    }
+
+    public static Arm getInstance() {
+        return instance;
+    }
+
+    public void setGoal(double angle) {
+        io.setGoal(angle);
+    }
+
+    public void resetAbsoluteEncoder() {
+        io.resetEncoder();
+    }
+
+    public SysIdRoutine sysIDRoutine() {
+        return sysIdRoutine;
+    }
+
+    public void manualVoltage(double voltage) {
+        io.setVoltage(voltage);
+    }
+
+    public Pose3d getArmPose(){
+        return new Pose3d(
+            new Translation3d(10 + -(0.45) * Math.cos(inputs.angularPosition), 1, 1 + (0.45) * Math.sin(inputs.angularPosition)),
+            new Rotation3d(0, inputs.angularPosition, 0)
+        );
+    }
+
+    public void setFFMode(Gamepiece gamepieceType) {
+        Logger.recordOutput(getName() + "/FFMode", gamepieceType);
+        switch (gamepieceType) {
+            case CORAL:
+                io.setFFValues(ArmConstants.CORALkS, ArmConstants.CORALkG, ArmConstants.CORALkV, ArmConstants.CORALkA);
+                break;
+            case ALGAE:
+                io.setFFValues(ArmConstants.ALGAEkS, ArmConstants.ALGAEkG, ArmConstants.ALGAEkV, ArmConstants.ALGAEkA);
+                break;
+            case SIM:
+                io.setFFValues(ArmConstants.SIMkS, ArmConstants.SIMkG, ArmConstants.SIMkV, ArmConstants.SIMkA);
+                break;
+            case NONE:
+                io.setFFValues(ArmConstants.DEFAULTkS, ArmConstants.DEFAULTkG, ArmConstants.DEFAULTkV, ArmConstants.DEFAULTkA);
+                break;
+        }
+    }
+
+    @Override
+    public void periodic() {
+        io.updateMotionProfile();
+        Logger.recordOutput(getName() + "/Pose", getArmPose());
+        io.updateInputs(inputs);
+        Logger.processInputs(getName(), inputs);
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/arm/ArmConstants.java b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java
new file mode 100644
index 0000000..5de237d
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/arm/ArmConstants.java
@@ -0,0 +1,51 @@
+package frc.robot.subsystems.arm;
+
+import edu.wpi.first.math.util.Units;
+
+public class ArmConstants {
+    public static final int CAN_ID = 14;
+    public static final int CURRENT_LIMIT = 50;
+    public static final double ARM_OFFSET = 1.8;
+
+    public static final double CORAL_L1 = Units.degreesToRadians(-34.74);
+    public static final double CORAL_L2 = Units.degreesToRadians(-54.1); // with elevator up
+    public static final double CORAL_L3 = Units.degreesToRadians(54.1);
+    public static final double CORAL_L4 = Units.degreesToRadians(54.61);
+
+    public static final double MAX_VELOCITY = 2;
+    public static final double MAX_ACCELERATION = 4;
+
+    // Simulation FF
+    public static final double SIMkG = 1.3;
+    public static final double SIMkV = 1.00;
+    public static final double SIMkA = 0.05;
+    public static final double SIMkS = 0.001;
+
+    // Default FF
+    public static final double DEFAULTkG = 1.02;
+    public static final double DEFAULTkV = 1.01;
+    public static final double DEFAULTkA = 0.07;
+    public static final double DEFAULTkS = 0.3;
+
+    // Algae FF
+    public static final double ALGAEkG = 1.02;
+    public static final double ALGAEkV = 1.01;
+    public static final double ALGAEkA = 0.05;
+    public static final double ALGAEkS = 0.1;
+
+    // Default FF
+    public static final double CORALkG = 1.09;
+    public static final double CORALkV = 1.01;
+    public static final double CORALkA = 0.05;
+    public static final double CORALkS = 0.1;
+
+    public static class Sim {
+        public static double GEARING = 60;
+        public static double MOI = 2.09670337984;
+        public static double LENGTH = 0.6604;
+        public static double MIN_ANGLE = Units.degreesToRadians(-95);
+        public static double MAX_ANGLE = Units.degreesToRadians(55);
+        public static boolean GRAVITY = true;
+        public static double INIT_ANGLE = -Math.PI / 2;
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIO.java b/src/main/java/frc/robot/subsystems/arm/ArmIO.java
new file mode 100644
index 0000000..5a8f209
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/arm/ArmIO.java
@@ -0,0 +1,47 @@
+package frc.robot.subsystems.arm;
+
+import org.littletonrobotics.junction.AutoLog;
+public interface ArmIO {
+    @AutoLog
+    public static class ArmIOInputs{
+        public double voltage = 0.0;
+        public double angularPosition = 0.0;
+        public double angularVelocity = 0.0;
+        public double current = 0.0;
+        public double goalAngle = 0.0;
+        public double setpointVelocity = 0.0;
+        public double appliedOutput = 0.0; 
+        public double busVoltage = 0.0;
+        public double relativeEncoderPosition = 0.0;
+        public double relativeEncoderVelocity = 0.0;
+        public double setpointPosition = 0.0;
+    }
+
+    public default void updateInputs(ArmIOInputs inputs) {
+        
+    }
+
+    public default void resetEncoder() {
+    }
+
+    public default void setVoltage(double voltage) {
+
+    }
+
+    public default double getGoal() {
+        return 0.0;
+    }
+
+    public default void updateMotionProfile(){
+        
+    }
+    
+    public default void setGoal(double angle) {
+    }
+
+    public default void hold() {}
+
+    public default void setFFValues(double kS, double kG, double kV, double kA) {
+
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java
new file mode 100644
index 0000000..c5d6703
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/arm/ArmIOReal.java
@@ -0,0 +1,132 @@
+package frc.robot.subsystems.arm;
+
+import org.littletonrobotics.junction.Logger;
+
+import com.revrobotics.spark.ClosedLoopSlot;
+import com.revrobotics.spark.SparkAbsoluteEncoder;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkFlex;
+import com.revrobotics.spark.SparkBase.ControlType;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.config.SparkFlexConfig;
+import com.revrobotics.spark.config.ClosedLoopConfig.FeedbackSensor;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+
+public class ArmIOReal implements ArmIO {
+    private final SparkFlex armMotor = new SparkFlex(ArmConstants.CAN_ID, MotorType.kBrushless);
+    private SparkFlexConfig config = new SparkFlexConfig();
+    private SparkAbsoluteEncoder armEncoder;
+
+    private ArmFeedforward ffmodel = new ArmFeedforward(ArmConstants.DEFAULTkS, ArmConstants.DEFAULTkG, ArmConstants.DEFAULTkV, ArmConstants.DEFAULTkA, 0.02);
+    private final SparkClosedLoopController onboardController = armMotor.getClosedLoopController();
+    private final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(ArmConstants.MAX_VELOCITY, ArmConstants.MAX_ACCELERATION);
+    private final TrapezoidProfile profile = new TrapezoidProfile(constraints);
+    private TrapezoidProfile.State goal;
+    private TrapezoidProfile.State setpoint;
+
+    public ArmIOReal(){
+        config.smartCurrentLimit(ArmConstants.CURRENT_LIMIT);
+        config.idleMode(IdleMode.kBrake);
+
+        config.absoluteEncoder.positionConversionFactor(Math.PI);
+        config.absoluteEncoder.velocityConversionFactor(Math.PI / 60);
+        config.absoluteEncoder.inverted(false);
+
+        config.encoder.positionConversionFactor(2 * Math.PI / ArmConstants.Sim.GEARING);
+        config.encoder.velocityConversionFactor(2 * Math.PI / ArmConstants.Sim.GEARING / 60);
+
+        // config.softLimit.forwardSoftLimitEnabled(true);
+        // config.softLimit.reverseSoftLimitEnabled(false);
+        // config.softLimit.forwardSoftLimit(ArmConstants.Sim.MAX_ANGLE);
+        // config.softLimit.reverseSoftLimit(ArmConstants.Sim.MIN_ANGLE);
+
+        config.closedLoop.feedbackSensor(FeedbackSensor.kPrimaryEncoder);
+        config.closedLoop.positionWrappingEnabled(true);
+        config.closedLoop.positionWrappingInputRange(0, Math.PI);
+        config.closedLoop.p(5.0);
+        config.closedLoop.i(0.0);
+        config.closedLoop.d(0.0);
+
+        config.inverted(true);
+
+        armMotor.clearFaults();
+        armMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+
+        armEncoder = armMotor.getAbsoluteEncoder();
+        armMotor.getEncoder().setPosition(getOffsetAngle());
+
+        goal = new TrapezoidProfile.State(getOffsetAngle(), 0);
+        setpoint = new TrapezoidProfile.State(getOffsetAngle(), 0);
+    }
+
+    @Override
+    public void setGoal(double angle) {
+        goal = new TrapezoidProfile.State(angle,  0);
+    }
+
+    @Override
+    public double getGoal() {
+        return goal.position;
+    }
+    
+    @Override
+    public void updateMotionProfile() {
+        double prevVelocity = setpoint.velocity;
+        Logger.recordOutput("prev velocity", prevVelocity);
+        setpoint = profile.calculate(0.02, setpoint, goal);
+        double acceleration = (setpoint.velocity - prevVelocity) / 0.02;
+        Logger.recordOutput("acceleration", acceleration);
+        double ffvolts = ffmodel.calculate(armMotor.getEncoder().getPosition(), setpoint.velocity, acceleration);
+        Logger.recordOutput("arm ffvolts", ffvolts);
+        onboardController.setReference(setpoint.position, ControlType.kPosition, ClosedLoopSlot.kSlot0, ffvolts);
+
+    }
+
+    @Override
+    public void setFFValues(double kS, double kG, double kV, double kA) {
+        ffmodel = new ArmFeedforward(kS, kG, kV);
+    }
+
+
+    @Override
+    public void resetEncoder() {
+        armMotor.getEncoder().setPosition(0);
+    }
+
+    @Override
+    public void setVoltage(double voltage) {
+        onboardController.setReference(voltage, ControlType.kVoltage);
+    }
+
+    public double getOffsetAngle() {
+       double withoffset = armEncoder.getPosition() - ArmConstants.ARM_OFFSET;
+       if(withoffset > Math.PI) {
+        return -1 * (2 * Math.PI - withoffset);
+        }
+        else {
+            return withoffset;
+        }
+    }
+
+    @Override
+    public void updateInputs(ArmIOInputs inputs) {
+        inputs.angularPosition = getOffsetAngle();
+        // inputs.angularPosition = armEncoder.getPosition();
+        inputs.angularVelocity = armEncoder.getVelocity();
+        inputs.current = armMotor.getOutputCurrent();
+        inputs.voltage = armMotor.getAppliedOutput() * armMotor.getBusVoltage();
+        inputs.setpointVelocity = setpoint.velocity;
+        inputs.goalAngle = goal.position;
+        inputs.busVoltage = armMotor.getBusVoltage();
+        inputs.appliedOutput = armMotor.getAppliedOutput();
+        inputs.relativeEncoderPosition = armMotor.getEncoder().getPosition();
+        inputs.relativeEncoderVelocity = armMotor.getEncoder().getVelocity();
+        inputs.setpointPosition = setpoint.position;
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java
new file mode 100644
index 0000000..deed76b
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/arm/ArmIOSim.java
@@ -0,0 +1,73 @@
+package frc.robot.subsystems.arm;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.ArmFeedforward;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.simulation.SingleJointedArmSim;
+
+public class ArmIOSim implements ArmIO {
+    private final SingleJointedArmSim armSim = new SingleJointedArmSim(
+        DCMotor.getNeoVortex(2),
+        ArmConstants.Sim.GEARING,
+        ArmConstants.Sim.MOI,
+        ArmConstants.Sim.LENGTH,
+        ArmConstants.Sim.MIN_ANGLE,
+        ArmConstants.Sim.MAX_ANGLE,
+        ArmConstants.Sim.GRAVITY,
+        ArmConstants.Sim.INIT_ANGLE
+    );
+    private ArmFeedforward ffmodel = new ArmFeedforward(ArmConstants.SIMkS, ArmConstants.SIMkG, ArmConstants.SIMkV, ArmConstants.SIMkA);
+    private final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(3, 10);
+    private final PIDController controller = new PIDController(1, 0, .5);
+    private final TrapezoidProfile profile = new TrapezoidProfile(constraints);
+    private TrapezoidProfile.State goal = new TrapezoidProfile.State(ArmConstants.Sim.INIT_ANGLE, 0);
+    private TrapezoidProfile.State setpoint = new TrapezoidProfile.State(ArmConstants.Sim.INIT_ANGLE, 0);
+    private double voltage = 0;
+
+    @Override
+    public void setVoltage(double voltage) {
+        armSim.setInputVoltage(MathUtil.clamp(voltage, -12, 12));
+        this.voltage = voltage;
+    }
+
+    @Override
+    public void resetEncoder() {
+        armSim.setState(0, armSim.getVelocityRadPerSec());
+    }
+
+    @Override
+    public void setGoal(double angle) {
+        goal = new TrapezoidProfile.State(angle, 0);
+    }
+
+    @Override
+    public void hold() {
+        double ffvolts = ffmodel.calculate(armSim.getAngleRads(), 0);
+        double pidvolts = controller.calculate(armSim.getAngleRads(), goal.position);
+        setVoltage(ffvolts + pidvolts);
+    }
+
+    @Override
+    public void setFFValues(double kS, double kG, double kV, double kA) {
+        ffmodel = new ArmFeedforward(kS, kG, kV, kA);
+    }
+
+    @Override
+    public void updateMotionProfile() {
+        setpoint = profile.calculate(0.02, setpoint, goal);
+        setVoltage(ffmodel.calculate(armSim.getAngleRads(), setpoint.velocity));
+    }
+
+    @Override
+    public void updateInputs(ArmIOInputs inputs) {
+        inputs.angularPosition = armSim.getAngleRads();
+        inputs.angularVelocity = armSim.getVelocityRadPerSec();
+        inputs.current = armSim.getCurrentDrawAmps();
+        inputs.voltage = voltage;
+        inputs.goalAngle = goal.position;
+        inputs.setpointVelocity = setpoint.velocity;
+        armSim.update(0.02);
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/elevator/Elevator.java b/src/main/java/frc/robot/subsystems/elevator/Elevator.java
new file mode 100644
index 0000000..860fc77
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/elevator/Elevator.java
@@ -0,0 +1,68 @@
+package frc.robot.subsystems.elevator;
+
+import org.littletonrobotics.junction.Logger;
+
+import edu.wpi.first.wpilibj2.command.Commands;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.wpilibj2.command.Command;
+
+
+public class Elevator extends SubsystemBase {
+    private ElevatorIO io;
+    private ElevatorIOInputsAutoLogged inputs = new ElevatorIOInputsAutoLogged();
+
+    private static Elevator instance;
+    
+    public static Elevator getInstance(){
+        return instance;
+    }
+
+    public static Elevator initialize(ElevatorIO io){
+        if(instance == null){
+            instance = new Elevator(io);
+        }
+        return instance;
+    }
+
+    private Elevator(ElevatorIO elevatorIO){
+        io = elevatorIO;
+        io.updateInputs(inputs);
+
+        setDefaultCommand(Commands.run(() -> io.hold(), this));
+    }
+
+    public Command setGoal(double goal){
+        return Commands.runOnce(() -> io.setGoal(goal), this)
+        .andThen(Commands.run(() -> io.updateMotionProfile(), this))
+        .withTimeout(2.5);
+    }
+
+    public  void moveElevator(double speed) {
+        io.moveElevator(speed);
+    }
+
+    public double getPosition(){
+        return inputs.elevatorLeftPositionMeters; //change later
+    }
+
+    public double getVelocity() {
+        return inputs.elevatorLeftVelocityMetersPerSecond; //change later
+        
+    }
+
+    public void reset() {
+        io.resetEncoders();
+        this.setGoal(getPosition());
+    }
+
+    public void setVoltage(double volts) {
+        io.setVoltage(volts);
+    }
+
+    @Override
+    public void periodic() {
+        io.updateInputs(inputs);
+        Logger.processInputs(getName(), inputs);
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java
new file mode 100644
index 0000000..a706c83
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorConstants.java
@@ -0,0 +1,32 @@
+package frc.robot.subsystems.elevator;
+
+import edu.wpi.first.math.trajectory.TrapezoidProfile.Constraints;
+import edu.wpi.first.math.util.Units;
+
+public final class ElevatorConstants{
+    public static final double maxDistance = 0.7112;
+    public static final double minDistance = 0.0;
+    // public static final double constraints = 0.2; //figure this out later
+    public static final Constraints constraints = new Constraints(4, 5); //change later
+
+    public static final double kD = 0; //figure this out later
+    public static final double kI = 0; //figure this out later
+    public static final double kP = 10; //figure this out later
+    public static final int rightDeviceID = 12;
+    public static final int leftDeviceID = 13;
+    public static final double ks = 0.01;
+    public static final double kg = 0.35;
+    public static final double kv = 7; //IS THIS RIGHT?!?!?
+    public static final double MAX_ACCELERATION = 1;
+    public static final double MAX_VELOCITY = 1; 
+    
+    public static final double conversionFactor = Units.inchesToMeters(1.0/9.0 * Math.PI * 1.751); // gearing * pi * sprocket diameter
+
+
+    public static final int elevatorCurrentLimits = 60; //might need to adjust later
+
+
+
+    
+
+}
diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java
new file mode 100644
index 0000000..4cf27f2
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIO.java
@@ -0,0 +1,49 @@
+package frc.robot.subsystems.elevator;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface ElevatorIO {
+    @AutoLog
+    public static class ElevatorIOInputs {
+        public double elevatorRightAppliedVolts = 0.0;
+        public double elevatorRightPositionMeters= 0.0;
+        public double elevatorRightVelocityMetersPerSecond = 0.0;
+        public double elevatorRightCurrent = 0.0;
+
+        public double elevatorLeftAppliedVolts = 0.0;
+        public double elevatorLeftPositionMeters = 0.0;
+        public double elevatorLeftVelocityMetersPerSecond = 0.0;
+        public double elevatorLeftCurrent = 0.0;
+        
+        public double goalHeight = 0.0;
+        public double setpointVelocity = 0.0;
+        public double setpointPosition = 0.0;
+    }
+    
+
+
+        public default void updateInputs(ElevatorIOInputs inputs) {
+            
+        }
+
+        public default void hold() {}
+
+        public default void setGoal(double goal) {
+
+    }        
+    
+        public default void moveElevator(double speed) {
+
+        }
+
+    public default void updateMotionProfile() {}
+    
+        public default void setVoltage(double volts) {}   
+        
+        // True is brake, false is coast
+        public default void setIdleMode(boolean mode) {}
+    
+        public default void resetEncoders() {}
+
+    } 
+    
diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java
new file mode 100644
index 0000000..a5db6a9
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOReal.java
@@ -0,0 +1,121 @@
+package frc.robot.subsystems.elevator;
+
+import com.revrobotics.RelativeEncoder;
+
+import edu.wpi.first.math.controller.ElevatorFeedforward;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+
+import com.revrobotics.spark.SparkBase.ControlType;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.ClosedLoopSlot;
+import com.revrobotics.spark.SparkClosedLoopController;
+import com.revrobotics.spark.SparkFlex;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.config.SparkFlexConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+
+public class ElevatorIOReal implements ElevatorIO {
+    private SparkFlex rightElevatorMotor = new SparkFlex(ElevatorConstants.rightDeviceID, MotorType.kBrushless);
+    private SparkFlex leftElevatorMotor = new SparkFlex(ElevatorConstants.leftDeviceID, MotorType.kBrushless); 
+
+    private RelativeEncoder leftEncoder = leftElevatorMotor.getEncoder();
+    private RelativeEncoder rightEncoder = rightElevatorMotor.getEncoder();
+
+    private SparkClosedLoopController controller = rightElevatorMotor.getClosedLoopController();
+
+ //   private AbsoluteEncoder leftAbsoluteEncoder = leftElevatorMotor.getAbsoluteEncoder();
+
+    private final ElevatorFeedforward ffmodel = new ElevatorFeedforward(ElevatorConstants.ks, ElevatorConstants.kg, ElevatorConstants.kv);
+    private final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(ElevatorConstants.MAX_VELOCITY, ElevatorConstants.MAX_ACCELERATION);
+    // private final PIDController controller = new PIDController(ElevatorConstants.kP, ElevatorConstants.kI, ElevatorConstants.kD);
+    private final TrapezoidProfile profile = new TrapezoidProfile(constraints);
+    private TrapezoidProfile.State setpoint = new TrapezoidProfile.State();
+    private TrapezoidProfile.State goal = new TrapezoidProfile.State();
+
+    private SparkFlexConfig config = new SparkFlexConfig();
+
+    //zero stuff 
+    public void zeroElevator(){
+        leftEncoder.setPosition(0);
+        rightEncoder.setPosition(0);
+    }
+    
+    
+    public ElevatorIOReal(){
+        leftElevatorMotor.clearFaults();
+        rightElevatorMotor.clearFaults();
+        config.encoder.positionConversionFactor(ElevatorConstants.conversionFactor);
+        config.encoder.velocityConversionFactor(ElevatorConstants.conversionFactor / 60);    
+
+        // config.softLimit.forwardSoftLimitEnabled(true);
+        // config.softLimit.reverseSoftLimitEnabled(true);
+        // config.softLimit.forwardSoftLimit(ElevatorConstants.maxDistance);
+        // config.softLimit.reverseSoftLimit(ElevatorConstants.minDistance);
+
+        config.closedLoop.p(10);
+        config.closedLoop.i(0);
+        config.closedLoop.d(0);
+
+        config.smartCurrentLimit(ElevatorConstants.elevatorCurrentLimits);
+        config.idleMode(IdleMode.kBrake);
+        config.inverted(true);
+        rightElevatorMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+        config.follow(ElevatorConstants.rightDeviceID, true);
+    
+        //   config.absoluteEncoder.positionConversionFactor(Units.inchesToMeters(11 / 18));             use these later
+        //   config.absoluteEncoder.velocityConversionFactor(Units.inchesToMeters(11 / 18) / 60);        use these later
+        leftElevatorMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+    }
+
+    @Override
+    public void setVoltage(double voltage) {
+        controller.setReference(voltage, ControlType.kVoltage);
+    }
+
+    @Override
+    public void setGoal(double height) {
+        setpoint = new TrapezoidProfile.State(leftEncoder.getPosition(), leftEncoder.getVelocity());
+        goal = new TrapezoidProfile.State(height, 0);
+    }
+
+    @Override
+    public void updateMotionProfile() {
+        double prevVelocity = setpoint.velocity;
+        setpoint = profile.calculate(0.02, setpoint, goal);
+        double acceleration = (setpoint.velocity - prevVelocity) / 0.02;
+        double ffvolts = ffmodel.calculate(setpoint.velocity, acceleration);
+        controller.setReference(setpoint.position, ControlType.kPosition, ClosedLoopSlot.kSlot0, ffvolts);
+    }
+
+    @Override
+    public void hold() {
+        double ffvolts = ffmodel.calculate(0);
+        setVoltage(ffvolts);
+    }
+
+    @Override
+    public void updateInputs(ElevatorIOInputs inputs) {
+        inputs.elevatorRightCurrent = rightElevatorMotor.getOutputCurrent();
+        inputs.elevatorRightAppliedVolts = rightElevatorMotor.getAppliedOutput() * rightElevatorMotor.getBusVoltage();
+        inputs.elevatorRightPositionMeters = rightEncoder.getPosition();
+        inputs.elevatorRightVelocityMetersPerSecond = rightEncoder.getVelocity();
+       // inputs.elevatorRightMotorPositionMeters = leftAbsoluteEncoder.getPosition();
+       // inputs.elevatorRightMotorVelocityMetersPerSecond = leftAbsoluteEncoder.getVelocity();     will use absolute encoder later
+
+
+        inputs.elevatorLeftCurrent = leftElevatorMotor.getOutputCurrent();
+        inputs.elevatorLeftAppliedVolts = leftElevatorMotor.getAppliedOutput() * leftElevatorMotor.getBusVoltage();
+        inputs.elevatorLeftPositionMeters = leftEncoder.getPosition();
+        inputs.elevatorLeftVelocityMetersPerSecond = leftEncoder.getVelocity();
+
+        inputs.setpointVelocity = setpoint.velocity;
+        inputs.goalHeight = goal.position;
+    }
+
+    @Override
+    public void moveElevator(double speed) {
+        rightElevatorMotor.set(speed);
+    }
+}
+    
diff --git a/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java
new file mode 100644
index 0000000..e9b4483
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/elevator/ElevatorIOSim.java
@@ -0,0 +1,67 @@
+package frc.robot.subsystems.elevator;
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.controller.ElevatorFeedforward;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.simulation.ElevatorSim;
+import edu.wpi.first.math.controller.SimpleMotorFeedforward;
+
+public class ElevatorIOSim implements ElevatorIO {
+    private ElevatorSim elevatorSim = new ElevatorSim(ElevatorConstants.kv, ElevatorConstants.ks, DCMotor.getNeoVortex(2), 0, 0.7112, true, 0);
+
+    private double elevatorAppliedVolts = 0.0;
+
+
+private final ElevatorFeedforward ffmodel = new ElevatorFeedforward(ElevatorConstants.ks, ElevatorConstants.kg - .28, ElevatorConstants.kv);
+private final TrapezoidProfile.Constraints constraints = new TrapezoidProfile.Constraints(2, 4);
+private final PIDController controller = new PIDController(50, 0, 1);
+private final TrapezoidProfile profile = new TrapezoidProfile(constraints);
+private TrapezoidProfile.State setpoint = new TrapezoidProfile.State();
+private TrapezoidProfile.State goal = new TrapezoidProfile.State();
+
+@Override 
+public void updateInputs(ElevatorIOInputs inputs) {
+    elevatorSim.update(0.02);
+    inputs.elevatorLeftPositionMeters = elevatorSim.getPositionMeters();
+    inputs.elevatorLeftVelocityMetersPerSecond = elevatorSim.getVelocityMetersPerSecond();
+    inputs.elevatorLeftCurrent = elevatorSim.getCurrentDrawAmps();
+    inputs.elevatorLeftAppliedVolts = elevatorAppliedVolts;
+    inputs.goalHeight = goal.position;
+    inputs.setpointVelocity = setpoint.velocity;
+    inputs.setpointPosition = setpoint.position;
+}
+
+@Override
+public void moveElevator(double speed) {
+    elevatorAppliedVolts = 12 * speed;
+    elevatorSim.setInputVoltage(elevatorAppliedVolts);
+}
+
+@Override
+public void updateMotionProfile() {
+    setVoltage(ffmodel.calculate(setpoint.velocity) + controller.calculate(elevatorSim.getPositionMeters(), setpoint.position));
+    setpoint = profile.calculate(0.02, setpoint, goal);
+}
+
+@Override
+public void hold() {
+    double ffvolts = ffmodel.calculate(0);
+    setVoltage(ffvolts);
+}
+
+@Override
+public void setVoltage(double volts){
+    elevatorAppliedVolts = MathUtil.clamp(volts, -12, 12);
+    // Logger.recordOutput("Setting output", volts);
+    elevatorSim.setInputVoltage(elevatorAppliedVolts);
+}
+
+@Override
+public void setGoal(double height) {
+    goal = new TrapezoidProfile.State(height, 0);
+        
+}
+
+    }
\ No newline at end of file
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..f06abcf
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/Intake.java
@@ -0,0 +1,82 @@
+package frc.robot.subsystems.intake;
+
+import edu.wpi.first.wpilibj2.command.Command;
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+import frc.robot.Constants.IntakeSetpoints;
+import org.littletonrobotics.junction.Logger;
+
+public class Intake extends SubsystemBase {
+    private IntakeIO io;
+    private IntakeIOInputsAutoLogged inputs = new IntakeIOInputsAutoLogged();
+    private double intakeSpeed = 0.0;
+
+    private static Intake instance;
+
+    public static Intake getInstance(){
+        return instance;
+    }
+
+    public static Intake initialize(IntakeIO io){
+        if(instance == null){
+            instance = new Intake(io);
+        }
+        return instance;
+    }
+
+    private Intake(IntakeIO io) {
+        this.io = io;
+        io.updateInputs(inputs);
+    }
+
+    public double getPosition() {
+        return inputs.angularPositionRot;
+    }
+
+    public void setIntakeSpeed(double speed){
+        intakeSpeed = speed;
+        io.setMotorSpeed(intakeSpeed);
+    }
+
+    /*
+    public Command intakeS() {
+        return startEnd(() -> setIntakeSpeed(IntakeSetpoints.intake), () -> setIntakeSpeed(0)).until(() -> GamePieceInitial()).andThen(() -> intakeSlow().until(() -> GamePeiceFinal()));
+        return startEnd(() -> setIntakeSpeed(IntakeSetpoints.intake), () -> setIntakeSpeed(0)).until(() -> GamePieceInitial());
+    }
+    */
+
+    public Command intake() {
+        return startEnd(() -> setIntakeSpeed(IntakeSetpoints.intake), () -> setIntakeSpeed(0));
+    }
+
+    public Command intakeSlow() {
+        return startEnd(() -> setIntakeSpeed(IntakeSetpoints.slow), () -> setIntakeSpeed(0));
+    }
+
+    public Command reverse() {
+        return startEnd(() -> setIntakeSpeed(IntakeSetpoints.reverse), () -> setIntakeSpeed(0));
+    }
+
+    
+    public boolean GamePieceInitial() {
+        return IntakeConstants.initialThreshold <= inputs.currentAmps;
+    }
+
+    public boolean GamePieceFinal() {
+        return IntakeConstants.currentThreshold <= inputs.currentAmps; 
+    }
+
+    public boolean hasGamepiece() {
+        return io.hasGamepiece();
+    }
+    
+    @Override
+    public void periodic(){
+        io.updateInputs(inputs);
+        Logger.processInputs("Intake", inputs);
+        Logger.recordOutput("Intake/Intake speed", intakeSpeed);
+        Logger.recordOutput("Intake/Gamepiece detected", hasGamepiece());
+    }
+}
+
+
+
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
new file mode 100644
index 0000000..6985873
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeConstants.java
@@ -0,0 +1,15 @@
+package frc.robot.subsystems.intake;
+
+public final class IntakeConstants {
+    //PLACEHOLDERS
+    public static final int canID = 18;
+
+    public static final double simGearRatio = 0;
+    public static final double overrideSpeed = -0.0;
+
+    public static final double intakeCurrent = 0;
+    public static final int currentLimit = 45; 
+    public static final double currentThreshold = 25; //change later based on akit numbers for gamepiece
+
+    public static final double initialThreshold = 25;
+}
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..1cf1d54
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeIO.java
@@ -0,0 +1,23 @@
+package frc.robot.subsystems.intake;
+
+import org.littletonrobotics.junction.AutoLog;
+
+public interface IntakeIO {
+    @AutoLog
+    public static class IntakeIOInputs {
+        public double voltage = 0.0;
+        public double angularPositionRot = 0.0;
+        public double angularVelocityRPM = 0.0;
+        public double currentAmps = 0.0;
+        public boolean hasGamepiece = false;
+    }
+    public default void updateInputs(IntakeIOInputs inputs) {
+    }
+
+    public default void setMotorSpeed(double speed) {
+    }
+
+    public default boolean hasGamepiece() {
+        return false;
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java
new file mode 100644
index 0000000..eb1e706
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOReal.java
@@ -0,0 +1,72 @@
+package frc.robot.subsystems.intake;
+
+import com.ctre.phoenix6.BaseStatusSignal;
+import com.ctre.phoenix6.StatusCode;
+import com.ctre.phoenix6.StatusSignal;
+import com.ctre.phoenix6.configs.TalonFXConfiguration;
+import com.ctre.phoenix6.hardware.TalonFX;
+import com.ctre.phoenix6.signals.InvertedValue;
+import com.ctre.phoenix6.signals.NeutralModeValue;
+import edu.wpi.first.units.measure.AngularVelocity;
+import edu.wpi.first.units.measure.Current;
+import edu.wpi.first.units.measure.Voltage;
+
+
+public class IntakeIOReal implements IntakeIO {
+    private final TalonFX intakeMotor = new TalonFX(IntakeConstants.canID);
+    
+    private TalonFXConfiguration talonFXConfig = new TalonFXConfiguration();
+
+    private final StatusSignal<Current> current = intakeMotor.getStatorCurrent();
+    private final StatusSignal<Voltage> voltage = intakeMotor.getMotorVoltage();
+    private final StatusSignal<AngularVelocity> velocity = intakeMotor.getVelocity();
+
+    public IntakeIOReal() {
+        talonFXConfig.MotorOutput.withInverted(InvertedValue.Clockwise_Positive);
+
+        talonFXConfig.CurrentLimits.StatorCurrentLimitEnable = true;
+        talonFXConfig.CurrentLimits.StatorCurrentLimit = IntakeConstants.currentLimit;
+
+
+        talonFXConfig.Audio.BeepOnBoot = true;
+        
+        talonFXConfig.MotorOutput.NeutralMode = NeutralModeValue.Coast;
+
+        intakeMotor.clearStickyFaults();
+
+        BaseStatusSignal.setUpdateFrequencyForAll(50, velocity, voltage, current);
+
+        intakeMotor.optimizeBusUtilization(4, 1);
+
+        StatusCode response = intakeMotor.getConfigurator().apply(talonFXConfig);
+        if (!response.isOK()) {
+            System.out.println(
+                    "Talon ID "
+                            + intakeMotor.getDeviceID()
+                            + " failed config with error "
+                            + response.toString());
+        }
+    }
+
+    public void updateInputs(IntakeIOInputs inputs) {
+        BaseStatusSignal.refreshAll(velocity, voltage, current);
+        
+        inputs.angularVelocityRPM = velocity.getValueAsDouble() * 60;
+        inputs.angularPositionRot = intakeMotor.getPosition().getValueAsDouble();
+        inputs.currentAmps = current.getValueAsDouble();
+        inputs.voltage = voltage.getValueAsDouble();
+        inputs.hasGamepiece = hasGamepiece();
+    }
+
+    @Override
+    public boolean hasGamepiece() {
+        return current.getValueAsDouble() > IntakeConstants.currentThreshold;
+    }
+
+    @Override
+    public void setMotorSpeed(double speed) {
+        intakeMotor.set(-speed);
+    }
+
+}
+
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIORealVortex.java b/src/main/java/frc/robot/subsystems/intake/IntakeIORealVortex.java
new file mode 100644
index 0000000..b583c82
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeIORealVortex.java
@@ -0,0 +1,44 @@
+package frc.robot.subsystems.intake;
+
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.SparkFlex;
+import com.revrobotics.spark.SparkBase.PersistMode;
+import com.revrobotics.spark.SparkBase.ResetMode;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.config.SparkFlexConfig;
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+
+
+public class IntakeIORealVortex implements IntakeIO {
+    private final SparkFlex intakeMotor = new SparkFlex(IntakeConstants.canID, MotorType.kBrushless);
+    private final RelativeEncoder encoder = intakeMotor.getEncoder();
+    
+    private SparkFlexConfig config = new SparkFlexConfig();
+
+    public IntakeIORealVortex() {
+        intakeMotor.clearFaults();
+        config.smartCurrentLimit(IntakeConstants.currentLimit);
+        config.idleMode(IdleMode.kCoast);
+
+        intakeMotor.configure(config, ResetMode.kResetSafeParameters, PersistMode.kPersistParameters);
+    }
+
+    public void updateInputs(IntakeIOInputs inputs) {
+        inputs.angularVelocityRPM = encoder.getVelocity();
+        inputs.angularPositionRot = encoder.getPosition();
+        inputs.currentAmps = intakeMotor.getOutputCurrent();
+        inputs.voltage = intakeMotor.getBusVoltage();
+    }
+
+    @Override
+    public boolean hasGamepiece() {
+        return intakeMotor.getOutputCurrent() > IntakeConstants.currentThreshold;
+    }
+
+    @Override
+    public void setMotorSpeed(double speed) {
+        intakeMotor.set(-speed);
+    }
+
+}
+
diff --git a/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java
new file mode 100644
index 0000000..e84c1c7
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/intake/IntakeIOSim.java
@@ -0,0 +1,32 @@
+package frc.robot.subsystems.intake;
+
+
+import edu.wpi.first.math.MathUtil;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.wpilibj.simulation.DCMotorSim;
+import edu.wpi.first.math.system.plant.LinearSystemId;
+
+public class IntakeIOSim implements IntakeIO {
+  // not done
+  private final DCMotorSim motor = new DCMotorSim(LinearSystemId.createDCMotorSystem(DCMotor.getKrakenX60(1), 0.001, 2), DCMotor.getKrakenX60(1));
+  private boolean isGamepieceDetected = false; 
+  @Override
+  public void updateInputs(IntakeIOInputs inputs) {
+    motor.update(0.02);
+
+    inputs.angularPositionRot = motor.getAngularPositionRotations();
+    inputs.angularVelocityRPM = motor.getAngularVelocityRPM();
+    inputs.currentAmps = motor.getCurrentDrawAmps();
+    isGamepieceDetected = inputs.currentAmps > IntakeConstants.currentThreshold;
+    inputs.hasGamepiece = hasGamepiece();
+  }
+  @Override
+  public boolean hasGamepiece(){
+    return isGamepieceDetected;
+  }
+
+  @Override
+  public void setMotorSpeed(double speed) {
+    motor.setInputVoltage(MathUtil.clamp(12 * speed, -12, 12));
+  }
+}
\ No newline at end of file
diff --git a/tuner-project.json b/tuner-project.json
new file mode 100644
index 0000000..c1658a3
--- /dev/null
+++ b/tuner-project.json
@@ -0,0 +1 @@
+{"Version":"1.0.0.0","LastState":11,"Modules":[{"ModuleName":"Front Left","ModuleId":0,"Encoder":{"Id":22,"Name":"Front Left Encoder","Model":"CANCoder","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":2,"Name":"FrontLeft Turn","Model":"Talon FX vers. C","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"DriveMotor":{"Id":1,"Name":"FrontLeft Drive","Model":"Talon FX vers. C","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":true,"SelectedEncoderType":"CANcoder","EncoderOffset":0.164794921875,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":2,"ValidatedDriveId":1,"ValidatedEncoderId":22},{"ModuleName":"Front Right","ModuleId":1,"Encoder":{"Id":23,"Name":"Front Right Encoder","Model":"CANCoder","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":4,"Name":"Front Right Turn","Model":"Talon FX vers. C","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"DriveMotor":{"Id":3,"Name":"Front Right Drive","Model":"Talon FX vers. C","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":true,"SelectedEncoderType":"CANcoder","EncoderOffset":-0.0107421875,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":4,"ValidatedDriveId":3,"ValidatedEncoderId":23},{"ModuleName":"Back Left","ModuleId":2,"Encoder":{"Id":21,"Name":"Back Left Encoder","Model":"CANCoder","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":7,"Name":"BackLeft Turn","Model":"Talon FX vers. C","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"DriveMotor":{"Id":8,"Name":"Back Left Drive","Model":"Talon FX vers. C","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":true,"SelectedEncoderType":"CANcoder","EncoderOffset":-0.458984375,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":7,"ValidatedDriveId":8,"ValidatedEncoderId":21},{"ModuleName":"Back Right","ModuleId":3,"Encoder":{"Id":24,"Name":"Back Right Encoder","Model":"CANCoder","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":null,"IsStandaloneFx":false},"SteerMotor":{"Id":5,"Name":"Back Right Turn","Model":"Talon FX vers. C","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"DriveMotor":{"Id":6,"Name":"Back Right Drive","Model":"Talon FX vers. C","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":{"Name":"WCP Kraken x60","FreeSpeedRps":96.7,"SlipCurrentLimit":120,"StatorCurrentLimit":60},"IsStandaloneFx":false},"IsEncoderInverted":false,"IsSteerInverted":true,"SelectedEncoderType":"CANcoder","EncoderOffset":0.07958984375,"DriveMotorSelectionState":1,"SteerMotorSelectionState":1,"SteerEncoderSelectionState":1,"IsModuleValidationComplete":true,"ValidatedSteerId":5,"ValidatedDriveId":6,"ValidatedEncoderId":24}],"SwerveOptions":{"kSpeedAt12Volts":4.749788892927424,"Gyro":{"Id":20,"Name":"Pigeon 2 (Device ID 10)","Model":"Pigeon 2","CANbus":"rio","CANbusFriendly":"","SelectedMotorType":null,"IsStandaloneFx":false},"IsValidGyroCANbus":true,"VerticalTrackSizeInches":21.25,"HorizontalTrackSizeInches":21.25,"WheelRadiusInches":2.0,"IsLeftSideInverted":false,"IsRightSideInverted":true,"SwerveModuleType":8,"SwerveModuleConfiguration":{"ModuleBrand":8,"DriveRatio":6.720000000000001,"SteerRatio":13.371428571428572,"CouplingRatio":4.2,"CustomName":"X2 10T"},"HasVerifiedSteer":true,"SelectedModuleManufacturer":"WestCoast Products (WCP)","HasVerifiedDrive":true,"IsValidConfiguration":true}}
\ No newline at end of file