diff --git a/include/EZ-Template/rotation_mechanisms/rotation_calc.hpp b/include/EZ-Template/rotation_mechanisms/rotation_calc.hpp
index 9aa959d..e848f92 100644
--- a/include/EZ-Template/rotation_mechanisms/rotation_calc.hpp
+++ b/include/EZ-Template/rotation_mechanisms/rotation_calc.hpp
@@ -1,33 +1,58 @@
 #pragma once
 
+#include <vector>
 #include "api.h"
 #include "pros/adi.hpp"
 #include "pros/rotation.hpp"
 
 namespace ez{
 
+    enum class ControlPeriod {
+        driver,
+        auton
+    };
+    enum class Controller {
+        master,
+        slave
+    };
+    enum class Buttons {
+        arrow_buttons,
+        Y_B_buttons,
+        left_shoulder_buttons,
+        right_shoulder_buttons
+    };
+
     class ArmMech{
         private:
-        std::vector<int> armStates;
-        int numArmStates;
+        std::vector<int> levelOneArmStates;
+        std::vector<int> levelTwoArmStates;
+        std::vector<int> levelThreeArmStates;
+        std::vector<int> levelFourArmStates;
+        int numArmStates = 2;
         float armKP;
 
         public:
+        std::vector<int> selectedStates;
+        int targetPos;
         int currArmState;
         int armTarget;
         float armError;
         float armVelocity;
-
+        
         ArmMech();
-        ArmMech(std::initializer_list<int> armStates, int numArmStates);
 
+        void arm_driver_control(enum Controller, enum Buttons);
+        void arm_auton_control(int pickState);
         void cycle_states();
         void reset_state();
         void arm_control();
 
+        void set_arm_kp(float _armKP);
+        void set_state_values(std::vector<int> _levelOneArmStates, std::vector<int> _levelTwoArmStates, std::vector<int> _levelThreeArmStates, std::vector<int> _levelFourArmStates);
+
         int get_arm_state();
         int get_num_arm_states();
-        int get_arm_kp();
+        float get_arm_kp();
     };
 
     class LiftMech {
@@ -35,23 +60,34 @@ namespace ez{
         std::vector<int> liftStates;
         int numLiftStates;
         float liftKP;
+        std::vector<int> firstRange;
+        std::vector<int> secondRange; 
+        std::vector<int> thirdRange; 
+        std::vector<int> fourthRange;
 
         public:
         int currLiftState;
         int liftTarget;
+        int liftHeight;
         float liftError;
         float liftVelocity;
-
+    
         LiftMech();
-        LiftMech(std::initializer_list<int> liftStates, int numLiftStates);
+        LiftMech(std::initializer_list<int> _liftStates, int _numLiftStates);
 
+        void lift_driver_control(enum Controller, enum Buttons);
+        void lift_auton_control(int pickState);
         void cycle_states();
         void reset_state();
         void lift_control();
+        void lift_range_finder();
+
+        void set_lift_kp(float _liftKP);
+        void set_ranges(std::vector<int> _firstRange, std::vector<int> _secondRange, std::vector<int> _thirdRange, std::vector<int> _fourthRange);
 
         int get_lift_state();
         int get_num_lift_states();
-        int get_lift_kp();
+        float get_lift_kp();
     };
 
     class WristMech {
@@ -67,14 +103,18 @@ namespace ez{
         float wristVelocity;
 
         WristMech();
-        WristMech(std::initializer_list<int> wristStates, int numWristStates);
+        WristMech(std::initializer_list<int> _wristStates, int _numWristStates);
 
-        void cycle_states();
+        void wrist_driver_control(enum Controller, enum Buttons);
+
+        void rotate_wrist(int wristThreshold);
         void reset_state();
         void wrist_control();
 
         int get_wrist_state();
         int get_num_wrist_states();
-        int get_wrist_kp();
+        float get_wrist_kp();
+
+        void set_wrist_kp(float _wristKP);
     };
 };
\ No newline at end of file
diff --git a/include/setup/initialize.hpp b/include/setup/initialize.hpp
index 67c87ff..96d6a15 100644
--- a/include/setup/initialize.hpp
+++ b/include/setup/initialize.hpp
@@ -30,7 +30,7 @@ inline ez::Piston claw({'G',true});
  */
 inline pros::Rotation liftRotation({15});
 inline pros::Rotation armRotation({16});
-inline pros::Rotation wristRotatio({17});
+inline pros::Rotation wristRotation({17});
 
 inline pros::Optical colorSensorOne({18});
 inline pros::Optical colorSensorTwo({19});
diff --git a/src/EZ-Template/rotation_mechanisms/arm_rotation.cpp b/src/EZ-Template/rotation_mechanisms/arm_rotation.cpp
index b3df61c..98579cf 100644
--- a/src/EZ-Template/rotation_mechanisms/arm_rotation.cpp
+++ b/src/EZ-Template/rotation_mechanisms/arm_rotation.cpp
@@ -3,27 +3,182 @@
 
 using namespace ez;
 
-ArmMech::ArmMech() {}
-ArmMech::ArmMech(std::initializer_list<int> _armStates, int _numArmStates) {
-    armStates = _armStates;
-    numArmStates = _numArmStates;
+LiftMech liftObj;
+
+ArmMech::ArmMech() {
     currArmState = 0;
     armTarget = 0;
-    armKP = 0.0173f;
+    targetPos = 0;
+    set_arm_kp(0.0173f);
+}
+
+void ArmMech::arm_driver_control(Controller currController, Buttons buttonUsed) {
+
+    switch (currController)
+    {
+    case Controller::master:
+            
+        switch (buttonUsed)
+        {
+        case Buttons::arrow_buttons:
+            if (master.get_digital_new_press(DIGITAL_RIGHT)){
+                arm.move(127);
+            }
+            else if (master.get_digital_new_press(DIGITAL_DOWN)) {
+                arm.move(-127);
+            }
+            break;
+        case Buttons::Y_B_buttons:
+            if (master.get_digital_new_press(DIGITAL_Y)){
+                arm.move(127);
+            }
+            else if (master.get_digital_new_press(DIGITAL_B)) {
+                arm.move(-127);
+            }
+            break;
+        case Buttons::left_shoulder_buttons:
+            if (master.get_digital_new_press(DIGITAL_L1)){
+                arm.move(127);
+            }
+            else if (master.get_digital_new_press(DIGITAL_L2)) {
+                arm.move(-127);
+            }
+            break;
+        case Buttons::right_shoulder_buttons:
+            if (master.get_digital_new_press(DIGITAL_R1)){
+                arm.move(127);
+            }
+            else if (master.get_digital_new_press(DIGITAL_R2)) {
+                arm.move(-127);
+            }
+            break;
+        default:
+            break;
+        }
+        break;
+    case Controller::slave:
+            
+        switch (buttonUsed)
+        {
+        case Buttons::arrow_buttons:
+            if (slave.get_digital_new_press(DIGITAL_RIGHT)){
+                cycle_states();
+            }
+            else if (slave.get_digital_new_press(DIGITAL_DOWN)) {
+                reset_state();
+            }
+            break;
+        case Buttons::Y_B_buttons:
+            if (slave.get_digital_new_press(DIGITAL_Y)){
+                cycle_states();
+            }
+            else if (slave.get_digital_new_press(DIGITAL_B)) {
+                reset_state();
+            }
+            break;
+        case Buttons::left_shoulder_buttons:
+            if (slave.get_digital_new_press(DIGITAL_L1)){
+                cycle_states();
+            }
+            else if (slave.get_digital_new_press(DIGITAL_L2)) {
+                reset_state();
+            }
+            break;
+        case Buttons::right_shoulder_buttons:
+            if (slave.get_digital_new_press(DIGITAL_R1)){
+                cycle_states();
+            }
+            else if (slave.get_digital_new_press(DIGITAL_R2)) {
+                reset_state();
+            }
+            break;
+        default:
+            break;
+        }
+        break;
+    default:
+        break;
+    }
+}
+
+void ArmMech::arm_auton_control(int pickState) {
+    liftObj.lift_range_finder();
+
+    switch (targetPos)
+    {
+    case 0:
+        selectedStates = levelOneArmStates;
+        if (pickState == 1) {
+            selectedStates[1];
+        }
+        else {
+            selectedStates[0];
+        }
+        break;
+    case 1:
+        selectedStates = levelOneArmStates;
+        if (pickState == 1) {
+            selectedStates[1];
+        }
+        else {
+            selectedStates[0];
+        }
+        break;
+    case 2:
+        selectedStates = levelTwoArmStates;
+        if (pickState == 1) {
+            selectedStates[1];
+        }
+        else {
+            selectedStates[0];
+        }
+        break;
+    case 3:
+        selectedStates = levelThreeArmStates;
+        if (pickState == 1) {
+            selectedStates[1];
+        }
+        else {
+            selectedStates[0];
+        }
+        break;
+    default:
+        break;
+    }
+
 }
 
 void ArmMech::cycle_states() {
+    liftObj.lift_range_finder();
+
+    switch (targetPos)
+    {
+    case 0:
+        selectedStates = levelOneArmStates;
+        break;
+    case 1:
+        selectedStates = levelTwoArmStates;
+        break;
+    case 2:
+        selectedStates = levelThreeArmStates;
+        break;
+    case 3:
+        selectedStates = levelFourArmStates;
+        break;
+    default:
+        break;
+    }
     currArmState ++;
 
     if (currArmState == numArmStates) {
         currArmState = 0;
     }
-    armTarget = armStates[currArmState];
+    armTarget = selectedStates[currArmState];
 }
 
 void ArmMech::reset_state() {
     currArmState = 0;
-    armTarget = armStates[currArmState];
+    armTarget = selectedStates[currArmState];
 }
 
 void ArmMech::arm_control() {
@@ -32,6 +187,16 @@ void ArmMech::arm_control() {
     arm.move(armVelocity);
 }
 
-int ArmMech::get_arm_state() {return armStates[currArmState];}
+void ArmMech::set_state_values(std::vector<int> _levelOneArmStates, std::vector<int> _levelTwoArmStates, std::vector<int> _levelThreeArmStates, std::vector<int> _levelFourArmStates) {
+    levelOneArmStates = _levelOneArmStates;
+    levelTwoArmStates = _levelTwoArmStates;
+    levelThreeArmStates = _levelThreeArmStates;
+    levelFourArmStates = _levelFourArmStates;
+}
+void ArmMech::set_arm_kp(float _armKP) {
+    armKP = _armKP;
+}
+
+int ArmMech::get_arm_state() {return selectedStates[currArmState];}
 int ArmMech::get_num_arm_states() {return numArmStates;}
-int ArmMech::get_arm_kp() {return armKP;}
+float ArmMech::get_arm_kp() {return armKP;}
\ No newline at end of file
diff --git a/src/EZ-Template/rotation_mechanisms/lift_rotation.cpp b/src/EZ-Template/rotation_mechanisms/lift_rotation.cpp
index 0196997..62a8bc7 100644
--- a/src/EZ-Template/rotation_mechanisms/lift_rotation.cpp
+++ b/src/EZ-Template/rotation_mechanisms/lift_rotation.cpp
@@ -3,13 +3,124 @@
 
 using namespace ez;
 
+ArmMech armObjRange;
+
 LiftMech::LiftMech() {}
 LiftMech::LiftMech(std::initializer_list<int> _liftStates, int _numLiftStates) {
     liftStates = _liftStates;
     numLiftStates = _numLiftStates;
     currLiftState = 0;
     liftTarget = 0;
-    liftKP = 0.0173f;
+    set_lift_kp(0.0173f);
+}
+
+void LiftMech::lift_driver_control(Controller currController, Buttons buttonUsed) {
+    switch (currController)
+    {
+    case Controller::master:
+
+        switch (buttonUsed)
+        {
+        case Buttons::arrow_buttons:
+            if (master.get_digital(DIGITAL_RIGHT)) {
+                lift.move(127);
+            }
+            else if (master.get_digital(DIGITAL_DOWN)) {
+                lift.move(-127);
+            }
+            break;
+        case Buttons::Y_B_buttons:
+            if (master.get_digital(DIGITAL_Y)) {
+                lift.move(127);
+            }
+            else if (master.get_digital(DIGITAL_B)) {
+                lift.move(-127);
+            }
+            break;
+        case Buttons::left_shoulder_buttons:
+            if (master.get_digital(DIGITAL_L1)) {
+                lift.move(127);
+            }
+            else if (master.get_digital(DIGITAL_L2)) {
+                lift.move(-127);
+            }
+            break;
+        case Buttons::right_shoulder_buttons:
+            if (master.get_digital(DIGITAL_R1)) {
+                lift.move(127);
+            }
+            else if (master.get_digital(DIGITAL_R2)) {
+                lift.move(-127);
+            }
+            break;
+        default:
+            break;
+        }
+        break;
+    case Controller::slave:
+
+        switch (buttonUsed)
+        {
+        case Buttons::arrow_buttons:
+            if (slave.get_digital(DIGITAL_RIGHT)) {
+                cycle_states();
+            }
+            else if (slave.get_digital(DIGITAL_DOWN)) {
+                reset_state();
+            }
+            break;
+        case Buttons::Y_B_buttons:
+            if (slave.get_digital(DIGITAL_Y)) {
+                cycle_states();
+            }
+            else if (slave.get_digital(DIGITAL_B)) {
+                reset_state();
+            }
+            break;
+        case Buttons::left_shoulder_buttons:
+            if (slave.get_digital(DIGITAL_L1)) {
+                cycle_states();
+            }
+            else if (slave.get_digital(DIGITAL_L2)) {
+                reset_state();
+            }
+            break;
+        case Buttons::right_shoulder_buttons:
+            if (slave.get_digital(DIGITAL_R1)) {
+                cycle_states();
+            }
+            else if (slave.get_digital(DIGITAL_R2)) {
+                reset_state();
+            }
+            break;
+        default:
+            break;
+        }
+        break;
+    default:
+        break;
+    }
+}
+
+void LiftMech::lift_auton_control(int pickState) {
+    switch (pickState)
+    {
+    case 1:
+        liftTarget = liftStates[0];
+        break;
+    case 2:
+        liftTarget = liftStates[1];
+        break;
+    case 3:
+        liftTarget = liftStates[2];
+        break;
+    case 4:
+        liftTarget = liftStates[3];
+        break;
+    default:
+        liftTarget = liftStates[0];
+        break;
+    }
 }
 
 void LiftMech::cycle_states() {
@@ -32,6 +143,34 @@ void LiftMech::lift_control() {
     lift.move(liftVelocity);
 }
 
+void LiftMech::lift_range_finder() {
+    liftHeight = liftRotation.get_position();
+
+    if (liftHeight >= firstRange[0] && liftHeight <= firstRange[1]) {
+        armObjRange.targetPos = 0;
+    }
+    else if (liftHeight >= secondRange[0] && liftHeight <= secondRange[1]) {
+        armObjRange.targetPos = 1;
+    }
+    else if (liftHeight >= thirdRange[0] && liftHeight <= thirdRange[1]) {
+        armObjRange.targetPos = 2;
+    }
+    else if (liftHeight >= fourthRange[0] && liftHeight <= fourthRange[1]) {
+        armObjRange.targetPos = 3;
+    }
+}
+
+void LiftMech::set_lift_kp(float _liftKp) {
+    liftKP = _liftKp;
+}
+
+void LiftMech::set_ranges(std::vector<int> _firstRange, std::vector<int> _secondRange, std::vector<int> _thirdRange, std::vector<int> _fourthRange) {
+    firstRange = _firstRange;
+    secondRange = _secondRange;
+    thirdRange = _thirdRange;
+    fourthRange = _fourthRange;
+}
+
 int LiftMech::get_lift_state() {return liftStates[currLiftState];}
 int LiftMech::get_num_lift_states() {return numLiftStates;}
-int LiftMech::get_lift_kp() {return liftKP;}
\ No newline at end of file
+float LiftMech::get_lift_kp() {return liftKP;}
\ No newline at end of file
diff --git a/src/EZ-Template/rotation_mechanisms/wrist_rotation.cpp b/src/EZ-Template/rotation_mechanisms/wrist_rotation.cpp
index d52bfc6..54b4c88 100644
--- a/src/EZ-Template/rotation_mechanisms/wrist_rotation.cpp
+++ b/src/EZ-Template/rotation_mechanisms/wrist_rotation.cpp
@@ -9,16 +9,80 @@ WristMech::WristMech(std::initializer_list<int> _wristStates, int _numWristState
     numWristStates = _numWristStates;
     currWristState = 0;
     wristTarget = 0;
-    wristKP = 0.0173f;
+    set_wrist_kp(0.0173f);
 }
 
-void WristMech::cycle_states() {
-    currWristState ++;
+void WristMech::wrist_driver_control(Controller currController, Buttons buttonUsed) {
+    switch (currController)
+    {
+    case Controller::master:
 
-    if (currWristState == numWristStates) {
-        currWristState = 0;
+        switch (buttonUsed)
+        {
+        case Buttons::arrow_buttons:
+            if (master.get_digital(DIGITAL_DOWN)) {
+                reset_state();
+            }
+            break;
+        case Buttons::Y_B_buttons:
+            if (master.get_digital(DIGITAL_Y)) {
+                reset_state();
+            }
+            break;
+        case Buttons::left_shoulder_buttons:
+            if (master.get_digital(DIGITAL_L1)) {
+                reset_state();
+            }
+            break;
+        case Buttons::right_shoulder_buttons:
+            if (master.get_digital(DIGITAL_R1)) {
+                reset_state();
+            }
+            break;
+        default:
+            break;
+        }
+        break;
+    case Controller::slave:
+
+        switch (buttonUsed)
+        {
+        case Buttons::arrow_buttons:
+            if (slave.get_digital(DIGITAL_DOWN)) {
+                reset_state();
+            }
+            break;
+        case Buttons::Y_B_buttons:
+            if (slave.get_digital(DIGITAL_Y)) {
+                reset_state();
+            }
+            break;
+        case Buttons::left_shoulder_buttons:
+            if (slave.get_digital(DIGITAL_L1)) {
+                reset_state();
+            }
+            break;
+        case Buttons::right_shoulder_buttons:
+            if (slave.get_digital(DIGITAL_R1)) {
+                reset_state();
+            }
+            break;
+        default:
+            break;
+        }
+        break;
+    default:
+        break;
+    }
+}
+
+void WristMech::rotate_wrist(int wristThreshold) {
+    if (armRotation.get_position() > wristThreshold) {
+        wristTarget = wristStates[1];
+    }
+    else if (armRotation.get_position() < wristThreshold) {
+        wristTarget = wristStates[0];
     }
-    wristTarget = wristStates[currWristState];
 }
 
 void WristMech::reset_state() {
@@ -34,4 +98,8 @@ void WristMech::wrist_control() {
 
 int WristMech::get_wrist_state() {return wristStates[currWristState];}
 int WristMech::get_num_wrist_states() {return numWristStates;}
-int WristMech::get_wrist_kp() {return wristKP;}
\ No newline at end of file
+float WristMech::get_wrist_kp() {return wristKP;}
+
+void WristMech::set_wrist_kp(float _wristKP) {
+    wristKP = _wristKP;
+}
\ No newline at end of file
diff --git a/src/main.cpp b/src/main.cpp
index 47ee5b6..747eaad 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -25,18 +25,14 @@ ez::Drive chassis(
 // ez::tracking_wheel vert_tracker(9, 2.75, 4.0);   // This tracking wheel is parallel to the drive wheels
 
 // Arm constructor
-ArmMech armSetup(
-    
-  {0, 300}, 
-  
-  2);
+ArmMech armSetup();
 
 // Lift constructor
 LiftMech liftSetup(
 
-  {0, 100, 200},
+  {0, 100, 200, 300},
 
-  3);
+  4);
 
 // Wrist constructor
 WristMech wristSetup(
@@ -57,7 +53,12 @@ void initialize() {
 
   pros::delay(500);  // Stop the user from doing anything while legacy ports configure
 
+  armObj.set_state_values({1, 2}, {1, 2}, {1, 2}, {1, 2});
+
   armRotation.reset_position();
+  liftRotation.reset_position();
+  wristRotation.reset_position();
+
   pros::Task liftControlTasck([&]{
     while (true) {
       armObj.arm_control();
@@ -286,6 +287,13 @@ void opcontrol() {
     pistonControl.control_piston(Pistons::claw_piston, ControlPeriod::driver, 
       Controller::slave, Buttons::arrow_buttons);
 
+    armObj.arm_driver_control(Controller::master, Buttons::left_shoulder_buttons);
+    liftObj.lift_driver_control(Controller::master, Buttons::right_shoulder_buttons);
+    
+    armObj.arm_driver_control(Controller::slave, Buttons::left_shoulder_buttons);
+    liftObj.lift_driver_control(Controller::slave, Buttons::right_shoulder_buttons);
+    wristObj.wrist_driver_control(Controller::slave, Buttons::arrow_buttons);
+    wristObj.rotate_wrist(90);
     // chassis.opcontrol_tank();  // Tank control
     chassis.opcontrol_arcade_standard(ez::SPLIT);   // Standard split arcade
     // chassis.opcontrol_arcade_standard(ez::SINGLE);  // Standard single arcade