diff --git a/simgui.json b/simgui.json
index 2fb8b04..78139d4 100644
--- a/simgui.json
+++ b/simgui.json
@@ -1,8 +1,24 @@
 {
+  "HALProvider": {
+    "Encoders": {
+      "window": {
+        "visible": true
+      }
+    },
+    "PWM Outputs": {
+      "window": {
+        "visible": true
+      }
+    }
+  },
   "NTProvider": {
     "types": {
       "/FMSInfo": "FMSInfo",
       "/SmartDashboard/Autonomous": "String Chooser",
+      "/SmartDashboard/Visualizers/Elevator": "Mechanism2d"
+    },
+    "windows": {
+      "/SmartDashboard/Visualizers/Elevator": {
       "/SmartDashboard/algae": "Mechanism2d"
     },
     "windows": {
@@ -25,7 +41,10 @@
         "open": true
       },
       "SmartDashboard": {
+        "Elevator": {
+
         "algae": {
+
           "open": true
         },
         "open": true
@@ -37,8 +56,22 @@
       "open": true
     },
     "Server": {
+
+      "Publishers": {
+        "open": true
+      },
+      "Subscribers": {
+        "open": true
+      }
+    },
+    "visible": true
+  },
+  "NetworkTables View": {
+    "visible": false
+
       "open": true
     },
     "visible": true
+
   }
 }
diff --git a/src/main/java/com/stuypulse/robot/Robot.java b/src/main/java/com/stuypulse/robot/Robot.java
index e0f788d..22585c8 100644
--- a/src/main/java/com/stuypulse/robot/Robot.java
+++ b/src/main/java/com/stuypulse/robot/Robot.java
@@ -5,12 +5,23 @@
 
 package com.stuypulse.robot;
 
+import com.stuypulse.robot.constants.Settings.RobotType;
+
 import edu.wpi.first.wpilibj.TimedRobot;
 import edu.wpi.first.wpilibj2.command.Command;
 import edu.wpi.first.wpilibj2.command.CommandScheduler;
 
 public class Robot extends TimedRobot {
 
+    public static final RobotType ROBOT;
+
+    static {
+        if (Robot.isSimulation())
+            ROBOT = RobotType.SIM;
+        else
+            ROBOT = RobotType.fromString(System.getenv("serialnum"));
+    }
+
     private RobotContainer robot;
     private Command auto;
 
diff --git a/src/main/java/com/stuypulse/robot/RobotContainer.java b/src/main/java/com/stuypulse/robot/RobotContainer.java
index 9bd6488..bf6a60e 100644
--- a/src/main/java/com/stuypulse/robot/RobotContainer.java
+++ b/src/main/java/com/stuypulse/robot/RobotContainer.java
@@ -5,6 +5,15 @@
 
 package com.stuypulse.robot;
 
+import java.lang.annotation.ElementType;
+import com.stuypulse.robot.commands.Elevator.ElevatorToBottom;
+import com.stuypulse.robot.commands.Elevator.ElevatorToLvl1;
+import com.stuypulse.robot.commands.Elevator.ElevatorToLvl2;
+import com.stuypulse.robot.commands.Elevator.ElevatorToLvl3;
+import com.stuypulse.robot.commands.Elevator.ElevatorToLvl4;
+import com.stuypulse.robot.commands.auton.DoNothingAuton;
+import com.stuypulse.robot.constants.Ports;
+import com.stuypulse.robot.subsystems.Elevator.Elevator;
 import javax.management.openmbean.OpenType;
 
 // Algae Commands
@@ -34,13 +43,14 @@ public class RobotContainer {
     public final Gamepad driver = new AutoGamepad(Ports.Gamepad.DRIVER);
     public final Gamepad operator = new AutoGamepad(Ports.Gamepad.OPERATOR);
     
-    // Subsystem
+    // Subsystems
+    public final Elevator elevator = Elevator.getInstance();
     public final Algae algae = Algae.getInstance();
 
     // Autons
     private static SendableChooser<Command> autonChooser = new SendableChooser<>();
 
-    // Robot container
+    // Robot Container
 
     public RobotContainer() {
         configureDefaultCommands();
@@ -58,16 +68,32 @@ private void configureDefaultCommands() {}
     /*** BUTTONS ***/
     /***************/
 
-
     private void configureButtonBindings() {
         configureDriverBindings();
         configureOperatorBindings();
     }
 
-    private void configureDriverBindings() {}
+    private void configureDriverBindings() {
+        
+    }
 
     private void configureOperatorBindings() {
 
+        operator.getDPadDown()
+            .onTrue(new ElevatorToLvl1());
+
+        operator.getDPadLeft()
+            .onTrue(new ElevatorToLvl2());
+            
+        operator.getDPadRight()
+            .onTrue(new ElevatorToLvl3());    
+
+        operator.getDPadUp()
+            .onTrue(new ElevatorToLvl4());
+
+        operator.getLeftButton()
+            .onTrue(new ElevatorToBottom());
+
         operator.getBottomButton()
             .onTrue(new AlgaeStow());
         
diff --git a/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToBottom.java b/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToBottom.java
new file mode 100644
index 0000000..83403f3
--- /dev/null
+++ b/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToBottom.java
@@ -0,0 +1,10 @@
+package com.stuypulse.robot.commands.Elevator;
+
+import com.stuypulse.robot.constants.Settings.Elevator;
+
+public class ElevatorToBottom extends ElevatorToHeight {
+
+    public ElevatorToBottom() {
+        super(Elevator.MIN_HEIGHT);
+    }
+}
diff --git a/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToHeight.java b/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToHeight.java
new file mode 100644
index 0000000..0ce2a69
--- /dev/null
+++ b/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToHeight.java
@@ -0,0 +1,21 @@
+package com.stuypulse.robot.commands.Elevator;
+
+import com.stuypulse.robot.subsystems.Elevator.Elevator;
+
+import edu.wpi.first.wpilibj2.command.InstantCommand;   
+
+public class ElevatorToHeight extends InstantCommand {
+    private final Elevator elevator;
+    private final double targetHeight;
+
+    public ElevatorToHeight(double targetHeight){
+        elevator = Elevator.getInstance();
+        this.targetHeight = targetHeight;
+        
+        addRequirements(elevator);
+    }
+
+    public void initialize(){
+        elevator.setTargetHeight(targetHeight);
+    }
+}
diff --git a/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl1.java b/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl1.java
new file mode 100644
index 0000000..4c2cfdf
--- /dev/null
+++ b/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl1.java
@@ -0,0 +1,9 @@
+package com.stuypulse.robot.commands.Elevator;
+
+import com.stuypulse.robot.constants.Settings.Elevator;
+
+public class ElevatorToLvl1 extends ElevatorToHeight{
+    public ElevatorToLvl1(){
+        super(Elevator.L1);
+    }
+}
diff --git a/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl2.java b/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl2.java
new file mode 100644
index 0000000..5d825df
--- /dev/null
+++ b/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl2.java
@@ -0,0 +1,9 @@
+package com.stuypulse.robot.commands.Elevator;
+
+import com.stuypulse.robot.constants.Settings.Elevator;
+
+public class ElevatorToLvl2 extends ElevatorToHeight {
+    public ElevatorToLvl2(){
+        super(Elevator.L2);
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl3.java b/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl3.java
new file mode 100644
index 0000000..2c5928c
--- /dev/null
+++ b/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl3.java
@@ -0,0 +1,9 @@
+package com.stuypulse.robot.commands.Elevator;
+
+import com.stuypulse.robot.constants.Settings.Elevator;
+
+public class ElevatorToLvl3 extends ElevatorToHeight {
+    public ElevatorToLvl3(){
+        super(Elevator.L3);
+    }
+}
diff --git a/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl4.java b/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl4.java
new file mode 100644
index 0000000..ce32054
--- /dev/null
+++ b/src/main/java/com/stuypulse/robot/commands/Elevator/ElevatorToLvl4.java
@@ -0,0 +1,9 @@
+package com.stuypulse.robot.commands.Elevator;
+
+import com.stuypulse.robot.constants.Settings.Elevator;
+
+public class ElevatorToLvl4 extends ElevatorToHeight{
+    public ElevatorToLvl4(){
+        super(Elevator.L4);
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/com/stuypulse/robot/constants/Ports.java b/src/main/java/com/stuypulse/robot/constants/Ports.java
index fa3873f..fc6e62f 100644
--- a/src/main/java/com/stuypulse/robot/constants/Ports.java
+++ b/src/main/java/com/stuypulse/robot/constants/Ports.java
@@ -13,6 +13,12 @@ public interface Gamepad {
         int DEBUGGER = 2;
     }
 
+    public interface Elevator {
+        int LEFT = 0;
+        int RIGHT = 1;
+        int SWITCH = 2;
+    }
+  
     public interface Algae {
         int ROLLER_ID = 0; // update ports later -- def won't be 0
         int PIVOT_ID = 1; // update ports later
diff --git a/src/main/java/com/stuypulse/robot/constants/Settings.java b/src/main/java/com/stuypulse/robot/constants/Settings.java
index 238ff2a..d9048b7 100644
--- a/src/main/java/com/stuypulse/robot/constants/Settings.java
+++ b/src/main/java/com/stuypulse/robot/constants/Settings.java
@@ -5,9 +5,10 @@
 
 package com.stuypulse.robot.constants;
 
-import com.stuypulse.stuylib.network.SmartBoolean;
 import com.stuypulse.stuylib.network.SmartNumber;
 
+import edu.wpi.first.math.util.Units;
+
 /*-
  * File containing tunable settings for every subsystem on the robot.
  *
@@ -16,6 +17,71 @@
  */
 public interface Settings {
 
+    // checks the current RIO's serial number to determine which robot is running
+    public enum RobotType {
+
+        AUNT_MARY("0000000"),
+        SIM("");
+
+        public final String serialNum;
+
+        RobotType(String serialNum) {
+            this.serialNum = serialNum;
+        }
+
+        public static RobotType fromString(String serialNum) {
+            for (RobotType robot : RobotType.values()) {
+                if (robot.serialNum.equals(serialNum.toUpperCase())) {
+                    return robot;
+                }
+            }
+
+            return RobotType.SIM;
+        }
+    }
+
+    double DT = 0.020;
+
+    public interface Robot {
+        double kG = 100.0;
+    }
+
+    public interface Elevator {
+        double MIN_HEIGHT = 0.0;
+        double MAX_HEIGHT = 12.0;
+        double MAX_ACCELERATION = 2.0;
+        double MAX_VELOCITY = 3.0;
+        double ENCODER_CONVERSION_FACTOR = 0;
+        
+        double MASS = 25.0;
+        double GEARING = 1.0/9.0;
+        double DRUM_RADIUS = Units.inchesToMeters(1.0);
+
+        double L1 = 1;
+        double L2 = 2;
+        double L3 = 3;
+        double L4 = 4;
+         
+        double POSITION_CONVERSION_FACTOR = 1.0;
+        double VELOCITY_CONVERSION_FACTOR = 1.0;
+
+        double SCALE_FACTOR = 0.5;
+    
+        public interface PID {
+            SmartNumber kP = new SmartNumber("kP",1.5);
+            SmartNumber kI = new SmartNumber("kI",0.0);
+            SmartNumber kD = new SmartNumber("kD",0.2);
+        }
+
+        public interface FF {
+            SmartNumber kS = new SmartNumber("kS",0.20506);
+            SmartNumber kV = new SmartNumber("kV",3.7672);
+            SmartNumber kA = new SmartNumber("kA", 0.27);
+            SmartNumber kG = new SmartNumber("kG", 0.37);
+        }
+    }
+}
+
     public interface Algae {
         double RAISED_ANGLE = 0.0;          // CHANGE
         double PROCESSOR_ANGLE = 0.0;       // CHANGE 
@@ -44,4 +110,3 @@ public interface FF{
         }
     }
 }
-
diff --git a/src/main/java/com/stuypulse/robot/subsystems/Elevator/Elevator.java b/src/main/java/com/stuypulse/robot/subsystems/Elevator/Elevator.java
new file mode 100644
index 0000000..e25af01
--- /dev/null
+++ b/src/main/java/com/stuypulse/robot/subsystems/Elevator/Elevator.java
@@ -0,0 +1,45 @@
+package com.stuypulse.robot.subsystems.Elevator;
+
+import com.stuypulse.robot.Robot;
+import com.stuypulse.robot.constants.Settings.RobotType;
+
+import edu.wpi.first.wpilibj2.command.SubsystemBase;
+
+public abstract class Elevator extends SubsystemBase {
+
+    private static final Elevator instance;
+
+    static {
+        if (Robot.ROBOT == RobotType.AUNT_MARY) {
+            instance = new ElevatorImpl();
+        } else {
+            instance = new ElevatorSimu();
+        }
+    }
+
+    private final ElevatorVisualizer visualizer;
+
+    public static Elevator getInstance() {
+        return instance;
+    }    
+
+    public Elevator() {
+        visualizer = new ElevatorVisualizer();
+    }
+
+    public abstract void setTargetHeight(double height);
+    
+    public abstract double getTargetHeight();
+
+    public abstract double getHeight();
+
+    public abstract void stopElevator();
+
+    public abstract boolean atBottom();
+    
+    public void periodic() {
+        visualizer.update();
+    }
+
+}
+    
\ No newline at end of file
diff --git a/src/main/java/com/stuypulse/robot/subsystems/Elevator/ElevatorImpl.java b/src/main/java/com/stuypulse/robot/subsystems/Elevator/ElevatorImpl.java
new file mode 100644
index 0000000..31573ae
--- /dev/null
+++ b/src/main/java/com/stuypulse/robot/subsystems/Elevator/ElevatorImpl.java
@@ -0,0 +1,127 @@
+package com.stuypulse.robot.subsystems.Elevator;
+
+import com.revrobotics.RelativeEncoder;
+import com.revrobotics.spark.SparkLowLevel.MotorType;
+import com.revrobotics.spark.SparkMax;
+import com.revrobotics.spark.config.SparkMaxConfig;
+import com.stuypulse.robot.constants.Ports;
+import com.stuypulse.robot.constants.Settings;
+import com.stuypulse.stuylib.math.SLMath;
+import com.stuypulse.stuylib.network.SmartNumber;
+
+import edu.wpi.first.math.controller.ElevatorFeedforward;
+import edu.wpi.first.math.controller.ProfiledPIDController;
+import edu.wpi.first.math.trajectory.TrapezoidProfile;
+import edu.wpi.first.wpilibj.DigitalInput;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class ElevatorImpl extends Elevator {
+    private final SparkMax leftMotor;
+    private final SparkMax rightMotor;
+
+    private final DigitalInput bumpSwitch;
+
+    private final RelativeEncoder leftEncoder;
+    private final RelativeEncoder rightEncoder;
+
+    private final SmartNumber targetHeight;
+
+    private final SparkMaxConfig leftConfig;
+    private final SparkMaxConfig rightConfig;
+
+    private ElevatorFeedforward FF;
+    private ProfiledPIDController PID;
+
+    public ElevatorImpl() {
+        leftMotor = new SparkMax(Ports.Elevator.LEFT, MotorType.kBrushless);
+        rightMotor = new SparkMax(Ports.Elevator.RIGHT, MotorType.kBrushless);
+
+        rightConfig = new SparkMaxConfig();
+        leftConfig = new SparkMaxConfig();
+
+        bumpSwitch = new DigitalInput(Ports.Elevator.SWITCH);
+
+        targetHeight = new SmartNumber("Elevator/Target Height", 0);
+
+        rightConfig.encoder.positionConversionFactor(Settings.Elevator.POSITION_CONVERSION_FACTOR);
+        leftConfig.encoder.positionConversionFactor(Settings.Elevator.POSITION_CONVERSION_FACTOR);
+        rightConfig.idleMode(SparkMaxConfig.IdleMode.kBrake);
+        leftConfig.idleMode(SparkMaxConfig.IdleMode.kBrake);
+
+        leftEncoder = leftMotor.getEncoder();
+        rightEncoder = rightMotor.getEncoder();
+
+        rightMotor.configure(leftConfig, SparkMax.ResetMode.kResetSafeParameters, SparkMax.PersistMode.kPersistParameters);
+        leftMotor.configure(rightConfig, SparkMax.ResetMode.kResetSafeParameters, SparkMax.PersistMode.kPersistParameters);
+
+         FF = new ElevatorFeedforward(
+            Settings.Elevator.FF.kS.getAsDouble(), 
+            Settings.Elevator.FF.kG.getAsDouble(), 
+            Settings.Elevator.FF.kV.getAsDouble()
+        );
+
+        PID = new ProfiledPIDController(
+            Settings.Elevator.PID.kP.getAsDouble(), 
+            Settings.Elevator.PID.kI.getAsDouble(), 
+            Settings.Elevator.PID.kD.getAsDouble(), 
+            new TrapezoidProfile.Constraints(
+                Settings.Elevator.MAX_ACCELERATION, 
+                Settings.Elevator.MAX_VELOCITY
+            )
+        );
+    }
+    
+    @Override
+    public void setTargetHeight(double height) {
+        targetHeight.set(
+            SLMath.clamp(
+                height, 
+                Settings.Elevator.MIN_HEIGHT, 
+                Settings.Elevator.MAX_HEIGHT
+            )
+        );
+    }
+
+    @Override
+    public double getTargetHeight() {
+        return targetHeight.getAsDouble();
+    }
+
+    @Override
+    public double getHeight() {
+        return (leftEncoder.getPosition() + rightEncoder.getPosition()) / 2.0;
+    }
+
+    @Override
+    public void stopElevator() {
+        rightMotor.stopMotor();
+        leftMotor.stopMotor(); 
+    }
+    @Override
+    public boolean atBottom() {
+        return !bumpSwitch.get();
+    }
+    
+    public void reset() {
+        leftEncoder.setPosition(0);
+        rightEncoder.setPosition(0);
+    }
+
+    @Override
+    public void periodic() {
+        super.periodic();
+        final double PIDOutput = PID.calculate(getHeight(), targetHeight.doubleValue());
+        final double FFOutput = FF.calculate(PID.getSetpoint().velocity);
+
+        if (atBottom() && (PIDOutput + FFOutput) < 0) {
+            stopElevator();
+            reset();
+        } else {
+            leftMotor.setVoltage(PIDOutput + FFOutput);
+            rightMotor.setVoltage(PIDOutput + FFOutput);
+        }
+
+        SmartDashboard.putNumber("Elevator/Target Height", targetHeight.getAsDouble());
+        SmartDashboard.putNumber("Elevator/Height", getHeight());
+    }
+}
\ No newline at end of file
diff --git a/src/main/java/com/stuypulse/robot/subsystems/Elevator/ElevatorSimu.java b/src/main/java/com/stuypulse/robot/subsystems/Elevator/ElevatorSimu.java
new file mode 100644
index 0000000..34f241d
--- /dev/null
+++ b/src/main/java/com/stuypulse/robot/subsystems/Elevator/ElevatorSimu.java
@@ -0,0 +1,167 @@
+package com.stuypulse.robot.subsystems.Elevator;
+
+import java.util.Optional;
+
+import com.revrobotics.spark.config.SparkBaseConfig.IdleMode;
+import com.stuypulse.robot.constants.Settings;
+import com.stuypulse.stuylib.math.SLMath;
+import com.stuypulse.stuylib.network.SmartNumber;
+
+import edu.wpi.first.math.controller.ElevatorFeedforward;
+import edu.wpi.first.math.controller.PIDController;
+import edu.wpi.first.math.system.plant.DCMotor;
+import edu.wpi.first.math.util.Units;
+import edu.wpi.first.wpilibj.simulation.BatterySim;
+import edu.wpi.first.wpilibj.simulation.ElevatorSim;
+import edu.wpi.first.wpilibj.simulation.EncoderSim;
+import edu.wpi.first.wpilibj.simulation.PWMSim;
+import edu.wpi.first.wpilibj.simulation.RoboRioSim;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+import edu.wpi.first.wpilibj.Encoder;
+import edu.wpi.first.wpilibj.motorcontrol.PWMSparkMax;
+
+
+public class ElevatorSimu extends Elevator {
+    private final ElevatorSim sim;
+    private final SmartNumber targetHeight;
+    private final double minHeight, maxHeight;
+    private final SmartNumber maxAccel, maxVel;
+    private Optional<Double> voltageOverride;
+    private ElevatorFeedforward FF;
+    private PIDController PID;
+    private final EncoderSim simEncoder;
+    private final Encoder encoder;
+    private final DCMotor gearbox;
+    private final PWMSparkMax motor;
+    private final PWMSim motorSim;
+    
+    ElevatorSimu() {
+        targetHeight = new SmartNumber("Elevator/Target Height", 0);
+        minHeight = Settings.Elevator.MIN_HEIGHT;
+        maxHeight = Settings.Elevator.MAX_HEIGHT;
+        maxAccel = new SmartNumber("Elevator/Max Acceleration",Settings.Elevator.MAX_ACCELERATION);
+        maxVel = new SmartNumber("Elevator/Max Velocity", Settings.Elevator.MAX_VELOCITY);
+        gearbox = DCMotor.getNEO(2);
+        
+        motor = new PWMSparkMax(3);
+        motorSim = new PWMSim(motor);
+
+
+        // magic numbers
+        encoder = new Encoder(0, 1);
+
+        simEncoder = new EncoderSim(encoder);
+        
+        sim = new ElevatorSim(
+            gearbox,
+            Settings.Elevator.GEARING,
+            Settings.Elevator.MASS,
+            Settings.Elevator.DRUM_RADIUS,
+            Settings.Elevator.MIN_HEIGHT,
+            Settings.Elevator.MAX_HEIGHT,
+            true,
+            0.0
+        );
+        FF = new ElevatorFeedforward(
+            Settings.Elevator.FF.kS.getAsDouble(), 
+            Settings.Elevator.FF.kG.getAsDouble(), 
+            Settings.Elevator.FF.kV.getAsDouble(), 
+            Settings.Elevator.FF.kA.getAsDouble()
+        );
+        PID = new PIDController(
+            Settings.Elevator.PID.kP.getAsDouble(), 
+            Settings.Elevator.PID.kI.getAsDouble(), 
+            Settings.Elevator.PID.kD.getAsDouble()
+        );
+
+        voltageOverride = Optional.empty();
+
+        //
+        encoder.setDistancePerPulse(256);
+    }
+
+    public ElevatorSim getSim() {
+        return sim;
+    }
+    public EncoderSim getSimEncoder() {
+        return simEncoder;
+    }
+
+    @Override
+    public void setTargetHeight(double height) {
+        targetHeight.set(SLMath.clamp(height, minHeight, maxHeight));
+        voltageOverride = Optional.empty();
+    }
+
+    @Override
+    public double getTargetHeight() {
+        return targetHeight.doubleValue();
+    }
+
+    @Override
+    public boolean atBottom() {
+        return sim.hasHitLowerLimit();
+    }
+    
+    public boolean elevatorTop() {
+        return sim.hasHitUpperLimit();
+    }
+
+    @Override
+    public double getHeight() {
+        return sim.getPositionMeters();
+    }
+
+    @Override
+    public void stopElevator() {
+        sim.setInputVoltage(0.0);
+    }
+    public void elevatorIdleMode(IdleMode mode) {}
+    public void setVoltageOverride(double voltage) {
+        voltageOverride = Optional.of(voltage);
+    }
+    public void setConstraints(double maxVelocity, double maxAcceleration) {
+        this.maxVel.set(maxVelocity);
+        this.maxAccel.set(maxAcceleration);
+    }
+    public double calculateVoltage() {
+        final double FFOutput = FF.calculate(encoder.getDistance());
+        final double PIDOutput = PID.calculate(getHeight(), targetHeight.doubleValue());
+        // Combine outputs and set motor voltage
+        System.out.println("ff: " + FFOutput);
+        System.out.println("pid: " + PIDOutput);
+        return FFOutput + PIDOutput;
+    }
+    
+    public void periodic() {
+        super.periodic();
+        double voltage = calculateVoltage();
+
+        // sim.update(Settings.DT);
+        
+        if (atBottom() && voltage < 0 || elevatorTop() && voltage > 0) {
+            stopElevator();
+        } else {
+            sim.setInputVoltage(voltage);
+        }
+        
+        ElevatorVisualizer.getVisualizerInstance().update();
+
+        SmartDashboard.putNumber("Elevator/Target Height", getTargetHeight());
+        SmartDashboard.putNumber("Elevator/Current", sim.getCurrentDrawAmps());
+        SmartDashboard.putNumber("Elevator/Height", getHeight());
+    }
+
+    public void simulationPeriodic() {
+
+        // System.out.println(motorSim.getSpeed() + " * " + calculateVoltage() + " = " +motorSim.getSpeed() * calculateVoltage());
+        // sim.setInput(motorSim.getSpeed() * calculateVoltage());
+        
+        // simEncoder.setDistance(getHeight());
+
+        // motor.setVoltage(calculateVoltage());
+
+        RoboRioSim.setVInVoltage(BatterySim.calculateDefaultBatteryLoadedVoltage(sim.getCurrentDrawAmps()));
+        sim.update(Settings.DT);
+    }
+}
diff --git a/src/main/java/com/stuypulse/robot/subsystems/Elevator/ElevatorVisualizer.java b/src/main/java/com/stuypulse/robot/subsystems/Elevator/ElevatorVisualizer.java
new file mode 100644
index 0000000..129cc7c
--- /dev/null
+++ b/src/main/java/com/stuypulse/robot/subsystems/Elevator/ElevatorVisualizer.java
@@ -0,0 +1,168 @@
+package com.stuypulse.robot.subsystems.Elevator;
+
+import edu.wpi.first.wpilibj.util.Color;
+import edu.wpi.first.wpilibj.util.Color8Bit;
+
+import com.stuypulse.robot.constants.Settings;
+import com.stuypulse.stuylib.control.feedforward.ElevatorFeedforward;
+
+import edu.wpi.first.wpilibj.smartdashboard.Mechanism2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismLigament2d;
+import edu.wpi.first.wpilibj.smartdashboard.MechanismRoot2d;
+import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
+
+public class ElevatorVisualizer {
+
+    private static final ElevatorVisualizer instance;
+
+    static {
+        instance = new ElevatorVisualizer();
+    }
+
+    private final Mechanism2d elevator2d;
+
+    private final MechanismRoot2d elevatorBL;
+    private final MechanismRoot2d elevatorTR;
+
+    private final MechanismRoot2d outerBL;
+    private final MechanismRoot2d outerTR;
+
+    private final MechanismRoot2d innerBL;
+    private final MechanismRoot2d innerTR;
+
+    public static ElevatorVisualizer getVisualizerInstance(){
+        return instance;
+    } 
+
+    public ElevatorVisualizer() {
+
+        // Mechanism2d
+        elevator2d = new Mechanism2d(6, 25);
+        
+        // Elevator Frame
+
+        // bottom left node
+        elevatorBL = elevator2d.getRoot("Elevator BL", 0, 0);
+
+        elevatorBL.append(new MechanismLigament2d(
+            "Left Tower",
+            14, 
+            90,
+            10, 
+            new Color8Bit(Color.kOrange)
+        ));
+
+        elevatorBL.append(new MechanismLigament2d(
+            "Bottom Tower",
+            6, 
+            0,
+            10, 
+            new Color8Bit(Color.kOrange)
+        ));
+
+        // top right node
+        elevatorTR = elevator2d.getRoot("Elevator TR", 6, 14);
+
+        elevatorTR.append(new MechanismLigament2d(
+            "Right Tower", 
+            14,
+            -90,
+            10,
+            new Color8Bit(Color.kOrange)
+        ));
+
+        elevatorTR.append(new MechanismLigament2d(
+            "Top Side",
+            6,
+            180, 
+            10, 
+            new Color8Bit(Color.kOrange)));
+
+        //outerFrame
+
+        // bottom left node
+        outerBL = elevator2d.getRoot("Outer BL", 1, 1);
+        outerBL.append(new MechanismLigament2d(
+            "Left Side",
+            14,
+            90, 
+            10,
+            new Color8Bit(Color.kYellow)));
+    
+        outerBL.append(new MechanismLigament2d(
+            "Bottom Side",
+            4,
+            0,
+            10,
+            new Color8Bit(Color.kYellow)));
+        
+        // top right node
+        outerTR = elevator2d.getRoot("Outer TR", 5, 15);
+        outerTR.append(new MechanismLigament2d(
+            "Top Side",
+            4,
+            180,
+            10,
+            new Color8Bit(Color.kYellow)));
+
+        outerTR.append(new MechanismLigament2d(
+            "Right Side",
+            14,
+            -90,
+            10,
+            new Color8Bit(Color.kYellow)));
+        
+
+        //innerFrame
+
+        // bottom left node
+        innerBL = elevator2d.getRoot("Inner BL", 2, 2);
+
+        innerBL.append(new MechanismLigament2d(
+            "Left Side",
+            2,
+            90, 
+            10,
+            new Color8Bit(Color.kPink)));
+
+        innerBL.append(new MechanismLigament2d(
+            "Bottom Side",
+            2,
+            0,
+            10,
+            new Color8Bit(Color.kPink)));
+
+        // top right node
+        innerTR = elevator2d.getRoot("Inner TR", 4, 4);
+
+        innerTR.append(new MechanismLigament2d(
+            "Top Side",
+            2,
+            180,
+            10,
+            new Color8Bit(Color.kPink)));
+            
+        innerTR.append(new MechanismLigament2d(
+            "Right Side",
+            2,
+            -90, 
+            10,
+            new Color8Bit(Color.kPink)));
+
+
+        SmartDashboard.putData("Visualizers/Elevator", elevator2d);
+    }
+
+    public void update() {
+        // the middle will be at targetHeight
+        ElevatorSimu simu = ((ElevatorSimu) ElevatorSimu.getInstance());
+        System.out.println("distance: " + simu.getSim().getPositionMeters());
+
+        innerBL.setPosition(2, simu.getHeight() + 2);
+        innerTR.setPosition(4, simu.getHeight() + 4);
+
+        outerBL.setPosition(1, 1 + simu.getHeight() * Settings.Elevator.SCALE_FACTOR);
+        outerTR.setPosition(5, 15 + simu.getHeight() * Settings.Elevator.SCALE_FACTOR);
+    }
+
+}
\ No newline at end of file