diff --git a/.vscode/settings.json b/.vscode/settings.json
index 565b080..374745b 100644
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -26,5 +26,5 @@
     },
   ],
   "java.test.defaultConfig": "WPIlibUnitTests",
-  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx4G -Xms100m -Xlog:disable"
+  "java.jdt.ls.vmargs": "-XX:+UseParallelGC -XX:GCTimeRatio=4 -XX:AdaptiveSizePolicyWeight=90 -Dsun.zip.disableMemoryMapping=true -Xmx8G -Xms100m -Xlog:disable"
 }
diff --git a/src/main/java/frc/robot/Robot.java b/src/main/java/frc/robot/Robot.java
index 742b65f..79169b4 100644
--- a/src/main/java/frc/robot/Robot.java
+++ b/src/main/java/frc/robot/Robot.java
@@ -22,8 +22,6 @@
 import frc.robot.systems.SmartPrinter;
 import frc.robot.systems.SwerveDrive;
 import frc.robot.systems.AutoBalance.Stage;
-import frc.robot.systems.ElevFourbar.ControlType;
-import frc.robot.systems.ElevFourbar.Setpoint;
 import frc.robot.systems.LEDLights;
 import frc.robot.systems.Bat;
 import frc.robot.subsystems.AxisRateLimiter;
@@ -39,7 +37,8 @@ public class Robot extends TimedRobot {
     public enum ControlMode {
         DISABLED,
         AUTONOMOUS,
-        TELEOPERATED
+        TELEOPERATED,
+        SAFETY
     }
 
     private static final XboxController controllerOne = (XboxController)Controls.getControllerByPort(0);
@@ -143,7 +142,7 @@ public void autonomousPeriodic() {
     @Override
     public void teleopInit() {
         controlMode = ControlMode.TELEOPERATED;
-        SwerveDrive.setMode(SwerveMode.Headless);
+        SwerveDrive.setMode(SwerveMode.HEADLESS);
         ElevFourbar.init();
         Intake.init();
     }
@@ -168,42 +167,33 @@ public void teleopPeriodic() {
         }
 
         SwerveDrive.run(x, y, controllerOne.getRightX(), controllerOne.getPOV());
-        SwerveDrive.setRockMode(controllerOne.getRightTriggerAxis() > 0.25);
+        SwerveDrive.conditionalTempMode(SwerveMode.ROCK, controllerOne.getRightTriggerAxis() > 0.25);
         
         // Left stick changes between headless and relative control modes.
         if (controllerOne.getLeftStickButtonReleased()) {
-            if (SwerveDrive.getMode() == SwerveMode.Headless) {
-                SwerveDrive.setMode(SwerveMode.Relative);
+            if (SwerveDrive.getMode() == SwerveMode.HEADLESS) {
+                SwerveDrive.setMode(SwerveMode.RELATIVE);
             } else {
-                SwerveDrive.setMode(SwerveMode.Headless);
+                SwerveDrive.setMode(SwerveMode.HEADLESS);
             }
         }
         
         Intake.run(
-            controlPanel.getRawButtonPressed(8), //controller one dpad to control pivot
-            controlPanel.getRawButton(9),
-            controlPanel.getRawButton(7),
-            controlPanel.getRawButton(6),
-            controlPanel.getRawButtonReleased(6)
+            controlPanel.getRawButtonPressed(8), //Pivot toggle
+            controlPanel.getRawButton(9), //Claw hold
+            controlPanel.getRawButton(6), //Intake hold
+            controlPanel.getRawButton(7) //Outtake hold
         );
-        
-        // Hold button for setpoints.
-        if (controlPanel.getRawButton(4)) {
-            ElevFourbar.setSetPoint(Setpoint.HIGH_SCORING);
-        } else if (controlPanel.getRawButton(3)) {
-            ElevFourbar.setSetPoint(Setpoint.MID_SCORING);
-        } else if (controlPanel.getRawButton(1)) {
-            ElevFourbar.setSetPoint(Setpoint.GROUND_INTAKE);
-        } else if (ElevFourbar.getControlType() == ControlType.POSITION) {
-            ElevFourbar.setSetPoint(Setpoint.STOWED);
-        }
 
         ElevFourbar.run(
-            controllerTwo.getRightY(),
-            Math.abs(controlPanel.getRawAxis(0) / 2) > Math.abs(controllerTwo.getLeftY()) ? controlPanel.getRawAxis(0) / 2 : -controllerTwo.getLeftY(),
-            controllerTwo.getPOV(),
-            controlPanel.getRawButtonPressed(5), //toggle game piece
-            controllerTwo.getStartButtonPressed() //zero elevator encoder
+            controllerTwo.getRightY(), //Manual elevator
+            Math.abs(controlPanel.getRawAxis(0) / 2) > Math.abs(controllerTwo.getLeftY()) ? controlPanel.getRawAxis(0) / 2 : -controllerTwo.getLeftY(), //Manual fourbar
+            controllerTwo.getPOV(), //DPad direction
+            controlPanel.getRawButtonPressed(5), //Toggle game piece
+            controlPanel.getRawButton(1), //Ground intake
+            controlPanel.getRawButton(3), //Mid scoring
+            controlPanel.getRawButton(4), //High scoring
+            controllerTwo.getStartButtonPressed() || controlPanel.getRawButton(2) //Zero elevator encoder
         );
     }
     
@@ -219,14 +209,6 @@ public void disabledPeriodic() {
         ElevFourbar.toggleGamePiece(controlPanel.getRawButtonReleased(5)); 
     }
     
-    /** This  function is called once when test mode is enabled. */
-    @Override
-    public void testInit() {}
-    
-    /** This function is called periodically during test mode. */
-    @Override
-    public void testPeriodic() {}
-    
     /** This function is called once when the robot is first started up. */
     @Override
     public void simulationInit() {}
diff --git a/src/main/java/frc/robot/subsystems/Angle.java b/src/main/java/frc/robot/subsystems/Angle.java
new file mode 100644
index 0000000..e695a93
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Angle.java
@@ -0,0 +1,39 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+package frc.robot.subsystems;
+
+public final class Angle {
+	public static final double TAU = Math.PI * 2;
+
+	private double dec = 0.0;
+
+	public double radians() {
+		return dec * TAU;
+	}
+
+	public double degrees() {
+		return dec * 360.0;
+	}
+
+	public Angle setRadians(double radians) {
+		dec = radians / TAU;
+		return this;
+	}
+
+	public Angle setDegrees(double degrees) {
+		dec = degrees / 360.0;
+		return this;
+	}
+
+	/**
+	 * Creates a new identical angle so that modification of the original will not
+	 * effect the value returned from this function.
+	 */
+	public Angle clone() {
+		var ang = new Angle();
+		ang.dec = this.dec;
+		return ang;
+	}
+}
diff --git a/src/main/java/frc/robot/subsystems/Claw.java b/src/main/java/frc/robot/subsystems/Claw.java
index 63d8509..99eda27 100644
--- a/src/main/java/frc/robot/subsystems/Claw.java
+++ b/src/main/java/frc/robot/subsystems/Claw.java
@@ -12,19 +12,20 @@ public Claw(PneumaticsModuleType moduleType, int forwardChannel, int reverseChan
         clawSolenoid = new DoubleSolenoid(moduleType, forwardChannel, reverseChannel);
     }
 
+    //Open the claw
     public void open() {
         clawSolenoid.set(Value.kForward);
     }
 
+    //Close the claw
     public void close() {
         clawSolenoid.set(Value.kReverse);
     }
 
+    //Returns the SolenoidState of the claw piston
     public SolenoidState getState() {
         if(clawSolenoid.get() == Value.kForward) {
             return SolenoidState.OPEN;
-        } else if(clawSolenoid.get() == Value.kReverse) {
-            return SolenoidState.CLOSED;
         } else {
             return SolenoidState.CLOSED;
         }
diff --git a/src/main/java/frc/robot/subsystems/Coordinate.java b/src/main/java/frc/robot/subsystems/Coordinate.java
new file mode 100644
index 0000000..f4200c2
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Coordinate.java
@@ -0,0 +1,11 @@
+package frc.robot.subsystems;
+
+public class Coordinate {
+    public double x;
+    public double y;
+
+    public Coordinate(double x, double y) {
+        this.x = x;
+        this.y = y;
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/DoubleSetpoint.java b/src/main/java/frc/robot/subsystems/DoubleSetpoint.java
new file mode 100644
index 0000000..e45a1ab
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/DoubleSetpoint.java
@@ -0,0 +1,16 @@
+package frc.robot.subsystems;
+
+/**
+ * Enables the ability to store both cube 
+ */
+public class DoubleSetpoint extends Setpoint {
+    public Setpoint cube;
+    public Setpoint cone;
+
+    public DoubleSetpoint(Setpoint cube, Setpoint cone) {
+        super(0, 0);
+
+        this.cube = cube;
+        this.cone = cone;
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/Elevator.java b/src/main/java/frc/robot/subsystems/Elevator.java
index ed68491..1423877 100644
--- a/src/main/java/frc/robot/subsystems/Elevator.java
+++ b/src/main/java/frc/robot/subsystems/Elevator.java
@@ -1,14 +1,13 @@
 package frc.robot.subsystems;
 
-import com.ctre.phoenix.motorcontrol.can.TalonFX;
-
-import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 
-import com.ctre.phoenix.motorcontrol.ControlMode;
 import com.ctre.phoenix.motorcontrol.FeedbackDevice;
+import com.ctre.phoenix.motorcontrol.ControlMode;
 import com.ctre.phoenix.motorcontrol.NeutralMode;
-import frc.robot.systems.ElevFourbar.Setpoint;
-import frc.robot.systems.ElevFourbar;
+import com.ctre.phoenix.motorcontrol.can.TalonFX;
+import edu.wpi.first.wpilibj.DigitalInput;
 
 /**
  * Controls the elevator
@@ -16,17 +15,14 @@
 public class Elevator {
     //Ticks per inch constant, gear reduction is 12:1
     private static final double TICKS_PER_INCH = -6144.0; //FINAL VALUE DO NOT CHANGE
+    // THROUGH BORE = -455.156
     
     //constant variables
     private static final double MANUAL_DEADZONE = 0.3;
     private static final int ELEVATOR_MOTOR_ID = 62; //CORRECT ID
-    
-    //position constants, in inches
-    private static final double SUBSTATION_INTAKE_SETPOINT = 9;
-    private static final double GROUND_INTAKE_SETPOINT = 30;
-    private static final double MID_SCORING_SETPOINT = 11;
-    private static final double HIGH_SCORING_SETPOINT = 30;
-    private static final double STOWED_SETPOINT = 0;
+
+    private static final int ENCODER_CHANNEL_A = 4;
+    private static final int ENCODER_CHANNEL_B = 5;
     
     //PID constants
     private static final double P = 1.0;
@@ -36,6 +32,10 @@ public class Elevator {
     //Motor and controller
     private final TalonFX elevatorMotor;
 
+    //Encoder and CAN Spark Max
+    private Encoder encoder;
+
+
     //variables
     private double encoderPosition;
     private double targetSetpoint;
@@ -46,6 +46,10 @@ public class Elevator {
      * Sets up elevator motor and Xbox controller, configures PID
      */
     public Elevator() {
+
+        //Encoder
+        encoder = new Encoder(ENCODER_CHANNEL_A, ENCODER_CHANNEL_B);
+        
         elevatorMotor = new TalonFX(ELEVATOR_MOTOR_ID);
 
         elevatorMotor.configFactoryDefault();
@@ -55,8 +59,8 @@ public Elevator() {
         elevatorMotor.config_kD(0, D);
         elevatorMotor.selectProfileSlot(0, 0);
 
-        elevatorMotor.configPeakOutputForward(0.85);
-        elevatorMotor.configPeakOutputReverse(-0.85);
+        elevatorMotor.configPeakOutputForward(0.7);
+        elevatorMotor.configPeakOutputReverse(-0.7);
         
         //Configures motor to brake when not being used
         elevatorMotor.setNeutralMode(NeutralMode.Brake);
@@ -67,10 +71,11 @@ public Elevator() {
      * The method to be run in teleopInit to reset variables
      */
     public void init() {
+
     }
 
     public void autonomousInit() {
-        targetSetpoint = STOWED_SETPOINT;
+        targetSetpoint = 0;
     }
 
     /**
@@ -99,6 +104,8 @@ public void manualControl(double speed, int dPadDirection) {
         }
 
         elevatorMotor.set(ControlMode.PercentOutput, speed * 0.6);
+    
+        SmartDashboard.putNumber("Elev Encoder", encoder.get());
     }
 
     /**
@@ -108,32 +115,17 @@ public void manualControl(double speed, int dPadDirection) {
     public void pidControl(Setpoint setpoint) {
         encoderPosition = elevatorMotor.getSensorCollection().getIntegratedSensorPosition();
 
-        updateTargetSetpoint(setpoint);
+        targetSetpoint = setpoint.getElevPos();
 
         //set elevator PID position to target setpoint
         elevatorMotor.set(ControlMode.Position, targetSetpoint * TICKS_PER_INCH);
-    }
-
-    /**
-     * PID Control of the elevator using coordinates
-     * @param coords The coordinates to move to, on an x and y plane
-     */
-    public void pidControl(double[] coords) {
-        encoderPosition = elevatorMotor.getSensorCollection().getIntegratedSensorPosition();
 
-        double[] pos = ElevFourbar.coordsToPos(coords[0], coords[1]);
-        targetSetpoint = pos[0];
-
-        if(coords[0] == 35.2) {
-            targetSetpoint = 31;
+        if(encoder.get() >= 0) {
+            zeroEncoder(0.25);
         }
 
-        /* if(Math.abs(targetSetpoint - 2.9) < 0.5) {
-            targetSetpoint = GROUND_INTAKE_SETPOINT;
-        } */
-        
-        //set elevator PID position to target setpoint
-        elevatorMotor.set(ControlMode.Position, targetSetpoint * TICKS_PER_INCH);
+        SmartDashboard.putNumber("Elev Encoder", encoder.get());
+        SmartDashboard.putNumber("Elev Pos", getPosition());
     }
 
     /**
@@ -152,28 +144,9 @@ public double getTargetPosition() {
         return targetSetpoint;
     }
 
-    private void updateTargetSetpoint(Setpoint setpoint) {
-        switch (setpoint) {
-            case SUBSTATION_INTAKE:
-                targetSetpoint = SUBSTATION_INTAKE_SETPOINT;
-                break;
-            case GROUND_INTAKE:
-                if(ElevFourbar.fourbar.getPosition() > 5)
-                    targetSetpoint = GROUND_INTAKE_SETPOINT;
-                break;
-            case MID_SCORING:
-                targetSetpoint = MID_SCORING_SETPOINT;
-                break;
-            case HIGH_SCORING:
-                targetSetpoint = HIGH_SCORING_SETPOINT;
-                break;
-            case STOWED:
-                targetSetpoint = STOWED_SETPOINT;
-                break;
-        }
-    }
+    public void zeroEncoder(double inches) {
+        double ticks = inches * TICKS_PER_INCH;
 
-    public void zeroEncoder() {
-        elevatorMotor.getSensorCollection().setIntegratedSensorPosition(elevatorMotor.getSensorCollection().getIntegratedSensorPosition() + 2000, 30);
+        elevatorMotor.getSensorCollection().setIntegratedSensorPosition(2000, 30);
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/Fourbar.java b/src/main/java/frc/robot/subsystems/Fourbar.java
index 031644c..ea073bc 100644
--- a/src/main/java/frc/robot/subsystems/Fourbar.java
+++ b/src/main/java/frc/robot/subsystems/Fourbar.java
@@ -1,16 +1,16 @@
 package frc.robot.subsystems;
 
 import com.revrobotics.CANSparkMax;
-import com.revrobotics.SparkMaxAbsoluteEncoder;
-import com.revrobotics.SparkMaxPIDController;
 import com.revrobotics.CANSparkMax.IdleMode;
-import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+import com.revrobotics.SparkMaxPIDController;
+import com.revrobotics.SparkMaxAbsoluteEncoder;
 import com.revrobotics.SparkMaxAbsoluteEncoder.Type;
 
+import com.revrobotics.CANSparkMaxLowLevel.MotorType;
+
+import frc.robot.systems.Intake.SolenoidState;
 import frc.robot.systems.ElevFourbar;
 import frc.robot.systems.Intake;
-import frc.robot.systems.ElevFourbar.Setpoint;
-import frc.robot.systems.Intake.SolenoidState;
 
 /** 
  * Class to control the fourbar mechanism 
@@ -21,29 +21,21 @@ public class Fourbar {
     private static final double FOURBAR_SPEED_UP = -0.45;
 
     //encoder offset
-    public final double ENCODER_OFFSET = 0.145 * 360;
+    public final double ENCODER_OFFSET = 0.330 * 360;
     
     //constant variables
     private static final int MOTOR_ID = 61; //CORRECT ID
-    private static final double MANUAL_DEADZONE = 0.3;
-
-    //position constants, in degrees
-    private static final int SUBSTATION_INTAKE_SETPOINT = 33 ;
-    private static final int GROUND_INTAKE_UP_SETPOINT = 140;
-    private static final int GROUND_INTAKE_DOWN_SETPOINT = 112;
-    private static final int MID_SCORING_SETPOINT = 33;
-    private static final int HIGH_SCORING_SETPOINT = 73;
-    private static final int STOWED_SETPOINT = 0;
+    private static final double MANUAL_DEADZONE = 0.2;
 
     //PID constants
-    private static final double P0 = 10;    
+    private static final double P0 = 5;    
     private static final double I0 = 0.0;
-    private static final double D0 = 1;
+    private static final double D0 = 0.05;
     private static final double F0 = 0.0;
 
-    private static final double P1 = 10;
+    private static final double P1 = 2.5;
     private static final double I1 = 0.0;
-    private static final double D1 = 4;
+    private static final double D1 = 0.3;
     private static final double F1 = 0.0;
     
     //Motor and controller
@@ -51,7 +43,7 @@ public class Fourbar {
     private final SparkMaxPIDController pidController; 
     private SparkMaxAbsoluteEncoder absoluteEncoder;
 
-    //variables
+    //instance variables
     private double targetSetpoint;
     private double encoderPosition;
     private double bumperIntercept = 0;
@@ -70,6 +62,7 @@ public void setBrake(boolean brake) {
      */
     public Fourbar(){
         fourbarMotor = new CANSparkMax(MOTOR_ID, MotorType.kBrushless);
+        fourbarMotor.setSmartCurrentLimit(30);
 
         absoluteEncoder = fourbarMotor.getAbsoluteEncoder(Type.kDutyCycle);
         pidController = fourbarMotor.getPIDController();
@@ -82,7 +75,8 @@ public Fourbar(){
         pidController.setP(P1, 1);
         pidController.setI(I1, 1);
         pidController.setD(D1, 1);
-        pidController.setFF(F1, 1);
+        pidController.setFF(F1, 1); 
+
         pidController.setOutputRange(FOURBAR_SPEED_UP, -FOURBAR_SPEED_UP);
 
         pidController.setFeedbackDevice(absoluteEncoder);
@@ -99,57 +93,42 @@ public Fourbar(){
      * @param setpoint The setpoint to move to, as defined in the Setpoint enum
      */
     public void pidControl(Setpoint setpoint){
-
-        encoderPosition = (absoluteEncoder.getPosition()*360) - ENCODER_OFFSET;
         
-        updateTargetSetpoint(setpoint);
-
-        pidController.setReference((targetSetpoint + ENCODER_OFFSET) / 360.0, CANSparkMax.ControlType.kPosition);
-    }
+        pidController.setOutputRange(FOURBAR_SPEED_UP, -FOURBAR_SPEED_UP);
 
-    /**
-     * Allows for translating to setpoints using PID
-     * @param coords The coordinates to move to, on an x and y plane
-     */
-    public void pidControl(double[] coords) {
         encoderPosition = (absoluteEncoder.getPosition()*360) - ENCODER_OFFSET;
-
-        double[] pos = ElevFourbar.coordsToPos(coords[0], coords[1]);
-        targetSetpoint = pos[1];
-
-        if(coords == ElevFourbar.GROUND_INTAKE_DOWN_COORDS) {
-            targetSetpoint = 103;
-        }
-
-        /* if(Math.abs(targetSetpoint - 68) < 0.5) {
-            targetSetpoint = 112; 
-        } */
+        
+        targetSetpoint = setpoint.getFourbarPos();
 
         pidController.setReference((targetSetpoint + ENCODER_OFFSET) / 360.0, CANSparkMax.ControlType.kPosition, 0);
+        
     }
 
     /**
      * Allows for manual control of motor output using the right joystick
      */
     public void manualControl(double speed){
+
+        pidController.setOutputRange(0.1, -0.6);
+
         encoderPosition = (absoluteEncoder.getPosition()*360) - ENCODER_OFFSET;
 
-        //ignore this for now
-        double[] coords = ElevFourbar.getCoords();
+        //TODO: FIX THIS
+        Coordinate coords = ElevFourbar.getCurrentPos();
         double b = ElevFourbar.elevator.getPosition();
-        slope = (coords[1] - b) / coords[0];
+        slope = (coords.y - b) / coords.x;
 
-        bumperIntercept = (slope * ElevFourbar.BUMPER_X) + b;
+        bumperIntercept = (slope * Setpoint.BUMPER_X) + b;
 
-        if(bumperIntercept <= ElevFourbar.BUMPER_Y) {
+        if(bumperIntercept <= Setpoint.BUMPER_Y) {
             if(speed < 0) {
                 speed = 0;
             }
-        } else if(coords[1] <= -3 && Intake.getPivotState() == SolenoidState.UP) {
+        } else if(coords.y <= -3 && Intake.getPivotState() == SolenoidState.UP) {
             if(speed < 0) {
                 speed = 0;
             }
-        } else if(coords[1] <= 21 && Intake.getPivotState() == SolenoidState.DOWN) {
+        } else if(coords.y <= 21 && Intake.getPivotState() == SolenoidState.DOWN) {
             if(speed < 0) {
                 speed = 0;
             }
@@ -163,30 +142,9 @@ public void manualControl(double speed){
         targetSetpoint -= speed * 0.9;
 
         pidController.setReference((targetSetpoint + ENCODER_OFFSET) / 360.0, CANSparkMax.ControlType.kPosition, 1);
-    }
 
-    private void updateTargetSetpoint(Setpoint setpoint) {
-        switch (setpoint) {
-            case SUBSTATION_INTAKE:
-                targetSetpoint = SUBSTATION_INTAKE_SETPOINT;
-                break;
-            case GROUND_INTAKE:
-                if(Intake.getPivotState() == SolenoidState.UP) {
-                    targetSetpoint = GROUND_INTAKE_UP_SETPOINT;
-                } else{
-                    targetSetpoint = GROUND_INTAKE_DOWN_SETPOINT;
-                }
-                break;
-            case MID_SCORING:
-                targetSetpoint = MID_SCORING_SETPOINT;
-                break;
-            case HIGH_SCORING:
-                targetSetpoint = HIGH_SCORING_SETPOINT;
-                break;
-            case STOWED:
-                targetSetpoint = STOWED_SETPOINT;
-                break;
-        }
+        /* speed /= 2;
+        fourbarMotor.set(-speed); */
     }
 
     /**
@@ -203,6 +161,9 @@ public double getTargetPosition() {
         return targetSetpoint;
     }
 
+    /**
+     * @return Position of the absolute encoder from 0 to 1
+     */
     public double getAbsolutePosition() {
         return absoluteEncoder.getPosition();
     }
@@ -219,4 +180,8 @@ public double getBumperIntercept() {
     public double getSlope() {
         return slope;
     }
+
+    public double getPower() {
+        return fourbarMotor.get();
+    }
 }
\ No newline at end of file
diff --git a/src/main/java/frc/robot/subsystems/Pivot.java b/src/main/java/frc/robot/subsystems/Pivot.java
index 5bbf9a9..4199c0e 100644
--- a/src/main/java/frc/robot/subsystems/Pivot.java
+++ b/src/main/java/frc/robot/subsystems/Pivot.java
@@ -3,6 +3,7 @@
 import edu.wpi.first.wpilibj.DoubleSolenoid;
 import edu.wpi.first.wpilibj.PneumaticsModuleType;
 import edu.wpi.first.wpilibj.DoubleSolenoid.Value;
+import frc.robot.systems.Intake.SolenoidState;
 
 public class Pivot {
     private final DoubleSolenoid pivotSolenoid;
@@ -11,11 +12,22 @@ public Pivot(int forwardChannel, int reverseChannel) {
         pivotSolenoid = new DoubleSolenoid(PneumaticsModuleType.CTREPCM, forwardChannel, reverseChannel);
     }
 
+    //Set the pivot to the up position
     public void up() {
         pivotSolenoid.set(Value.kReverse);
     }
 
+    //Set the pivot to the down position
     public void down() {
         pivotSolenoid.set(Value.kForward);
     }
+
+    //Returns the SolenoidState of the claw piston
+    public SolenoidState getState() {
+        if(pivotSolenoid.get() == Value.kForward) {
+            return SolenoidState.DOWN;
+        } else {
+            return SolenoidState.UP;
+        }
+    }
 }
diff --git a/src/main/java/frc/robot/subsystems/Roller.java b/src/main/java/frc/robot/subsystems/Roller.java
index 729d6b9..c639dd6 100644
--- a/src/main/java/frc/robot/subsystems/Roller.java
+++ b/src/main/java/frc/robot/subsystems/Roller.java
@@ -3,10 +3,8 @@
 import edu.wpi.first.wpilibj.PowerDistribution;
 import edu.wpi.first.wpilibj.PowerDistribution.ModuleType;
 import edu.wpi.first.wpilibj.motorcontrol.PWMTalonSRX;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
-import frc.robot.SmartPrintable;
 
-public class Roller extends SmartPrintable {
+public class Roller {
     private static final int PDH_ID = 41;
     private static final double MAX_AMPS = 1000;
 
@@ -21,14 +19,16 @@ public Roller(int rightRollerID) {
     public void setSpeed(double speed) {
         if (pdh.getCurrent(rollerMotor.getChannel()) > MAX_AMPS) {
             speed = 0.1;
-        }
+        } 
 
         rollerMotor.set(speed);
     }
 
-    @Override
-    public void print() {
-        SmartDashboard.putNumber("Roller Amps", pdh.getCurrent(17));
-        SmartDashboard.putNumber("Roller Speed", rollerMotor.get());
+    public double getAmps() {
+        return pdh.getCurrent(17);
+    }
+
+    public double get() {
+        return rollerMotor.get();
     }
 }
diff --git a/src/main/java/frc/robot/subsystems/Setpoint.java b/src/main/java/frc/robot/subsystems/Setpoint.java
new file mode 100644
index 0000000..6a814c0
--- /dev/null
+++ b/src/main/java/frc/robot/subsystems/Setpoint.java
@@ -0,0 +1,179 @@
+package frc.robot.subsystems;
+
+import frc.robot.subsystems.Coordinate;
+
+/**
+ * Class to simplify the use of setpoints so I don't
+ * have to deal with even more methods in ElevFourbar
+ */
+public class Setpoint {
+
+    public static final double HYPOTENUSE = 37.5;
+    public static final double ELEVATOR_MAX = 32;
+
+    public static final double BUMPER_X = 14;
+    public static final double BUMPER_Y = 6.5;
+    
+    //Instance variables
+    private Coordinate coords; //Formatted as [ x, y ]
+    private double elevPos;
+    private double fourbarPos;
+    private boolean isNegative;
+    
+    /**
+     * isNeg forces the fourbar to be angled down (if possible)
+     * just to help with the occasional ambiguity of positions
+     */
+    public Setpoint(double x, double y, boolean isNegative) {
+        this.coords = new Coordinate(x, y);
+        this.isNegative = isNegative;
+
+        double[] pos = toPositions(coords, isNegative);
+        this.elevPos = pos[0];
+        this.fourbarPos = pos[1];
+    }
+
+    /**
+     * Initialize a setpoint without override of position
+     */
+    public Setpoint(double x, double y) {
+        this(x, y, false);
+    }
+
+    public Setpoint(boolean uselessBooleanToMakeThisADifferentConstructor, double e, double f) {
+        this.elevPos = e;
+        this.fourbarPos = f;
+        this.coords = toCoords(e, f);
+    }
+
+    /**
+     * Ew dont use this one at least make use of my hard work :(
+     */
+    /* public Setpoint(double elevPos, double fourbarPos) {
+        this.elevPos = elevPos;
+        this.fourbarPos = fourbarPos;
+
+        this.coords = toCoords(elevPos, fourbarPos);
+        this.isNegative = false;
+    } */
+
+    /**
+     * @return ( x, y ) coordinates of the setpoint
+     */
+    public Coordinate getCoords() {
+        return coords;
+    }
+
+    /**
+     * @return elevator position of the setpoint in inches
+     */
+    public double getElevPos() {
+        return elevPos;
+    }
+
+    /**
+     * @return fourbar position of the setpoint in degrees
+     */
+    public double getFourbarPos() {
+        return fourbarPos;
+    }
+
+    /**
+     * @return true if the fourbar is forced to angle down
+     */
+    public boolean getIsNegative() {
+        return isNegative;
+    }
+
+    public void setElevPos(double e) {
+        elevPos = e;
+    }
+
+    public void setFourbarPos(double f) {
+        fourbarPos = f;
+    }
+
+    public void translate(Coordinate c) {
+
+    }
+
+    /**
+     * @param elevPos
+     * @param fourbarPos
+     * @return Coordinates resulting from the inputted positions
+     */
+    public static Coordinate toCoords(double elevPos, double fourbarPos) {
+
+        double x;
+        double y;
+
+        x = Math.sin(Math.toRadians(fourbarPos)) * HYPOTENUSE;
+        y = Math.cos(Math.toRadians(fourbarPos)) * HYPOTENUSE + elevPos;
+        
+        return new Coordinate(x, y);
+    }
+
+    /**
+     * This thing works now!!
+     * If it doesnt return the correct values, try modifying 
+     * the bumper coordinates or the elevator max
+     * @param coord x, y coordinates to convert from
+     * @param isNegative true if the fourbar needs to be forced to angle down
+     * @return Positions in the format of { elevator, fourbar }
+     */
+    public static double[] toPositions(Coordinate coord, boolean isNegative) {
+
+        double x = coord.x;
+        double y = coord.y;
+        
+        double e;
+        double f;
+
+        //JESUS CHRIST WHY WAS THIS PART SO HARD
+        double adj = Math.sqrt((Math.pow(HYPOTENUSE, 2) - Math.pow(x, 2)));
+
+        double uprightHeight = y - adj;
+        double downHeight = y + adj;
+        
+        if((approx(uprightHeight, 0, 0.000001) || uprightHeight > 0) && uprightHeight < ELEVATOR_MAX && !isNegative) {
+            e = clamp(uprightHeight, 0, ELEVATOR_MAX);
+        } else {
+            e = clamp(downHeight, 0, ELEVATOR_MAX);
+        }
+
+        /* e = (y >= adj || approx(y, adj, 0.0001)) && !isNegative 
+            ? y - adj 
+            : y + adj;
+
+        e = clamp(e, 0, ELEVATOR_MAX); */
+        
+        //atan2 my beloved <3
+        f = Math.toDegrees(Math.atan2(x, y - e));
+
+        //Limit the angle so we dont break the bumper on accident
+        double maxAngle = Math.toDegrees(Math.atan2(BUMPER_X, BUMPER_Y - e));
+        f = clamp(f, 0, maxAngle);
+
+        double[] res = { e, f };
+        return res;
+    }
+
+    /**
+     * Used to clamp the range of variables so I don't bend
+     * any more steel hex shafts :)
+     */
+    private static double clamp(double val, double min, double max) {
+        return Math.max(min, Math.min(max, val));
+    }
+
+    /**
+     * 
+     * @param a
+     * @param b
+     * @param sensitivity
+     * @return true if the difference between a and b is less than sensitivity
+     */
+    private static boolean approx(double a, double b, double sensitivity) {
+        return Math.abs(a - b) < sensitivity;
+    }
+}
diff --git a/src/main/java/frc/robot/subsystems/SwerveMode.java b/src/main/java/frc/robot/subsystems/SwerveMode.java
index 278e52a..790fb8c 100644
--- a/src/main/java/frc/robot/subsystems/SwerveMode.java
+++ b/src/main/java/frc/robot/subsystems/SwerveMode.java
@@ -9,11 +9,16 @@ public enum SwerveMode {
      * Moves relative to the driver station, pressing the joystick away from
      * you should always move the robot away from you.
      */
-    Headless,
+    HEADLESS,
 
     /**
      * Pressing the joystick forward moves the robot froward, regardless of 
      * position relative to driver.
      */
-    Relative
+    RELATIVE,
+
+    /**
+     * Rock mode, resists movement and holds position.
+     */
+    ROCK
 }
diff --git a/src/main/java/frc/robot/subsystems/SwerveModule.java b/src/main/java/frc/robot/subsystems/SwerveModule.java
index 0856d39..848bff3 100644
--- a/src/main/java/frc/robot/subsystems/SwerveModule.java
+++ b/src/main/java/frc/robot/subsystems/SwerveModule.java
@@ -6,7 +6,7 @@
 
 import com.ctre.phoenix.sensors.CANCoder;
 import com.ctre.phoenix.sensors.SensorInitializationStrategy;
-
+import com.ctre.phoenix.sensors.SensorTimeBase;
 import com.revrobotics.CANSparkMax;
 import com.revrobotics.CANSparkMax.IdleMode;
 import com.revrobotics.CANSparkMaxLowLevel.MotorType;
@@ -14,27 +14,29 @@
 
 import edu.wpi.first.math.controller.PIDController;
 import edu.wpi.first.math.geometry.Rotation2d;
+import edu.wpi.first.math.geometry.Translation2d;
 import edu.wpi.first.math.kinematics.SwerveModulePosition;
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
 import frc.robot.SmartPrintable;
 
 /** 
  * Class for managing and manipulating a swerve module. 
  */
 public class SwerveModule extends SmartPrintable {
-    private static final double CONVERSION_FACTOR_ROTATION = 150 / 7;                                         // Rotations to degrees.
+    private static final double CONVERSION_FACTOR_ROTATION = Math.toRadians(150 / 7);                         // Rotations to radians.
     private static final double CONVERSION_FACTOR_MOVEMENT = 6.75;                                            // Rotations to meters.
-    private static final double CONVERSION_FACTOR_ROTATION_VELOCITY = CONVERSION_FACTOR_ROTATION * (1 / 60);  // RPM to degrees per second.
+    private static final double CONVERSION_FACTOR_ROTATION_VELOCITY = CONVERSION_FACTOR_ROTATION * (1 / 60);  // RPM to radians per second.
     private static final double CONVERSION_FACTOR_MOVEMENT_VELOCITY = CONVERSION_FACTOR_MOVEMENT * (1 / 60);  // RPM to meters per second.
 
-    private static double PID_P = 0.01;
-    private static double PID_I = 0.0;
-    private static double PID_D = 0.0;
+    private static final double PID_P = 0.5;
+    private static final double PID_I = 0.0;
+    private static final double PID_D = 0.0;
 
-    private static double ROCK_PID_P = 0.1;
-    private static double ROCK_PID_I = 0.0;
-    private static double ROCK_PID_D = 0.0;
+    private static final double ROCK_PID_P = 0.1;
+    private static final double ROCK_PID_I = 0.0;
+    private static final double ROCK_PID_D = 0.0;
 
     private final SwerveModulePosition position;
 
@@ -48,25 +50,80 @@ public class SwerveModule extends SmartPrintable {
     private final PIDController rotationController;
     private final PIDController rockController;
 
-    private final double canCoderOffset;
+    private final RelativePosition physicalPosition;
+    private final Angle canCoderOffset;
     private final double coefficient;
 
-    /**
-     * Set to NaN if not in rock mode, NaN does not equal itself by definition
-     * (see some IEEE standard or something) and so this is how rock mode is 
-     * checked.
-     */
-    private double rockPos;
+    // Set to NaN if not in rock mode, NaN does not equal itself by definition
+    // (see some IEEE standard or something) and so this is how rock mode is 
+    // checked. Max speed works the same way.
+    private double rockPos = Double.NaN;
+    private double maxSpeed = Double.NaN;
+
+    private enum RelativePosition {
+        FRONT_RIGHT (  1.0,  1.0 ),
+        FRONT_LEFT  ( -1.0,  1.0 ),
+        BACK_LEFT   ( -1.0, -1.0 ),
+        BACK_RIGHT  (  1.0, -1.0 );
+
+        private boolean front = true;
+        private boolean right = true;
+
+        RelativePosition(double x, double y) {
+            right = Math.signum(x) > 0.0;
+            front = Math.signum(y) > 0.0;
+        }
+
+        public String asString() {
+            String str = "";
+            
+            if (front) {
+                str += "Front ";
+            } else {
+                str += "Back ";
+            }
+
+            if (right) {
+                str += "Right";
+            } else {
+                str += "Left";
+            }
+
+            return str;
+        }
+
+        public static RelativePosition fromTranslation(Translation2d translation) {
+            var x_sign = Math.signum(translation.getX()) > 0.0;
+            var y_sign = Math.signum(translation.getY()) > 0.0;
+
+            if (x_sign && y_sign) {
+                return FRONT_RIGHT;
+            } else if (x_sign) {
+                return BACK_RIGHT;
+            } else if (y_sign) {
+                return FRONT_LEFT;
+            } 
+                
+            return BACK_LEFT;
+        }
+    }
 
-    public SwerveModule(int movementMotorID, int rotationalMotorID, int canCoderID, double canCoderOffset, double coefficient) {
+    public SwerveModule(
+        int movementMotorID, 
+        int rotationalMotorID, 
+        int canCoderID, 
+        Angle canCoderOffset, 
+        double coefficient,
+        Translation2d physicalPosition
+    ) {
         super();
         
-        this.canCoderOffset = canCoderOffset;
+        this.physicalPosition = RelativePosition.fromTranslation(physicalPosition);
+        this.canCoderOffset = canCoderOffset.clone();
         this.coefficient = coefficient;
 
         rotationMotor = new CANSparkMax(rotationalMotorID, MotorType.kBrushless);
         rotationMotor.setInverted(false);
-        //rotationMotor.setVoltage(11.0);
         rotationMotor.setSmartCurrentLimit(30);
         
         movementMotor = new CANSparkMax(movementMotorID, MotorType.kBrushless);
@@ -76,7 +133,18 @@ public SwerveModule(int movementMotorID, int rotationalMotorID, int canCoderID,
 
         angularEncoder = new CANCoder(canCoderID);
         angularEncoder.configFactoryDefault();
-        angularEncoder.configSensorInitializationStrategy(SensorInitializationStrategy.BootToAbsolutePosition, 1000);
+
+        // Magic and forbidden config from the code orange wizards. Makes the 
+        // encoder initialize to absolute.
+        angularEncoder.configSensorInitializationStrategy(SensorInitializationStrategy.BootToAbsolutePosition);
+        angularEncoder.configFeedbackCoefficient(
+            // Since the default coefficiant used for degrees is not 
+            // particularly intuitive we just grab it and run a deg -> rad
+            // conversion on it.
+            Math.toRadians(angularEncoder.configGetFeedbackCoefficient()), 
+            "rad", 
+            SensorTimeBase.PerSecond
+        );
 
         rotationEncoder = rotationMotor.getEncoder();
         rotationEncoder.setPosition(angularEncoder.getPosition());
@@ -89,33 +157,37 @@ public SwerveModule(int movementMotorID, int rotationalMotorID, int canCoderID,
         movementEncoder.setVelocityConversionFactor(CONVERSION_FACTOR_MOVEMENT_VELOCITY);
         
         rotationController = new PIDController(PID_P, PID_I, PID_D);
-        rotationController.enableContinuousInput(0.0, 360.0);
-        rotationController.setTolerance(0.5);
+        rotationController.enableContinuousInput(0.0, Angle.TAU);
+        rotationController.setTolerance(0.005); // This is more precise than before, TODO: test.
 
         rockController = new PIDController(ROCK_PID_P, ROCK_PID_I, ROCK_PID_D);
         rockController.setTolerance(0.5);
 
-        position = new SwerveModulePosition(movementEncoder.getPosition() * CONVERSION_FACTOR_MOVEMENT, new Rotation2d(angularEncoder.getPosition() / 180.0 * Math.PI));
+        position = new SwerveModulePosition(
+            movementEncoder.getPosition() * CONVERSION_FACTOR_MOVEMENT, 
+            new Rotation2d(angularEncoder.getPosition())
+        );
     }
 
     /**
-     * Sets the desired module state for this module.
+     * Sets the desired module state for this module. This must be run 
+     * repeatedly to continue PID calculations.
      */
     public void setDesiredState(SwerveModuleState state) {
-        /* if (Math.abs(state.speedMetersPerSecond) < 0.001) {
-            movementMotor.set(0.0);
-            rotationMotor.set(0.0);
-            return;
-        } */
-        
-        double currentPosition = (angularEncoder.getPosition() + canCoderOffset) % 360.0;
-        state = SwerveModuleState.optimize(state, new Rotation2d(currentPosition * Math.PI / 180.0));
-        double calculation = rotationController.calculate(currentPosition, (state.angle.getDegrees() + 360.0) % 360.0);
+        double currentPosition = (angularEncoder.getPosition() + canCoderOffset.radians()) % Angle.TAU;
+        state = SwerveModuleState.optimize(state, new Rotation2d(currentPosition));
         
         if (rockPos != rockPos) {
-            movementMotor.set(state.speedMetersPerSecond);
+            movementMotor.set(
+                Math.abs(state.speedMetersPerSecond) > maxSpeed 
+                    ? Math.signum(state.speedMetersPerSecond) * maxSpeed 
+                    : state.speedMetersPerSecond
+            );
+        } else {
+            movementMotor.set(rockController.calculate(getDistanceTraveled(), rockPos));
         }
 
+        double calculation = rotationController.calculate(currentPosition, (state.angle.getRadians() + Angle.TAU) % Angle.TAU);
         rotationMotor.set(calculation * coefficient);
 
         position.angle = new Rotation2d(angularEncoder.getPosition());
@@ -129,45 +201,55 @@ public void setRockMode(boolean shouldHold) {
         if (!shouldHold) {
             rockPos = Double.NaN;
             return;
+        } else if (rockPos != rockPos) {
+            rockPos = getDistanceTraveled();
         }
-
-        if (shouldHold && rockPos != rockPos) {
-            rockPos = getPosition();
-        }
-
-        movementMotor.set(rockController.calculate(getPosition(), rockPos));
     }
 
     /**
      * True if in rock mode.
      */
-    public boolean getRockMode() {
+    public boolean inRockMode() {
         return rockPos == rockPos;
     }
 
+    /**
+     * Sets this module's maximum movement speed. Use NaN for no limit.
+     */
+    public void setMaxSpeed(double maxSpeed) {
+        this.maxSpeed = maxSpeed;
+    }
+
+    /**
+     * Gets this module's maximum movement speed
+     */
+    public double getMaxSpeed() {
+        return maxSpeed;
+    }
+
     /**
      * Gets distance traveled, should not be used for ablsolute distances as 
      * this function currently makes no guarantee as to the starting position
      * of the module. (This can be mitigated if you zero positions, but it will
      * interupt odometry).
      */
-    public double getPosition() {
+    public double getDistanceTraveled() {
         return movementEncoder.getPosition();
     }
 
     @Override
     public void print() {
-        SmartDashboard.putNumber("Module " + angularEncoder.getDeviceID() + " Position", angularEncoder.getPosition());
-        SmartDashboard.putNumber("Module " + angularEncoder.getDeviceID() + " Position mod 360", angularEncoder.getPosition() % 360);
-        SmartDashboard.putNumber("Module " + angularEncoder.getDeviceID() + " Position + off", angularEncoder.getPosition() + canCoderOffset);
-        SmartDashboard.putNumber("Module " + angularEncoder.getDeviceID() + " Position + off mod 360", (angularEncoder.getPosition() + canCoderOffset) % 360);
-        SmartDashboard.putNumber("Module " + angularEncoder.getDeviceID() + " Position (Distance) ", movementEncoder.getPosition());
+        SmartDashboard.putNumber("Module " + physicalPosition.asString() + " Position", angularEncoder.getPosition());
+        SmartDashboard.putNumber("Module " + physicalPosition.asString() + " Position mod tau", angularEncoder.getPosition() % Angle.TAU);
+        SmartDashboard.putNumber("Module " + physicalPosition.asString() + " Position + off", angularEncoder.getPosition() + canCoderOffset.radians());
+        SmartDashboard.putNumber("Module " + physicalPosition.asString() + " Position + off mod tau", (angularEncoder.getPosition() + canCoderOffset.radians()) % Angle.TAU);
+        SmartDashboard.putNumber("Module " + physicalPosition.asString() + " Position (Distance) ", movementEncoder.getPosition());
     }
 
     /**
      * Gets the angle of the module. 
      */
-    public SwerveModulePosition getAngle() {
+    public SwerveModulePosition getPosition() {
         return position;
     }
 
diff --git a/src/main/java/frc/robot/systems/AutoBalance.java b/src/main/java/frc/robot/systems/AutoBalance.java
index 4dd589b..5a707b2 100644
--- a/src/main/java/frc/robot/systems/AutoBalance.java
+++ b/src/main/java/frc/robot/systems/AutoBalance.java
@@ -33,7 +33,6 @@ public enum Stage {
         /** 
          * auto balance is ramming, meant to dock the charge station. 
          */
-
         RAMMING,
 
         /** 
@@ -141,7 +140,7 @@ private Stage adjust() {
             return Stage.PAUSED;
         }
 
-        SwerveDrive.runUncurved(0.0, Math.signum(Pigeon.getPitch()) * HALTING_SPEED, 0.0);
+        SwerveDrive.runUncurved(0.0, Math.signum(Pigeon.getPitch().degrees()) * HALTING_SPEED, 0.0);
         return Stage.ADJUSTING;
     }
 
@@ -160,7 +159,7 @@ private Stage pause() {
      * Returns true if auto balance beleives it is balanced, not 100% accurate.
      */
     private boolean level() {
-        return Math.abs(Pigeon.getPitch()) < 10.0 && Math.abs(Pigeon.getChangePerSecond().pitchPerSecond) < 10.0;
+        return Math.abs(Pigeon.getPitch().degrees()) < 10.0 && Math.abs(Pigeon.getChangePerSecond().pitchPerSecond) < 10.0;
     }
 
     /**
diff --git a/src/main/java/frc/robot/systems/AutonomousRunner.java b/src/main/java/frc/robot/systems/AutonomousRunner.java
index bca9a66..5128569 100644
--- a/src/main/java/frc/robot/systems/AutonomousRunner.java
+++ b/src/main/java/frc/robot/systems/AutonomousRunner.java
@@ -4,9 +4,11 @@
 
 import edu.wpi.first.wpilibj.Timer;
 import edu.wpi.first.wpilibj.smartdashboard.*;
+
 import frc.robot.systems.AutoBalance.BalanceType;
-import frc.robot.systems.ElevFourbar.Setpoint;
+import frc.robot.systems.ElevFourbar.GamePiece;
 import frc.robot.systems.Intake.SolenoidState;
+import frc.robot.subsystems.DoubleSetpoint;
 import frc.robot.SmartPrintable;
 
 public class AutonomousRunner extends SmartPrintable {
@@ -15,13 +17,13 @@ public class AutonomousRunner extends SmartPrintable {
 
     private static final Runnable[] MODES = {
         //() -> {},
-        AutonomousRunner::modeSeven,
         AutonomousRunner::modeOne,
         AutonomousRunner::modeTwo,
         AutonomousRunner::modeThree,
         AutonomousRunner::modeFour,
         AutonomousRunner::modeFive,
         AutonomousRunner::modeSix,
+        AutonomousRunner::modeSeven,
         () -> {},
     };
 
@@ -66,8 +68,6 @@ public static void init() {
     }
     
     public static void run() {
-        SwerveDrive.setRockMode(false);
-
         SmartDashboard.putNumber("Auto Stage", instance.autoStage);
 
         //Run the auto mode
@@ -75,12 +75,9 @@ public static void run() {
     }
 
     /**
-     * Scores mid then moves out of community
+     * Only move out of the community
      */
     private static void modeOne() {
-
-        score(Setpoint.MID_SCORING);
-        
         /*******************
          * EXIT THE COMUNITY
          * *****************    
@@ -96,28 +93,15 @@ private static void modeOne() {
      * Score only mid
      */
     private static void modeTwo() {
-        score(Setpoint.MID_SCORING);
+        score(ElevFourbar.MID_SCOORING_SETPOINT);
     }
 
     /**
-     * Score mid then auto balance
+     * Scores mid then moves out of community
      */
     private static void modeThree() {
 
-        score(Setpoint.MID_SCORING);
-        
-        if (instance.autoStage > 1 || instance.timer.get() > 6) {
-            AutoBalance.run();
-        }
-    }
-
-    /**
-     * Score high then move out of the community
-     */
-    private static void modeFour() {
-
-        score(Setpoint.HIGH_SCORING);
-
+        score(ElevFourbar.MID_SCOORING_SETPOINT);
         
         /*******************
          * EXIT THE COMUNITY
@@ -130,48 +114,61 @@ private static void modeFour() {
         }
     }
 
+    /**
+     * Score mid then auto balance
+     */
+    private static void modeFour() {
+
+        score(ElevFourbar.MID_SCOORING_SETPOINT);
+        
+        if (instance.autoStage > 1 || instance.timer.get() > 6) {
+            AutoBalance.run();
+        }
+    }
+
     /**
      * Only score high
      */
     private static void modeFive() {
-        score(Setpoint.HIGH_SCORING);
+        score(ElevFourbar.HIGH_SCORING_SETPOINT);
     }
 
     /**
-     * Score high and auto balance
+     * Score high then move out of the community
      */
     private static void modeSix() {
-        score(Setpoint.HIGH_SCORING);
+
+        score(ElevFourbar.HIGH_SCORING_SETPOINT);
+
         
-        if (instance.autoStage > 1 || instance.timer.get() > 6) {
-            AutoBalance.run();
+        /*******************
+         * EXIT THE COMUNITY
+         * *****************    
+         */
+        if (SwerveDrive.getDistance() < 670 && (instance.autoStage == 2 || instance.timer.get() > 10)) {
+            SwerveDrive.runUncurved(0.0, -0.6, 0.0);
+        } else {
+            SwerveDrive.runUncurved(0.0, 0.0, 0.0);
         }
     }
 
     /**
-     * Only drop game piece and move out of the community
+     * Score high and auto balance
      */
     private static void modeSeven() {
-        score(Setpoint.HIGH_SCORING);
+        score(ElevFourbar.HIGH_SCORING_SETPOINT);
         
         if (instance.autoStage > 1 || instance.timer.get() > 6) {
-            AutoBalance.setType(BalanceType.OVER_AND_BACK);
             AutoBalance.run();
         }
     }
 
-    private static void score(Setpoint setpoint) {
+    private static void score(DoubleSetpoint setpoint) {
         if(instance.autoStage == 0){
             //Move the pivot up
             Intake.autoPivot(SolenoidState.UP);
-            if(setpoint == Setpoint.HIGH_SCORING) 
-                ElevFourbar.autoRun((ElevFourbar.getGamePieceSelected() == ElevFourbar.GamePiece.CUBE) ? ElevFourbar.HIGH_SCORING_COORDS_CUBE : ElevFourbar.HIGH_SCORING_COORDS_CONE);
-
-            else if(setpoint == Setpoint.MID_SCORING)
-                ElevFourbar.autoRun((ElevFourbar.getGamePieceSelected() == ElevFourbar.GamePiece.CUBE) ? ElevFourbar.MID_SCORING_COORDS_CUBE : ElevFourbar.MID_SCORING_COORDS_CONE);
-
-            else
-                ElevFourbar.autoRun(setpoint);
+            
+            ElevFourbar.autoRun(ElevFourbar.getGamePieceSelected() == GamePiece.CUBE ? setpoint.cube : setpoint.cone);
             
             //Move the elevator to the high scoring position
             if(instance.timer.get() > 1.0) {
@@ -185,7 +182,7 @@ else if(setpoint == Setpoint.MID_SCORING)
             //1 second delay to prevent closing on the cube again >:(
             if(instance.secondaryTimer.get() > 1) {
                 //Move to stowed setpoint
-                if(ElevFourbar.autoRun(ElevFourbar.STOWED_COORDS_CUBES)) {
+                if(ElevFourbar.autoRun(ElevFourbar.STOWED_SETPOINT)) {
                     //Close the claw and put the pivot down
                     instance.autoStage++;
                 }
diff --git a/src/main/java/frc/robot/systems/ElevFourbar.java b/src/main/java/frc/robot/systems/ElevFourbar.java
index 8fc739b..4e48386 100644
--- a/src/main/java/frc/robot/systems/ElevFourbar.java
+++ b/src/main/java/frc/robot/systems/ElevFourbar.java
@@ -1,36 +1,28 @@
 package frc.robot.systems;
 
+import java.text.DecimalFormat;
+
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
 import frc.robot.SmartPrintable;
+import frc.robot.systems.Intake.SolenoidState;
+import frc.robot.subsystems.DoubleSetpoint;
+import frc.robot.subsystems.Coordinate;
 import frc.robot.subsystems.Elevator;
+import frc.robot.subsystems.Setpoint;
 import frc.robot.subsystems.Fourbar;
-import frc.robot.systems.Intake.SolenoidState;
-
-import java.text.DecimalFormat;
+import frc.robot.SmartPrintable;
 
 public class ElevFourbar extends SmartPrintable {
 
-    //Constants for trig functions
-    private static final double FOURBAR_HYPOTENUSE = 37.5;
-    private static final double ELEVATOR_MAX_POS = 32.0;
-    public static final double BUMPER_X = 14;
-    public static final double BUMPER_Y = 8;
-
-    //COORDINATE CONSTANTS FOR PID CONTROL
-    public static double[] STOWED_COORDS_CUBES = { 0, FOURBAR_HYPOTENUSE};
-    public static double[] STOWED_COORDS_CONE = STOWED_COORDS_CUBES;
-    public static double[] GROUND_INTAKE_DOWN_COORDS = { 35.2, 15 -4};
-    public static double[] GROUND_INTAKE_UP_COORDS = { 30, 1 };
-    public static double[] MID_SCORING_COORDS_CONE = { 15, 34 };
-    public static double[] MID_SCORING_COORDS_CUBE = { 15, 37 };
-    public static double[] SUBSTATION_INTAKE_COORDS = { 20.4, 40.5 };
-    public static double[] HIGH_SCORING_COORDS_CONE = { 33.194, 48.581 };
-    public static double[] HIGH_SCORING_COORDS_CUBE = { 26.6, 50 };
-
-    //enums
-    private Setpoint setpoint = Setpoint.STOWED;
-    private ControlType controlType = ControlType.POSITION;
-    public static GamePiece gamePieceSelected = GamePiece.CUBE;
+    //SETPOINTS FOR PID CONTROL
+    public static final DoubleSetpoint STOWED_SETPOINT = new DoubleSetpoint(new Setpoint(0, Setpoint.HYPOTENUSE), new Setpoint(0, Setpoint.HYPOTENUSE));
+    public static final DoubleSetpoint MID_SCOORING_SETPOINT = new DoubleSetpoint(new Setpoint(15, 37), new Setpoint(15, 34.4));
+    public static final DoubleSetpoint HIGH_SCORING_SETPOINT = new DoubleSetpoint(new Setpoint(26.6, 50), new Setpoint(33.2, 48.6));
+
+    public static final Setpoint GROUND_INTAKE_DOWN_SETPOINT = new Setpoint(35.2, 11, true);
+    public static final Setpoint GROUND_INTAKE_UP_SETPOINT = new Setpoint(30, 1, true);
+    public static final Setpoint SUBSTATION_INTAKE_SETPOINT = new Setpoint(20.4, 40.5);
 
     //deadzone to determine when manual control is enabled
     static final double DEADZONE = 0.3;
@@ -38,22 +30,21 @@ public class ElevFourbar extends SmartPrintable {
     //make a decimal format object to improve readability of coordinates
     private static DecimalFormat df = new DecimalFormat("#.###");
 
-    //instantiate coordinates
-    private double[] coords = {0, 0};
-    private double[] targetCoords = { 0, FOURBAR_HYPOTENUSE };
-
-    private static ElevFourbar instance = new ElevFourbar();
+    //Instance veriables
+    private Setpoint setpoint;
+    private Setpoint currentSetpoint;
+    private ControlType controlType = ControlType.POSITION;
+    private GamePiece gamePieceSelected = GamePiece.CUBE;
+    private boolean translateMode = false;
 
+    //Elevator and Fourbar objects
     public static Fourbar fourbar;
     public static Elevator elevator;
 
-    public ElevFourbar() {
-        super();
-        fourbar = new Fourbar();
-        elevator = new Elevator();
-    }
+    //Instance of this class
+    private static ElevFourbar instance = new ElevFourbar();
 
-    public static enum Setpoint {
+    public static enum SetpointEnum {
         SUBSTATION_INTAKE, GROUND_INTAKE, MID_SCORING, HIGH_SCORING, STOWED
     }
 
@@ -65,136 +56,100 @@ public static enum GamePiece {
         CONE, CUBE
     }
 
+    /**
+     * Literally just a constructor
+     */
+    public ElevFourbar() {
+        super();
+        fourbar = new Fourbar();
+        elevator = new Elevator();
+        currentSetpoint = new Setpoint(0, Setpoint.HYPOTENUSE);
+        setpoint = STOWED_SETPOINT.cone;
+    }
+
+    /**
+     * Initialize teleop
+     */
     public static void init() {
         instance.controlType = ControlType.POSITION;
         elevator.init();
         fourbar.setPIDSpeed(0.45);
-    }
+        instance.translateMode = false;
 
-    public static void setSetPoint(Setpoint set) {
-        instance.controlType = ControlType.POSITION;
-        instance.setpoint = set;
-
-        if (set == Setpoint.STOWED) {
-            instance.targetCoords = (gamePieceSelected == GamePiece.CONE) ? STOWED_COORDS_CONE : STOWED_COORDS_CUBES;
-        } else if (set == Setpoint.GROUND_INTAKE) {
-            instance.setpoint = Setpoint.GROUND_INTAKE;
-            if (Intake.getPivotState() == SolenoidState.UP) {
-                instance.targetCoords = GROUND_INTAKE_UP_COORDS;
-            } else {
-                instance.targetCoords = GROUND_INTAKE_DOWN_COORDS;
-            }
-        } else if (set == Setpoint.MID_SCORING) {
-            instance.targetCoords = (gamePieceSelected == GamePiece.CONE ? MID_SCORING_COORDS_CONE : MID_SCORING_COORDS_CUBE);
-        } else if(set == Setpoint.HIGH_SCORING) {
-            instance.targetCoords = (gamePieceSelected == GamePiece.CONE ? HIGH_SCORING_COORDS_CONE : HIGH_SCORING_COORDS_CUBE);
-        } 
+        instance.setpoint = getGamePieceSelected() == GamePiece.CUBE ? STOWED_SETPOINT.cube : STOWED_SETPOINT.cone;
     }
-
-    public static ControlType getControlType() {
-        return instance.controlType;
+    
+    /**
+     * Initialize autonomous
+     */
+    public static void autonomousInit() {
+        elevator.autonomousInit();
+        fourbar.setPIDSpeed(0.3);
     }
 
-    public static void run(double elevatorSpeed, double fourbarSpeed, int dPadDirection, boolean toggleGamePieceMode, boolean zeroElevEncoder) {
-        
-        instance.coords = posToCoords(elevator.getPosition(), fourbar.getPosition());
+    public static void run(double elevatorSpeed, double fourbarSpeed, int dPadDirection, boolean toggleGamePieceMode, boolean groundIntake, boolean mid, boolean high, boolean zeroElevEncoder) {
+        instance.currentSetpoint = new Setpoint(false, elevator.getPosition(), fourbar.getPosition());
 
         //toggle between cone and cube mode
         toggleGamePiece(toggleGamePieceMode);
 
-        if(instance.setpoint == Setpoint.GROUND_INTAKE) {
-            if(Intake.getPivotState() == SolenoidState.UP) {
-                instance.targetCoords = GROUND_INTAKE_UP_COORDS;
-            } else {
-                instance.targetCoords = GROUND_INTAKE_DOWN_COORDS;
-            }
-        }
+        //set the target setpoint based on which button is pressed
+        setSetPoint(groundIntake, mid, high);
 
         //switches between control modes when button is pressed or manual control detects input
         if(Math.abs(elevatorSpeed) > DEADZONE || Math.abs(fourbarSpeed) > DEADZONE) {
             instance.controlType = ControlType.MANUAL;
-        }
+        } else if(mid || groundIntake || high) {
+            instance.controlType = ControlType.POSITION;
+        } 
 
-        if(instance.controlType == ControlType.POSITION) {
-            /* elevator.pidControl(instance.setpoint);
-            fourbar.pidControl(instance.setpoint); */
+        if(instance.controlType == ControlType.POSITION) { //Run PID control
+            elevator.pidControl(instance.setpoint);
+            fourbar.pidControl(instance.setpoint);
+        } else { //Run manual control
+            if(instance.translateMode) {
 
-            elevator.pidControl(instance.targetCoords);
-            fourbar.pidControl(instance.targetCoords);
-        } else {
-            elevator.manualControl(elevatorSpeed, dPadDirection);
-            fourbar.manualControl(fourbarSpeed);
+            } else {
+
+                elevator.manualControl(elevatorSpeed, dPadDirection);
+                fourbar.manualControl(fourbarSpeed);
+
+            }
         }
 
+
         if (zeroElevEncoder) {
-            elevator.zeroEncoder();
+            elevator.zeroEncoder(0.5);
         }
+
+        SmartDashboard.putNumber("FB Power", fourbar.getPower());
     }
 
     /**
-     * 
-     * @param toggleGamePieceMode
-     * @param groundIntake
+     * Set the current setpoint based on which button is held
+     * @param ground
      * @param mid
      * @param high
-     * @param stowed
      */
-    public static void run(double elevatorSpeed, double fourbarSpeed, int dPadDirection, boolean toggleGamePieceMode, boolean groundIntake, boolean mid, boolean high, boolean stowed, boolean zeroElevEncoder) {
-        
-        instance.coords = posToCoords(elevator.getPosition(), fourbar.getPosition());
+    public static void setSetPoint(boolean ground, boolean mid, boolean high) {
 
-        //toggle between cone and cube mode
-        toggleGamePiece(toggleGamePieceMode);
-
-        //set the setpoint depending on which button is pressed
-        if(stowed) {
-            instance.setpoint = Setpoint.STOWED;
-            instance.targetCoords = (gamePieceSelected == GamePiece.CONE) ? STOWED_COORDS_CONE : STOWED_COORDS_CUBES;
-        } else if(groundIntake) {
-            instance.setpoint = Setpoint.GROUND_INTAKE;
+        if(ground) {
+            instance.setpoint = Intake.getPivotState() == SolenoidState.DOWN 
+                ? GROUND_INTAKE_DOWN_SETPOINT 
+                : GROUND_INTAKE_UP_SETPOINT;
         } else if(mid) {
-            instance.setpoint = Setpoint.MID_SCORING;
-            instance.targetCoords = (gamePieceSelected == GamePiece.CONE ? MID_SCORING_COORDS_CONE : MID_SCORING_COORDS_CUBE);
+            instance.setpoint = getGamePieceSelected() == GamePiece.CUBE 
+                ? MID_SCOORING_SETPOINT.cube 
+                : MID_SCOORING_SETPOINT.cone;
         } else if(high) {
-            instance.setpoint = Setpoint.HIGH_SCORING;
-            instance.targetCoords = (gamePieceSelected == GamePiece.CONE ? HIGH_SCORING_COORDS_CONE : HIGH_SCORING_COORDS_CUBE);
-        } 
-
-        if(instance.setpoint == Setpoint.GROUND_INTAKE) {
-            if(Intake.getPivotState() == SolenoidState.UP) {
-                instance.targetCoords = GROUND_INTAKE_UP_COORDS;
-            } else {
-                instance.targetCoords = GROUND_INTAKE_DOWN_COORDS;
-            }
-        }
-
-        //switches between control modes when button is pressed or manual control detects input
-        if(groundIntake || mid || high || stowed) {
-            instance.controlType = ControlType.POSITION;
-        } else if(Math.abs(elevatorSpeed) > DEADZONE || Math.abs(fourbarSpeed) > DEADZONE) {
-            instance.controlType = ControlType.MANUAL;
-        } 
-
-        if(instance.controlType == ControlType.POSITION) {
-            /* elevator.pidControl(instance.setpoint);
-            fourbar.pidControl(instance.setpoint); */
-
-            elevator.pidControl(instance.targetCoords);
-            fourbar.pidControl(instance.targetCoords);
+            instance.setpoint = getGamePieceSelected() == GamePiece.CUBE 
+                ? HIGH_SCORING_SETPOINT.cube 
+                : HIGH_SCORING_SETPOINT.cone;
         } else {
-            elevator.manualControl(elevatorSpeed, dPadDirection);
-            fourbar.manualControl(fourbarSpeed);
+            instance.setpoint = getGamePieceSelected() == GamePiece.CUBE
+                ? STOWED_SETPOINT.cube 
+                : STOWED_SETPOINT.cone;
         }
-
-        if (zeroElevEncoder) {
-            elevator.zeroEncoder();
-        }
-    }
-    /**Toggles between cube and cone mode when button pressed
-     * @param buttonReleased The button to toggle the game piece with (use a get button released method)
-     */
-    public static void toggleGamePiece(boolean buttonReleased) {
-        gamePieceSelected = (buttonReleased ? (gamePieceSelected == GamePiece.CONE ? GamePiece.CUBE : GamePiece.CONE) : gamePieceSelected);
     }
 
     /**
@@ -210,128 +165,58 @@ public static boolean autoRun(Setpoint setpoint) {
         //return true if the fourbar reached it's destination
         return (Math.abs(fourbar.getPosition() - fourbar.getTargetPosition()) < 1) && (Math.abs(elevator.getPosition() - elevator.getTargetPosition()) < 0.2);
     }
-    
-    /**
-     * Sets elevator and fourbar to desired setpoint
-     * @param setpoint The setpoint to run to from the Setpoint enum
-     */
-    public static boolean autoRun(double[] coords) {
 
-        //Run the fourbar and elevator to inputted setpoint
-        elevator.pidControl(coords);
-        fourbar.pidControl(coords);
-
-        //return true if the fourbar reached it's destination
-        return (Math.abs(fourbar.getPosition() - fourbar.getTargetPosition()) < 1) && (Math.abs(elevator.getPosition() - elevator.getTargetPosition()) < 0.2);
-    }
     
-    public static void autonomousInit() {
-        elevator.autonomousInit();
-        fourbar.setPIDSpeed(0.3);
-    }
-
-
-    /**
-     * Converts position of the elevator and fourbar to x and y coordinates on a 2d plane
-     * @param elevPos The encoder position of the elevator in inches
-     * @param fourbarDeg The encoder position of the fourbar in degrees
-     * @return [x, y]
+    /**Toggles between cube and cone mode when button pressed
+     * @param buttonReleased The button to toggle the game piece with (use a get button released method)
      */
-    public static double[] posToCoords(double elevPos, double fourbarDeg) {
-
-        double angle = Math.toRadians(90 - fourbarDeg);
-        double x;
-        double y;
-
-        //funni math stuff my brain hurts
-        x = Math.cos(angle) * FOURBAR_HYPOTENUSE;
-        y = (Math.sin(angle) * FOURBAR_HYPOTENUSE) + elevPos;
-
-        //return coordinates
-        double[] output = { x, y };
-        return output;
+    public static void toggleGamePiece(boolean buttonReleased) {
+        instance.gamePieceSelected = (buttonReleased ? (instance.gamePieceSelected == GamePiece.CONE ? GamePiece.CUBE : GamePiece.CONE) : instance.gamePieceSelected);
     }
-    
-    /**
-     * Converts x and y coordinates into encoder positions for the elevator and fourbar
-     * @param x
-     * @param y
-     * @return [elevator position (in), fourbar position (deg), is coord altered (0 if no, 1 if yes)]
-     */
-    public static double[] coordsToPos(double x, double y) {
-
-        double elevPos;
-        double fourbarDeg;
-        boolean isAltered = false;
 
-        //calculate elevator position
-        elevPos = y >= Math.sqrt((Math.pow(FOURBAR_HYPOTENUSE, 2) - Math.pow(x, 2)))
-            ? y - Math.sqrt((Math.pow(FOURBAR_HYPOTENUSE, 2) - Math.pow(x, 2)))
-            : y + Math.sqrt((Math.pow(FOURBAR_HYPOTENUSE, 2) - Math.pow(x, 2)));
+    public static Coordinate getCurrentPos() {
+        return instance.currentSetpoint.getCoords();
+    }
 
-        //correct position if the calculation returns a position that is out of range
-        if(elevPos > ELEVATOR_MAX_POS || elevPos < 0) {
-            elevPos = 0;
-            isAltered = true;
-        }
+    public static Setpoint getSetpoint() {
+        return instance.setpoint;
+    }
 
-        //calculate fourbar position
-        fourbarDeg = Math.toDegrees(Math.atan2(x, y-elevPos));
-        
-        //old method of finding foubrar position, delete after testing new method
-        /* fourbarDeg = y > elevPos
-            ? -Math.toDegrees(Math.atan(x / (elevPos - y)))
-            : 180 - Math.toDegrees(Math.atan(x / (elevPos - y))); */
-
-        //Correct fourbar pos if out of bounds
-        double maxAngle = Math.toDegrees(Math.atan2(BUMPER_X, BUMPER_Y - elevPos));
-
-        if(fourbarDeg > maxAngle) {
-            fourbarDeg = maxAngle;
-            isAltered = true;
-        } else if(fourbarDeg < 0) {
-            fourbarDeg = 0;
-            isAltered = true;
-        }
+    public static GamePiece getGamePieceSelected() {
+        return instance.gamePieceSelected;
+    }
 
-        //return positions
-        double[] output = { elevPos, fourbarDeg, isAltered ? 1 : 0 };
-        return output;
+    public static ControlType getControlType() {
+        return instance.controlType;
     }
 
-    public static double[] getCoords() {
-        return instance.coords;
+    public static void setFourbarBrake(boolean brake) {
+        fourbar.setBrake(brake);
     }
 
-    public static GamePiece getGamePieceSelected() {
-        return gamePieceSelected;
+    public static void toggleTranslateMode() {
+        instance.translateMode = !instance.translateMode;
     }
 
     public void print() {
-        SmartDashboard.putString("Setpoint", instance.setpoint.toString());
         SmartDashboard.putString("Control Type", instance.controlType.toString());
 
         //fourbar and elevator coordinates
-        SmartDashboard.putString("X", df.format(instance.coords[0]));
-        SmartDashboard.putString("Y", df.format(instance.coords[1]));
-        SmartDashboard.putString("Target Coords", "(" + instance.targetCoords[0] + ", " + instance.targetCoords[1] + ")");
+        SmartDashboard.putString("Current coords", "(" + getCurrentPos().x + ", " + getCurrentPos().y + ")");
+        SmartDashboard.putString("Target Coords", "(" + instance.setpoint.getCoords().x + ", " + instance.setpoint.getCoords().y + ")");
         SmartDashboard.putNumber("Bumper Intercept", fourbar.getBumperIntercept());
         SmartDashboard.putNumber("Fourbar Slope", fourbar.getSlope());
 
         //Elevator values
         SmartDashboard.putNumber("Elevator Position", elevator.getPosition());
-        SmartDashboard.putNumber("Elevator Target", elevator.getTargetPosition()); //ADD THIS
+        SmartDashboard.putNumber("Elevator Target", elevator.getTargetPosition());
 
         //Fourbar values
         SmartDashboard.putNumber("Fourbar Position", fourbar.getPosition());
-        SmartDashboard.putNumber("Fourbar Target", (fourbar.getTargetPosition() + fourbar.ENCODER_OFFSET)/360.0); //ADD THIS
+        SmartDashboard.putNumber("Fourbar Target", (fourbar.getTargetPosition() - fourbar.ENCODER_OFFSET));
+        SmartDashboard.putNumber("Fourbar Power", fourbar.getPower());
         SmartDashboard.putNumber("Abs Encoder Position", fourbar.getAbsolutePosition());
 
-        SmartDashboard.putBoolean("Cube Mode Selected?", ElevFourbar.gamePieceSelected == ElevFourbar.GamePiece.CUBE);
-        SmartDashboard.putBoolean("Cone Mode Selected?", ElevFourbar.gamePieceSelected == ElevFourbar.GamePiece.CONE);
-    }
-
-    public static void setFourbarBrake(boolean brake) {
-        fourbar.setBrake(brake);
+        SmartDashboard.putString("Game Piece", getGamePieceSelected().toString());
     }
 }
diff --git a/src/main/java/frc/robot/systems/Intake.java b/src/main/java/frc/robot/systems/Intake.java
index 103ec60..3ec968f 100644
--- a/src/main/java/frc/robot/systems/Intake.java
+++ b/src/main/java/frc/robot/systems/Intake.java
@@ -1,15 +1,16 @@
 package frc.robot.systems;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import edu.wpi.first.wpilibj.PneumaticsModuleType;
+import frc.robot.systems.ElevFourbar.GamePiece;
 import edu.wpi.first.wpilibj.Compressor;
 import edu.wpi.first.wpilibj.Timer;
-import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.SmartPrintable;
 import frc.robot.subsystems.*;
-import frc.robot.systems.ElevFourbar.GamePiece;
 
 public class Intake extends SmartPrintable {
 
+    //Enum for claw and pivot states
     public enum SolenoidState {
         OPEN, CLOSED, UP, DOWN
     }
@@ -20,80 +21,120 @@ public enum SolenoidState {
     private static final int CLAW_REVERSE_CHANNEL = 0;
     private static final int PIVOT_FORWARD_CHANNEL = 2;
     private static final int PIVOT_REVERSE_CHANNEL = 3;
-    
-    private static double rollerStartTime;
 
-    //declare objects
+    //Declare objects
     public static Compressor comp;
     private static Pivot pivot;
     private static Roller roller;
     private static Claw claw;
-    private static Timer timer = new Timer();
 
-    private SolenoidState clawState;
-    private SolenoidState pivotState;
+    private Timer timer = new Timer();
 
     //instantiate instance of class
     private static Intake instance = new Intake();
+    
 
     public Intake() {
         super();
 
         comp = new Compressor(0, PneumaticsModuleType.CTREPCM);
-        //comp.enableDigital();
+        comp.enableDigital();
 
-        //initiali
+        //initialize objects
         roller = new Roller(ROLLER_ID);
         claw = new Claw(PneumaticsModuleType.CTREPCM, CLAW_FORWARD_CHANNEL, CLAW_REVERSE_CHANNEL);
         pivot = new Pivot(PIVOT_FORWARD_CHANNEL, PIVOT_REVERSE_CHANNEL);
-        timer.start();
     }
 
-    public static void init() { //opens claw if cube is selected, closes claw if cone is selected
-        instance.pivotState = SolenoidState.DOWN;
-        instance.clawState = (ElevFourbar.getGamePieceSelected() == ElevFourbar.GamePiece.CONE) ? SolenoidState.CLOSED : SolenoidState.OPEN;
-        timer.reset();
-        timer.start();
-    }
+    //Opens claw if cube is selected, closes claw if cone is selected
+    public static void init() {
+        claw.close();
 
-    public static void autoInit() {
-        timer.reset();
-        timer.start();
+        instance.timer.reset();
+        instance.timer.start();
     }
 
     /**
-     * The method to be run from teleopPeriodic
-     * @param dPadDirection - the direction of the DPAD
-     * @param rollerSpeed - The speed of the roller
-     * @param clawButton - The button to extend/retract the claw
-     * @param pivotButton - The button to extend/retract the pivot
+     * Runs the intake
+     * @param pivotToggle Toggles the pivot's state when pressed
+     * @param clawHeld Opens the claw when held
+     * @param clawReleased 
+     * @param intake
+     * @param outtake
      */
-    public static void run(boolean pivotToggle, boolean intake, boolean outtake, boolean clawHeld, boolean clawReleased) {
-        instance.clawState = claw.getState();
+    public static void run(boolean pivotToggle, boolean clawHeld, boolean intake, boolean outtake) {
+        double rollerSpeed;
 
+        //IF CUBE IS SELECTED
         if(ElevFourbar.getGamePieceSelected() == GamePiece.CUBE) {
-            //dispense cubes at low power
-            if(intake) {
-                runRoller(-1.0);
+            if(intake) { //Run rollers in at full speed and open claw
+                rollerSpeed = -1.0;
                 clawHeld = true;
             }
-            else if(clawHeld)
-                runRoller(0.3);
-            else if(outtake)
-                runRoller(1.0);
-            else 
-                runRoller(0);
-
-            runClaw(clawHeld, clawReleased);
+            else if(clawHeld) //Dispense cubes at low power
+                rollerSpeed = 0.3;
+
+            else if(outtake) //Shoot cubes at full power
+                rollerSpeed = 1.0;
+
+            else //Idly run at very low power to keep in game pieces
+                rollerSpeed = -0.06;
+        } 
+
+        //IF CONE IS SELECTED
+        else {
+            if(intake) //Intake at full speed
+                rollerSpeed = -1.0;
+            
+            else if(outtake) //Outtake at full speed
+                rollerSpeed = 1.0;
+
+            else //Idly run at very low power to keep in game pieces
+                rollerSpeed = -0.06;
+        }
+        
+        //Run the mechanisms
+        runClaw(clawHeld);
+        runPivot(pivotToggle);
+        runRoller(rollerSpeed);
+    }
+
+    /**
+     * Runs the claw
+     * @param held Opens claw if button is held
+     * @param released Restarts auto intake timer
+     */
+    private static void runClaw(boolean held) {
+        if (held) {
+            claw.open();
         } else {
-            runClaw(clawHeld, clawReleased);
+            claw.close();
+        }
+    }
 
-            runRoller(intake, outtake);
+    /**
+     * @param toggle toggle between up and down when true
+     */
+    private static void runPivot(boolean toggle) {
+        if(toggle) {
+            if(pivot.getState() == SolenoidState.UP) {
+                pivot.down();
+            } else {
+                pivot.up();
+            }
         }
+    }
 
-        runPivot(pivotToggle);
+    /**
+     * @param speed speed of the roller
+     */
+    public static void runRoller(double speed) {
+        roller.setSpeed(speed);
     }
 
+    /**
+     * @param state state to set claw to
+     */
     public static void autoClaw(SolenoidState state) {
         if(state == SolenoidState.OPEN) {
             claw.open();
@@ -102,6 +143,9 @@ public static void autoClaw(SolenoidState state) {
         }
     }
 
+    /**
+     * @param state state to set pivot to
+     */
     public static void autoPivot(SolenoidState state) {
         if(state == SolenoidState.DOWN) {
             pivot.down();
@@ -111,110 +155,48 @@ public static void autoPivot(SolenoidState state) {
     }
 
     /**
-     * Automatically runs rollers for a set amount of time
-     * @param startTime the time to start the rollers
-     * @param endTime the time to stop the rollers
-     * @param speed the speed of the rollers
+     * Run the roller for a set amount of time
+     * @param startTime 
+     * @param duration
+     * @param speed
      */
-    public static void autoRoller(double startTime, double endTime, double speed) { 
-        //if the timer is between the start and end times, run the rollers at the given speed
-        if(timer.get() > startTime && timer.get() < endTime) {
+    public static void autoRoller(double startTime, double duration, double speed) {
+        if(instance.timer.get() > startTime && instance.timer.get() < startTime + duration) {
             roller.setSpeed(speed);
-        }
-    }
-
-    /**
-     * Runs the roller at speeds determined by the state of the claw and the game piece selected
-     * @param intake
-     * @param outtake
-     */
-    public static void runRoller(boolean intake, boolean outtake) {
-        double rollerSpeed;
-
-        if(intake) {
-            //intake at 100 percent
-            rollerSpeed = -1.0;
-        } else if(outtake) {
-            //in cube mode, outtake at 100 percent if closed (shooting) and 60 percent if open (dispensing)
-            if(ElevFourbar.getGamePieceSelected() == ElevFourbar.GamePiece.CUBE) {
-                if(instance.clawState == SolenoidState.CLOSED) 
-                    rollerSpeed = 1.0;
-                else rollerSpeed = 0.6;
-            } else {
-                //outtake at 100 percent if in cone mode
-                rollerSpeed = 1.0;
-            }
         } else {
-            //idly run rollers at 6 percent to secure game pieces
-            rollerSpeed = -0.06;
+            roller.setSpeed(0);
         }
-
-        roller.setSpeed(rollerSpeed);
     }
 
     /**
-     * Runs the roller at a given speed
-     * @param speed
+     * @return current state of the pivot
      */
-    public static void runRoller(double speed) {
-        roller.setSpeed(speed);
+    public static SolenoidState getPivotState() {
+        return pivot.getState();
     }
 
     /**
-     * Extends or retracts the claw, toggled with button press
-     * @param switchClawState
+     * @return current state of the claw
      */
-    private static void runClaw(boolean switchClawState, boolean closeClaw) {
-            if (switchClawState) {
-                claw.open();
-            } else {
-                claw.close();
-                autoRoller(rollerStartTime, rollerStartTime + 0.5, -1);
-            }
-            
+    public static SolenoidState getClawState() {
+        return claw.getState();
+    }
 
-            if (closeClaw) {
-                claw.close();
-                if (ElevFourbar.gamePieceSelected == GamePiece.CONE)
-                    rollerStartTime = timer.get();
-            }
 
-            SmartDashboard.putNumber("Roller start time", rollerStartTime);
-            SmartDashboard.putNumber("Rotime", timer.get());
-            SmartDashboard.putBoolean("Close claw", switchClawState);
-    }  
-    
     /**
-     * Extends or retracts the pivot, toggled with button press
-     * @param switchPivotState
+     * Prints needed information to the SmartDashboard through the SmartPrintable class
      */
-    private static void runPivot(boolean switchPivotState) {
-        if(switchPivotState) {
-            if(instance.pivotState == SolenoidState.UP) {
-                instance.pivotState = SolenoidState.DOWN;
-            } else {
-                instance.pivotState = SolenoidState.UP;
-            }
-        }
+    @Override
+    public void print() {
+        //Claw
+        SmartDashboard.putString("Claw State", claw.getState().toString());
 
-        if(instance.pivotState == SolenoidState.DOWN) {
-            pivot.down();
-        } else if(instance.pivotState == SolenoidState.UP) {
-            pivot.up();
-        }
-    }
+        //Pivot
+        SmartDashboard.putString("Pivot State", pivot.getState().toString());
 
-    public static SolenoidState getPivotState() {
-        return instance.pivotState;
-    }
+        //Roller
+        SmartDashboard.putNumber("Roller Speed", roller.get());
+        SmartDashboard.putNumber("Roller Amps", roller.getAmps());
 
-    /**
-     * Updates Smart Dashboard with important variables
-     * @param rollerSpeed
-     */
-    public void print() {
-        SmartDashboard.putString("Claw State", instance.clawState.toString());
-        SmartDashboard.putString("Pivot State", instance.pivotState.toString());
     }
-    
 }
diff --git a/src/main/java/frc/robot/systems/LEDLights.java b/src/main/java/frc/robot/systems/LEDLights.java
index 4632ab3..c879b3c 100644
--- a/src/main/java/frc/robot/systems/LEDLights.java
+++ b/src/main/java/frc/robot/systems/LEDLights.java
@@ -8,6 +8,7 @@
 import frc.robot.patterns.FadeIntoPattern;
 import frc.robot.subsystems.LightPattern;
 import frc.robot.subsystems.LightRenderer;
+import frc.robot.subsystems.SwerveMode;
 import frc.robot.systems.ElevFourbar.GamePiece;
 
 public class LEDLights {
@@ -42,7 +43,7 @@ public static void run() {
         } else if (Robot.getControlMode() == ControlMode.AUTONOMOUS) {
             setPattern = new FadeIn(new Color(0.0, 1.0, 0.0), 1.0);
         } else {
-            if (SwerveDrive.getRockMode()) {
+            if (SwerveDrive.getMode() == SwerveMode.ROCK) {
                 // If in rock mode make wyvern scary >:D
                 setPattern = new FadeIn(new Color(1.0, 0.0, 0.0), 1.0);
             } else {
diff --git a/src/main/java/frc/robot/systems/Pigeon.java b/src/main/java/frc/robot/systems/Pigeon.java
index 9f3121d..2b09080 100644
--- a/src/main/java/frc/robot/systems/Pigeon.java
+++ b/src/main/java/frc/robot/systems/Pigeon.java
@@ -13,7 +13,9 @@
 import com.ctre.phoenix.sensors.Pigeon2;
 
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
 import frc.robot.SmartPrintable;
+import frc.robot.subsystems.Angle;
 
 /*
  * Manages the robot's pigeon.
@@ -48,7 +50,6 @@ private Pigeon(int canID) {
      */
     public static void setFeildZero() {
         instance.pigeon.setYaw(0.0);
-        
     }
 
     /**
@@ -61,31 +62,31 @@ public static OrientationalChange getChangePerSecond() {
     /**
      * Gets Yaw.
      */
-    public static double getYaw() {
-        return instance.pigeon.getYaw() % 360.0;
+    public static Angle getYaw() {
+        return new Angle().setDegrees(instance.pigeon.getYaw() % 360.0);
     }
 
     /**
      * Gets pitch.
      */
-    public static double getPitch() {
+    public static Angle getPitch() {
         // Pitch and roll are flipped due to mounting orientation.
-        return instance.pigeon.getRoll();
+        return new Angle().setDegrees(instance.pigeon.getRoll());
     }
 
     /**
      * Gets roll.
      */
-    public static double getRoll() {
+    public static Angle getRoll() {
         // Pitch and roll are flipped due to mounting orientation.
-        return instance.pigeon.getPitch();
+        return new Angle().setDegrees(instance.pigeon.getPitch());
     }
     
     @Override
     public void print() {
-        SmartDashboard.putNumber("Pigeon Yaw", getYaw());
-        SmartDashboard.putNumber("Pigeon Pitch", getPitch());
-        SmartDashboard.putNumber("Pigeon Roll", getRoll());
+        SmartDashboard.putNumber("Pigeon Yaw", getYaw().radians());
+        SmartDashboard.putNumber("Pigeon Pitch", getPitch().radians());
+        SmartDashboard.putNumber("Pigeon Roll", getRoll().radians());
 
         OrientationalChange change = getChangePerSecond();
 
diff --git a/src/main/java/frc/robot/systems/SmartPrinter.java b/src/main/java/frc/robot/systems/SmartPrinter.java
index 3e4c51c..3823994 100644
--- a/src/main/java/frc/robot/systems/SmartPrinter.java
+++ b/src/main/java/frc/robot/systems/SmartPrinter.java
@@ -5,16 +5,18 @@
 package frc.robot.systems;
 
 import java.security.InvalidParameterException;
+import java.util.ArrayList;
 import java.util.LinkedList;
 
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.SmartPrintable;
 
 public class SmartPrinter {
-    private LinkedList<SmartPrintable> printables;
+    private ArrayList<SmartPrintable> printables;
     private static final SmartPrinter instance = new SmartPrinter();
 
     public SmartPrinter() {
-        printables = new LinkedList<SmartPrintable>();
+        printables = new ArrayList<SmartPrintable>();
     }
 
     public static void register(SmartPrintable printable) {
diff --git a/src/main/java/frc/robot/systems/SwerveDrive.java b/src/main/java/frc/robot/systems/SwerveDrive.java
index 5676f9e..356ec5f 100644
--- a/src/main/java/frc/robot/systems/SwerveDrive.java
+++ b/src/main/java/frc/robot/systems/SwerveDrive.java
@@ -19,6 +19,7 @@
 import edu.wpi.first.math.kinematics.SwerveModuleState;
 import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
 import frc.robot.SmartPrintable;
+import frc.robot.subsystems.Angle;
 import frc.robot.subsystems.SwerveMode;
 import frc.robot.subsystems.SwerveModule;
 
@@ -30,27 +31,46 @@ public class SwerveDrive extends SmartPrintable {
     private static final int MODULE_ROTATION_CAN_IDS[] = { 5,  6,   7,   8  };
     private static final int MODULE_CANCODER_CAN_IDS[] = { 9,  10,  11,  12 };
     
-    private static final double MODULE_CANCODER_OFFSETS[] = { -252.24607237 + 90.0, -224.033203125 + 270.0, -11.425719246268272 + 270.0, -179.56050113588573 + 90.0 };
-    //private static final double MODULE_CANCODER_OFFSETS[] = { -252.24607237 + 90.0, -405.26363752 + 90.0, -189.66795267 + 90.0, -175.16600078 + 90.0 };
-    private static final double MODULE_ROCK_MODE_PSOITIONS[] = { -Math.PI / 4, Math.PI / 4, -Math.PI / 4, Math.PI / 4 };
     private static final double MODULE_COEFFIENTS[] = { -1.0, -1.0, -1.0, -1.0 };
-    
     private static final double LOW_SENSITIVITY_RATIO = 0.08;
-
     private static final double CHASSIS_SIDE_LENGTH = 0.6;
-    private static final double RADIAN_DEGREE_RATIO = Math.PI / 180.0;
 
+    private static final Angle MODULE_CANCODER_OFFSETS[] = {
+        new Angle().setDegrees(-252.24607237 + 90.0), 
+        new Angle().setDegrees(-224.033203125 + 270.0), 
+        new Angle().setDegrees(-11.425719246268272 + 270.0), 
+        new Angle().setDegrees(-179.56050113588573 + 90.0) 
+    };
+
+    private static final Angle MODULE_ROCK_MODE_POSITIONS[] = { 
+        new Angle().setRadians( -Math.PI / 4), 
+        new Angle().setRadians(  Math.PI / 4), 
+        new Angle().setRadians( -Math.PI / 4), 
+        new Angle().setRadians(  Math.PI / 4) 
+    };
+
+    private static final Translation2d MODULE_PHYSICAL_POSITIONS[] = {
+        new Translation2d(   CHASSIS_SIDE_LENGTH / 2,   CHASSIS_SIDE_LENGTH / 2),
+        new Translation2d(  -CHASSIS_SIDE_LENGTH / 2,   CHASSIS_SIDE_LENGTH / 2),
+        new Translation2d(  -CHASSIS_SIDE_LENGTH / 2,  -CHASSIS_SIDE_LENGTH / 2),
+        new Translation2d(   CHASSIS_SIDE_LENGTH / 2,  -CHASSIS_SIDE_LENGTH / 2)
+    };
+
+    // Singleton instance.
     private static final SwerveDrive instance = new SwerveDrive();
 
-    private BiFunction<Double, Double, Double> directionCurve = Controls::defaultCurveTwoDimensional;
-    private Function<Double, Double> turnCurve = Controls::defaultCurve;
-
+    // These drive state objects modify themselves internally through methods,
+    // but require no setting and are therefore final.
     private final SwerveModule modules[] = new SwerveModule[MODULE_MOVEMENT_CAN_IDS.length];
     private final SwerveModulePosition positions[] = new SwerveModulePosition[MODULE_MOVEMENT_CAN_IDS.length];
     private final SwerveDriveKinematics kinematics;
     private final SwerveDriveOdometry odometry;
     
-    private SwerveMode mode = SwerveMode.Headless;
+    // Variable drive states objects.
+    private BiFunction<Double, Double, Double> directionCurve = Controls::defaultCurveTwoDimensional;
+    private Function<Double, Double> turnCurve = Controls::defaultCurve;
+    private SwerveMode mode = SwerveMode.HEADLESS;
+    private SwerveMode inactiveMode = SwerveMode.HEADLESS;
 
     private SwerveDrive() {
         super();
@@ -61,22 +81,27 @@ private SwerveDrive() {
                 MODULE_ROTATION_CAN_IDS[i], 
                 MODULE_CANCODER_CAN_IDS[i], 
                 MODULE_CANCODER_OFFSETS[i], 
-                MODULE_COEFFIENTS[i]
+                MODULE_COEFFIENTS[i],
+                MODULE_PHYSICAL_POSITIONS[i]
             );
         }
 
         for (int i = 0; i < modules.length; i++) {
-            positions[i] = modules[i].getAngle();
+            positions[i] = modules[i].getPosition();
         }
 
         kinematics = new SwerveDriveKinematics(
-            new Translation2d(   CHASSIS_SIDE_LENGTH / 2,   CHASSIS_SIDE_LENGTH / 2),
-            new Translation2d(  -CHASSIS_SIDE_LENGTH / 2,   CHASSIS_SIDE_LENGTH / 2),
-            new Translation2d(  -CHASSIS_SIDE_LENGTH / 2,  -CHASSIS_SIDE_LENGTH / 2),
-            new Translation2d(   CHASSIS_SIDE_LENGTH / 2,  -CHASSIS_SIDE_LENGTH / 2)
+            MODULE_PHYSICAL_POSITIONS[0],
+            MODULE_PHYSICAL_POSITIONS[1],
+            MODULE_PHYSICAL_POSITIONS[2],
+            MODULE_PHYSICAL_POSITIONS[3]
         );
 
-        odometry = new SwerveDriveOdometry(kinematics, new Rotation2d(Pigeon.getYaw() * RADIAN_DEGREE_RATIO), positions);
+        odometry = new SwerveDriveOdometry(
+            kinematics, 
+            new Rotation2d(Pigeon.getYaw().radians()), 
+            positions
+        );
     }
 
     /**
@@ -95,24 +120,50 @@ public static SwerveMode getMode() {
         return instance.mode;
     }
 
+    /**
+     * May be called before running the swerve drive to temporarily set a mode
+     * for that call of a run method.
+     * 
+     * Each time a run method completes it will change the mode back to what it
+     * was when this method was called.
+     * 
+     * If this method is called more than once before running then the latest
+     * given temporary mode will be used, the inactive mode to be reset to after
+     * running will not be effected
+     */
+    public static void tempMode(SwerveMode mode) {
+        if (instance.inactiveMode != null) {
+            instance.mode = mode;
+            return;
+        }
+
+        instance.inactiveMode = instance.mode;
+        instance.mode = mode;
+    }
+
+    /**
+     * Exactly like `tempMode` but predicates the temporary mode on the given 
+     * boolean condition.
+     */
+    public static void conditionalTempMode(SwerveMode mode, boolean condition) {
+        if (!condition) {
+            return;
+        }
+
+        tempMode(mode);
+    }
+
     /**
      * Runs swerve, behavior changes based on the drive's mode. Derives speed
-     * from directional inputs.
+     * from directional inputs. This will reset temporary modes on completion.
      * @param directionalX The X axis of the directional control, between 1 and -1
      * @param directionalY The Y axis of the directional control, between 1 and -1.
      * @param turn A value between 1 and -1 that determines the turning angle.
      * @param lowSense The angle to move in low sensitivity in degrees, -1 for no movement.
      */
     public static void run(double directionalX, double directionalY, double turn, int lowSense) {
-        if (getRockMode()) {
-            runRockMode();
-            return;
-        }
-        
-        instance.odometry.update(new Rotation2d(Pigeon.getYaw() * RADIAN_DEGREE_RATIO), instance.positions);
-        
         if (lowSense != -1) {
-            double angle = (2 * Math.PI) - ((double)lowSense * RADIAN_DEGREE_RATIO);
+            double angle = Angle.TAU - Math.toRadians((double)lowSense);
 
             // inverted since the drive is rotated to compensate for joystick stuff
             directionalX = -(Math.sin(angle) * LOW_SENSITIVITY_RATIO);
@@ -125,79 +176,73 @@ public static void run(double directionalX, double directionalY, double turn, in
         directionalX = instance.directionCurve.apply(directionalX, directionalY);
         directionalY = instance.directionCurve.apply(directionalY, directionalX);
         turn = instance.turnCurve.apply(turn);
-
-        ChassisSpeeds chassisSpeeds;
-
-        if (instance.mode == SwerveMode.Headless) {
-            chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(directionalX, -directionalY, turn, new Rotation2d(Pigeon.getYaw() * RADIAN_DEGREE_RATIO));
-        } else {
-            chassisSpeeds = new ChassisSpeeds(directionalX, -directionalY, turn);
-        }
-
-        SwerveModuleState[] moduleStates = instance.kinematics.toSwerveModuleStates(chassisSpeeds);
-
-        for (int i = 0; i < instance.modules.length; i++) {
-            instance.modules[i].setDesiredState(moduleStates[i]);
-        }
+        runUncurved(directionalX, directionalY, turn);
     }
-
+    
     /**
-     * Run the swerve drive exactly by the arguments passed, no cuving will 
-     * occure on the given inputs, nor will the be deadbanded.
+     * Run the swerve drive exactly by the arguments passed, no curving will 
+     * occure on the given inputs, nor will they be deadbanded. This will reset
+     * temporary modes on completion.
      * @param directionalX Speed along the x axis. -1.0 - 1.0
      * @param directionalY Speed along the y axis. -1.0 - 1.0
      * @param turn Rate of turn. -1.0 - 1.0
      */
     public static void runUncurved(double directionalX, double directionalY, double turn) {
-        if (getRockMode()) {
-            runRockMode();
-            return;
-        }
-
-        ChassisSpeeds chassisSpeeds;
+        SwerveModuleState[] moduleStates = new SwerveModuleState[instance.modules.length];
+        boolean holdPos = false;
+
+        switch (instance.mode) {
+            case HEADLESS:
+                moduleStates = instance.kinematics.toSwerveModuleStates(
+                    ChassisSpeeds.fromFieldRelativeSpeeds(
+                        directionalX, -directionalY, turn, 
+                        new Rotation2d(Pigeon.getYaw().radians())
+                    )
+                );
+                break;
+
+            case RELATIVE: 
+                moduleStates = instance.kinematics.toSwerveModuleStates(
+                    new ChassisSpeeds(directionalX, -directionalY, turn)
+                ); 
+                break;
+
+            case ROCK: {
+                assert moduleStates.length == instance.modules.length;
+                holdPos = true;
+
+                for (int i = 0; i < instance.modules.length; i++) {
+                    moduleStates[i] = new SwerveModuleState(0.0, new Rotation2d(MODULE_ROCK_MODE_POSITIONS[i].radians()));
+                }
 
-        if (instance.mode == SwerveMode.Headless) {
-            chassisSpeeds = ChassisSpeeds.fromFieldRelativeSpeeds(directionalX, -directionalY, turn, new Rotation2d(Pigeon.getYaw() * RADIAN_DEGREE_RATIO));
-        } else {
-            chassisSpeeds = new ChassisSpeeds(directionalX, -directionalY, turn);
-        }
+                break;
+            } 
 
-        SwerveModuleState[] moduleStates = instance.kinematics.toSwerveModuleStates(chassisSpeeds);
+            // This branch should never be reached as the enum used should never
+            // have more than the above possible values.
+            default: assert false;
+        } 
 
         for (int i = 0; i < instance.modules.length; i++) {
             instance.modules[i].setDesiredState(moduleStates[i]);
+            instance.modules[i].setRockMode(holdPos);
         }
-    }
 
-    /**
-     * Wyvern becomes rock, rock do not move, Wyvern do not move...
-     */
-    public static void runRockMode() {
-        for (int i = 0; i < instance.modules.length; i++) {
-            instance.modules[i].setDesiredState(new SwerveModuleState(0.0, new Rotation2d(MODULE_ROCK_MODE_PSOITIONS[i])));
-        }
-    }
+        instance.odometry.update(new Rotation2d(Pigeon.getYaw().radians()), instance.positions);
 
-    /**
-     * True if rock mode should be active.
-     */
-    public static void setRockMode(boolean shouldHold) {
-        for (SwerveModule module : instance.modules) {
-            module.setRockMode(shouldHold);
+        if (instance.inactiveMode != null) {
+            instance.mode = instance.inactiveMode;
+            instance.inactiveMode = null;
         }
     }
 
     /**
-     * Returns true if in rock mode.
+     * Sets the max speed of all modules, use NaN for no limit.
      */
-    public static boolean getRockMode() {
-        boolean rockMode = false;
-
+    public static void setMaxSpeed(double maxSpeed) {
         for (SwerveModule module : instance.modules) {
-            rockMode |= module.getRockMode();
+            module.setMaxSpeed(maxSpeed);
         }
-
-        return rockMode;
     }
 
     /**
@@ -282,15 +327,14 @@ public static void setTurnCurve(Function<Double, Double> curve) {
      */
     @Override
     public void print() {
-        SmartDashboard.putString("Orientation", getMode().toString());
-        SmartDashboard.putBoolean("In Rock Mode", getRockMode());
+        SmartDashboard.putString("Swerve Mode", getMode().toString());
     }
 
     /**
      * Gets the longest distance traveled by any modules.
      */
     public static double getDistance() {
-        return Math.max(Math.abs(instance.modules[0].getPosition()), Math.abs(instance.modules[1].getPosition()));
+        return Math.max(Math.abs(instance.modules[0].getDistanceTraveled()), Math.abs(instance.modules[1].getDistanceTraveled()));
     }
 
     /**