Added a way to control the arm, lift, and wrist mechanisms.

Made the arm check the height of the lift and move to a different state based on the height of the lift
and made the wrist move a specific amount only when the arm passes a threshold.
Added new functions that allow me to quickly change the controller and buttons being used to control
different mechanisms.
Added a new function specificly for autonomouse which allows me to select the desired positon of every
mechanism.
Changed the name of almost every function and enumeration to better match the rest of the program.

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;}
+float LiftMech::get_lift_kp() {return liftKP;}