Fixed a problem where the OPControl was trying to pull values from an array that never had any values to begin with

causeing a segmentation fault and crashing the brain.
Also changed how the function lift_range_finder worked.  Instead of just makeing a variable eqaul to a number it also returns the value
so we can use it outside of the function.
We also added actual values to all the arrays.

include/EZ-Template/pneumatics.hpp

@@ -6,24 +6,35 @@
 #include "EZ-Template/piston.hpp"
 
 namespace ez {
-
-    enum class Pistons {
-        claw_piston
-    };
-    enum class ControlPeriod {
+     /**
+     * Enumeration to hold the current part of a match were in.
+     */
+    enum class ControlPeriodP {
         driver,
         auton
     };
-    enum class Controller {
+    /**
+     * Enumeration to hold the different controllers avalable. 
+     */
+    enum class ControllerP {
         master,
         slave
     };
-    enum class Buttons {
+    /**
+     * Enumeration to hold the different buttons to use.
+     */
+    enum class ButtonsP {
         arrow_buttons,
         Y_B_buttons,
         left_shoulder_buttons,
         right_shoulder_buttons
     };
+    /**
+     * Enumeration to hold all the pistons avalable.
+     */
+    enum class PistonsP {
+        claw_piston
+    };
 
     class Pneumatics{
 
@@ -34,7 +45,7 @@ namespace ez {
 
         Pneumatics();
 
-        void control_piston(enum Pistons, enum ControlPeriod, enum Controller, enum Buttons);
+        void control_piston(enum PistonsP, enum ControlPeriodP, enum ControllerP, enum ButtonsP);
 
         bool get_piston_state();
         void set_piston_state(bool value);

include/EZ-Template/rotation_mechanisms/rotation_calc.hpp

@@ -1,62 +1,123 @@
 #pragma once
 
+#include <vector>
 #include "api.h"
+//#include "main.h"
 #include "pros/adi.hpp"
 #include "pros/rotation.hpp"
 
 namespace ez{
+    /**
+     * Enumeration to hold the current part of a match were in.
+     */
+    enum class ControlPeriodR {
+        driver,
+        auton
+    };
+    /**
+     * Enumeration to hold the different controllers avalable. 
+     */
+    enum class ControllerR {
+        master,
+        slave
+    };
+    /**
+     * Enumeration to hold the different buttons to use.
+     */
+    enum class ButtonsR {
+        arrow_buttons,
+        Y_B_buttons,
+        left_shoulder_buttons,
+        right_shoulder_buttons
+    };
 
+    /**
+     * Arm mechanism class.
+     * 
+     * Sets up all constructors, variables, and functions.
+     */
     class ArmMech{
         private:
-        std::vector<int> armStates;
-        int numArmStates;
+        // Arrays to hold different values for the arm.
+        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; // Array for the arm to actaully use.
+        int targetPos; // Value to be changed based on the height of the lift
         int currArmState;
         int armTarget;
         float armError;
         float armVelocity;
         
-        ArmMech();
-        ArmMech(std::initializer_list<int> armStates, int numArmStates);
+        ArmMech(); // Defualt constructor
 
+        void arm_driver_control(enum ControllerR, enum ButtonsR);
+        void arm_auton_control(int pickState);
         void cycle_states();
         void reset_state();
-        void arm_control();
+        void arm_control(); // Computes the velocity for the arm to move at.
+
+        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();
     };
 
+    /**
+     * List mechanism class.
+     * 
+     * Sets up all constructors, variables, and functions.
+     */
     class LiftMech {
         private:
-        std::vector<int> liftStates;
+        std::vector<int> liftStates; // Array the lift uses to move.
+        std::vector<int> firstRange;
+        std::vector<int> secondRange; 
+        std::vector<int> thirdRange; 
+        std::vector<int> fourthRange;
         int numLiftStates;
         float liftKP;
 
         public:
+        int liftHeight; // Current height of the lift in centdegrees
         int currLiftState;
         int liftTarget;
         float liftError;
         float liftVelocity;
     
-        LiftMech();
-        LiftMech(std::initializer_list<int> liftStates, int numLiftStates);
+        LiftMech(); // Default constructor
+        LiftMech(std::initializer_list<int> _liftStates, int _numLiftStates); // Constructor to initialze array and amount of values in array.
 
+        void lift_driver_control(enum ControllerR, enum ButtonsR);
+        void lift_auton_control(int pickState);
         void cycle_states();
         void reset_state();
-        void lift_control();
+        void lift_control(); // Computes the velocity for the lift to move at.  
+        int lift_range_finder(); // Finds the ranges for the arm to use 
+
+        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();
     };
 
+    /**
+     * Wrist mechanism class.
+     * 
+     * Sets up all constructors, variables, and functions.
+     */
     class WristMech {
         private:
-        std::vector<int> wristStates;
+        std::vector<int> wristStates; // Array the wrist uses to move
         int numWristStates;
         float wristKP;
 
@@ -66,15 +127,19 @@ namespace ez{
         float wristError;
         float wristVelocity;
 
-        WristMech();
-        WristMech(std::initializer_list<int> wristStates, int numWristStates);
+        WristMech(); // Default constuctor
+        WristMech(std::initializer_list<int> _wristStates, int _numWristStates); // Constructor to initialze array and amount of values in array.
 
-        void cycle_states();
+        void wrist_driver_control(enum ControllerR, enum ButtonsR);
+
+        void rotate_wrist(int wristThreshold); // Rotates the wrist based where the arm is.
         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);
     };
 };

include/setup/initialize.hpp

@@ -8,29 +8,39 @@
 #define CONFIG_H
 
 /**
- * 
+ * Sets up the master and slave controllers.
  */
 inline pros::Controller master(pros::E_CONTROLLER_MASTER);
 inline pros::Controller slave(pros::E_CONTROLLER_PARTNER);
 
 /**
- * Declare mechinism motors
+ * Declares mechinism motors and motor groups.
  */
-inline pros::MotorGroup lift({10, 11});
-inline pros::MotorGroup wrist({13, 14});
-inline pros::Motor arm({12});
+inline pros::MotorGroup lift({15, -19});
+inline pros::MotorGroup wrist({1, 9});
+inline pros::Motor arm({6});
 
 /**
- * Declare pistons here
+ * Declares piston mechanisms.
  */
 inline ez::Piston claw({'G',true});
 
 /**
- * Declare sensors here
+ * Declare limit switches and other ADI sensors
  */
-inline pros::Rotation liftRotation({15});
-inline pros::Rotation armRotation({16});
-inline pros::Rotation wristRotatio({17});
+inline pros::ADIButton liftReset({'A'});
+inline pros::ADIButton armReset({'B'});
+
+/**
+ * Declares all sensors.
+ * 
+ * [1] - Rotation sensors
+ * 
+ * [2] - Color sensors
+ */
+inline pros::Rotation liftRotation({17});
+inline pros::Rotation armRotation({8});
+inline pros::Rotation wristRotation({21});
 
 inline pros::Optical colorSensorOne({18});
 inline pros::Optical colorSensorTwo({19});

src/EZ-Template/pneumatics.cpp

@@ -5,22 +5,22 @@ using namespace ez;
 
 Pneumatics::Pneumatics(){}
 
-void Pneumatics::control_piston(Pistons activePiston, ControlPeriod currPeriod, Controller currController, Buttons buttonUsed) {
+void Pneumatics::control_piston(PistonsP activePiston, ControlPeriodP currPeriod, ControllerP currController, ButtonsP buttonUsed) {
 
-    if (currPeriod == ControlPeriod::driver) {
+    if (currPeriod == ControlPeriodP::driver) {
         switch (activePiston)
         {
-        case Pistons::claw_piston:
+        case PistonsP::claw_piston:
             /*More code can go here if the claw piston is selected*/
 
             switch (currController)
             {
-            case Controller::master:
+            case ControllerP::master:
                 /*More code can go here if we want to use the master controller*/
 
                 switch (buttonUsed)
                 {
-                case Buttons::arrow_buttons:
+                case ButtonsP::arrow_buttons:
                     if (master.get_digital(DIGITAL_RIGHT)) {
                         claw.set(true);
                     }
@@ -28,7 +28,7 @@ void Pneumatics::control_piston(Pistons activePiston, ControlPeriod currPeriod,
                         claw.set(false);
                     }
                     break;
-                case Buttons::Y_B_buttons:
+                case ButtonsP::Y_B_buttons:
                     if (master.get_digital(DIGITAL_Y)) {
                         claw.set(true);
                     }
@@ -36,7 +36,7 @@ void Pneumatics::control_piston(Pistons activePiston, ControlPeriod currPeriod,
                         claw.set(false);
                     }
                     break;
-                case Buttons::left_shoulder_buttons:
+                case ButtonsP::left_shoulder_buttons:
                     if (master.get_digital(DIGITAL_L1)) {
                         claw.set(true);
                     }
@@ -44,7 +44,7 @@ void Pneumatics::control_piston(Pistons activePiston, ControlPeriod currPeriod,
                         claw.set(false);
                     }
                     break;
-                case Buttons::right_shoulder_buttons:
+                case ButtonsP::right_shoulder_buttons:
                     if (master.get_digital(DIGITAL_R1)) {
                         claw.set(true);
                     }
@@ -56,11 +56,11 @@ void Pneumatics::control_piston(Pistons activePiston, ControlPeriod currPeriod,
                     break;
                 }
                 break;
-            case Controller::slave:
+            case ControllerP::slave:
 
                 switch (buttonUsed)
                 {
-                case Buttons::arrow_buttons:
+                case ButtonsP::arrow_buttons:
                     if (slave.get_digital(DIGITAL_RIGHT)) {
                         claw.set(true);
                     }
@@ -68,7 +68,7 @@ void Pneumatics::control_piston(Pistons activePiston, ControlPeriod currPeriod,
                         claw.set(false);
                     }
                     break;
-                case Buttons::Y_B_buttons:
+                case ButtonsP::Y_B_buttons:
                     if (slave.get_digital(DIGITAL_Y)) {
                         claw.set(true);
                     }
@@ -76,7 +76,7 @@ void Pneumatics::control_piston(Pistons activePiston, ControlPeriod currPeriod,
                         claw.set(false);
                     }
                     break;
-                case Buttons::left_shoulder_buttons:
+                case ButtonsP::left_shoulder_buttons:
                     if (slave.get_digital(DIGITAL_L1)) {
                         claw.set(true);
                     }
@@ -84,7 +84,7 @@ void Pneumatics::control_piston(Pistons activePiston, ControlPeriod currPeriod,
                         claw.set(false);
                     }
                     break;
-                case Buttons::right_shoulder_buttons:
+                case ButtonsP::right_shoulder_buttons:
                     if (slave.get_digital(DIGITAL_R1)) {
                         claw.set(true);
                     }
@@ -111,7 +111,7 @@ void Pneumatics::control_piston(Pistons activePiston, ControlPeriod currPeriod,
     else {
         switch (activePiston)
         {
-        case Pistons::claw_piston:
+        case PistonsP::claw_piston:
             /*code here*/
             break;
         default:

src/EZ-Template/rotation_mechanisms/arm_rotation.cpp

@@ -3,35 +3,267 @@
 
 using namespace ez;
 
-ArmMech::ArmMech() {}
-ArmMech::ArmMech(std::initializer_list<int> _armStates, int _numArmStates) {
-    armStates = _armStates;
-    numArmStates = _numArmStates;
+LiftMech liftRangeAccess; // Lift object to use lift variables and functions
+
+/**
+ * Default constructor
+ */
+ArmMech::ArmMech() {
     currArmState = 0;
     armTarget = 0;
-    armKP = 0.0173f;
+    targetPos = 0;
+    set_arm_kp(0.0173f);
+
+    selectedStates = {0, 0}; // Sets default array values
 }
 
+/**
+ * control the arm during driver
+ * 
+ * \param currController
+ *        Input the controller being used
+ * \param buttonUsed
+ *        Select the button config you want use
+ */
+void ArmMech::arm_driver_control(ControllerR currController, ButtonsR buttonUsed) {
+
+    switch (currController) // Switch that check which controller was selected
+    {
+    case ControllerR::master:
+        // Code you want to run when the master controller is selcted
+        switch (buttonUsed)
+        {
+        case ButtonsR::arrow_buttons:
+        // If the arrow_buttons are selected run this code
+            if (master.get_digital(DIGITAL_RIGHT)){
+                arm.move(127);
+            }
+            else if (master.get_digital(DIGITAL_DOWN)) {
+                arm.move(-127);
+            }
+            break;
+        case ButtonsR::Y_B_buttons:
+        // If the Y_B_buttons are selected run this code
+            if (master.get_digital(DIGITAL_Y)){
+                arm.move(127);
+            }
+            else if (master.get_digital(DIGITAL_B)) {
+                arm.move(-127);
+            }
+            break;
+        case ButtonsR::left_shoulder_buttons:
+        // If the left-shoulder_buttons are selected run this code
+            if (master.get_digital(DIGITAL_L1)){
+                arm.move(127);
+            }
+            else if (master.get_digital(DIGITAL_L2)) {
+                arm.move(-127);
+            }
+            break;
+        case ButtonsR::right_shoulder_buttons:
+        // If the right_shoulder_buttons are selected run this code
+            if (master.get_digital(DIGITAL_R1)){
+                arm.move(127);
+            }
+            else if (master.get_digital(DIGITAL_R2)) {
+                arm.move(-127);
+            }
+            break;
+        default:
+            break;
+        }
+        break;
+    case ControllerR::slave:
+        // Code you want to run when the slave controller is selected    
+        switch (buttonUsed)
+        {
+        case ButtonsR::arrow_buttons:
+        // If the arrow_buttons are selected run this code
+            if (slave.get_digital(DIGITAL_RIGHT)){
+                cycle_states();
+            }
+            else if (slave.get_digital(DIGITAL_DOWN)) {
+                reset_state();
+            }
+            break;
+        case ButtonsR::Y_B_buttons:
+        // If the Y_B_buttons are selected run this code
+            if (slave.get_digital(DIGITAL_Y)){
+                cycle_states();
+            }
+            else if (slave.get_digital(DIGITAL_B)) {
+                reset_state();
+            }
+            break;
+        case ButtonsR::left_shoulder_buttons:
+        // If the left_shoulder_buttons are selected run this code
+            if (slave.get_digital(DIGITAL_L1)){
+                cycle_states();
+            }
+            else if (slave.get_digital(DIGITAL_L2)) {
+                reset_state();
+            }
+            break;
+        case ButtonsR::right_shoulder_buttons:
+        // If the right_shoulder_buttons are selected run this code
+            if (slave.get_digital(DIGITAL_R1)){
+                cycle_states();
+            }
+            else if (slave.get_digital(DIGITAL_R2)) {
+                reset_state();
+            }
+            break;
+        default:
+            break;
+        }
+        break;
+    default:
+        break;
+    }
+}
+
+/**
+ * Control the arm during auton
+ * 
+ * \param pickState
+ *        Variable to move the arm. Input: 1 to move or Input: 0 which won't move it
+ */
+void ArmMech::arm_auton_control(int pickState) {
+    liftRangeAccess.lift_range_finder(); // Returns a value 0 - 3 based on where the lift is currently
+
+    switch (targetPos)
+    {
+    case 0:
+        selectedStates = levelOneArmStates; // Makes the main array equal the level one array
+        if (pickState == 1) {
+            armTarget = selectedStates[1]; // Makes the target value the second in the array
+        }
+        else {
+            armTarget = selectedStates[0]; // Makes the target value the first in the array
+        }
+        break;
+    case 1:
+        selectedStates = levelOneArmStates; // Make the main array equal the level two array
+        if (pickState == 1) {
+            armTarget = selectedStates[1];
+        }
+        else {
+            armTarget = selectedStates[0];
+        }
+        break;
+    case 2:
+        selectedStates = levelTwoArmStates; // Makes the main array equal the level three array
+        if (pickState == 1) {
+            armTarget = selectedStates[1];
+        }
+        else {
+            armTarget = selectedStates[0];
+        }
+        break;
+    case 3:
+        selectedStates = levelThreeArmStates; // Makes the main array equal the level four array
+        if (pickState == 1) {
+            armTarget = selectedStates[1];
+        }
+        else {
+            armTarget = selectedStates[0];
+        }
+        break;
+    default:
+        break;
+    }
+
+}
+
+/**
+ * Function to cycle through each value in the array
+ */
 void ArmMech::cycle_states() {
+    liftRangeAccess.lift_range_finder(); // Returns a value 0 - 3 based on were the lift is currently
+
+    // Checks the array to make sure it isn't empty to prevent a segmentation fault(trying to access unassigned memory)
+    if (!selectedStates.empty()) {
+        // Based on the value returned by the lift_range_finder function change the array to match the level of the lift
+        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) {
+    if (currArmState >= numArmStates) {
         currArmState = 0;
     }
-    armTarget = armStates[currArmState];
+    armTarget = selectedStates[currArmState]; // Makes the target value equal the next spot in the array
 }
 
+/**
+ * Function reset the array indexer to zero
+ */
 void ArmMech::reset_state() {
     currArmState = 0;
-    armTarget = armStates[currArmState];
+
+    // Checks the array to make sure it isn't empty to prevent a segmentation fault(trying to access unassigned memory)
+    if (!selectedStates.empty()) {
+        armTarget = selectedStates[currArmState]; // Makes the target value equal the first value in the array
+    }
 }
 
+/**
+ * Function to set the velocity of the arm
+ */
 void ArmMech::arm_control() {
-    armError = armTarget - armRotation.get_position();
-    armVelocity = get_arm_kp() * armError;
-    arm.move(armVelocity);
+    armError = armTarget - armRotation.get_position(); // Sets the error value
+    armVelocity = get_arm_kp() * armError; // Sets the velocity
+    arm.move(armVelocity); // moves the arm
 }
 
-int ArmMech::get_arm_state() {return armStates[currArmState];}
+/**
+ * Setup the four different level arrays
+ * 
+ * \param _levelOneArmStates
+ *        The array used if the lift is at level one  
+ * \param _levelTwoArmStates
+ *        The array used if the lift is at level two  
+ * \param _levelThreeArmStates
+ *        The array used if the lift is at level three  
+ * \param _levelFourArmStates
+ *        The array used if the lift is at level four
+ */
+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;
+}
+/**
+ * Gives the ability to set the private arm kp value
+ * 
+ * \param _armKP
+ *        The input value that is then used for the kp value
+ */
+void ArmMech::set_arm_kp(float _armKP) {
+    armKP = _armKP;
+}
+
+int ArmMech::get_arm_state() {
+    if (selectedStates.empty()) {
+        return 0;
+    }
+    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;}

src/EZ-Template/rotation_mechanisms/lift_rotation.cpp

@@ -3,27 +3,149 @@
 
 using namespace ez;
 
-LiftMech::LiftMech() {}
+ArmMech armObjRange;
+
+LiftMech::LiftMech() {
+    liftHeight = 0;
+    liftError = 0;
+    liftVelocity = 0;
+
+    liftStates = {0, 0, 0, 0};
+}
 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(ControllerR currController, ButtonsR buttonUsed) {
+    switch (currController)
+    {
+    case ControllerR::master:
+
+        switch (buttonUsed)
+        {
+        case ButtonsR::arrow_buttons:
+            if (master.get_digital(DIGITAL_RIGHT)) {
+                lift.move(127);
+            }
+            else if (master.get_digital(DIGITAL_DOWN)) {
+                lift.move(-127);
+            }
+            break;
+        case ButtonsR::Y_B_buttons:
+            if (master.get_digital(DIGITAL_Y)) {
+                lift.move(127);
+            }
+            else if (master.get_digital(DIGITAL_B)) {
+                lift.move(-127);
+            }
+            break;
+        case ButtonsR::left_shoulder_buttons:
+            if (master.get_digital(DIGITAL_L1)) {
+                lift.move(127);
+            }
+            else if (master.get_digital(DIGITAL_L2)) {
+                lift.move(-127);
+            }
+            break;
+        case ButtonsR::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 ControllerR::slave:
+
+        switch (buttonUsed)
+        {
+        case ButtonsR::arrow_buttons:
+            if (slave.get_digital(DIGITAL_RIGHT)) {
+                cycle_states();
+            }
+            else if (slave.get_digital(DIGITAL_DOWN)) {
+                reset_state();
+            }
+            break;
+        case ButtonsR::Y_B_buttons:
+            if (slave.get_digital(DIGITAL_Y)) {
+                cycle_states();
+            }
+            else if (slave.get_digital(DIGITAL_B)) {
+                reset_state();
+            }
+            break;
+        case ButtonsR::left_shoulder_buttons:
+            if (slave.get_digital(DIGITAL_L1)) {
+                cycle_states();
+            }
+            else if (slave.get_digital(DIGITAL_L2)) {
+                reset_state();
+            }
+            break;
+        case ButtonsR::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() {
     currLiftState ++;
 
-    if (currLiftState == numLiftStates) {
-        currLiftState = 0;
+    if (!liftStates.empty()) {
+        if (currLiftState == numLiftStates) {
+            currLiftState = 0;
+        }
+        liftTarget = liftStates[currLiftState];
     }
-    liftTarget = liftStates[currLiftState];
 }
 
 void LiftMech::reset_state() {
     currLiftState = 0;
-    liftTarget = liftStates[currLiftState];
+
+    if (!liftStates.empty()) {
+        liftTarget = liftStates[currLiftState];
+    }
 }
 
 void LiftMech::lift_control() {
@@ -32,6 +154,38 @@ void LiftMech::lift_control() {
     lift.move(liftVelocity);
 }
 
-int LiftMech::get_lift_state() {return liftStates[currLiftState];}
+int LiftMech::lift_range_finder() {
+    liftHeight = liftRotation.get_position();
+
+    if (firstRange.size() >= 2 && liftHeight >= firstRange[0] && liftHeight <= firstRange[1]) {
+        return armObjRange.targetPos = 0;
+    }
+    else if (secondRange.size() >= 2 && liftHeight >= secondRange[0] && liftHeight <= secondRange[1]) {
+        return armObjRange.targetPos = 1;
+    }
+    else if (thirdRange.size() >= 2 && liftHeight >= thirdRange[0] && liftHeight <= thirdRange[1]) {
+        return armObjRange.targetPos = 2;
+    }
+    else if (fourthRange.size() >= 2 && liftHeight >= fourthRange[0] && liftHeight <= fourthRange[1]) {
+        return 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() {
+    if (!liftStates.empty()) {
+        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;}

src/EZ-Template/rotation_mechanisms/wrist_rotation.cpp

@@ -3,35 +3,114 @@
 
 using namespace ez;
 
-WristMech::WristMech(){}
+WristMech::WristMech() {
+    wristError = 0;
+    wristVelocity = 0;
+
+    wristStates = {0, 0};
+}
 WristMech::WristMech(std::initializer_list<int> _wristStates, int _numWristStates) {
     wristStates = _wristStates;
     numWristStates = _numWristStates;
     currWristState = 0;
     wristTarget = 0;
-    wristKP = 0.0173f;
+    set_wrist_kp(0.0173f);
 }
 
-void WristMech::cycle_states() {
-    currWristState ++;
+void WristMech::wrist_driver_control(ControllerR currController, ButtonsR buttonUsed) {
+    switch (currController)
+    {
+    case ControllerR::master:
 
-    if (currWristState == numWristStates) {
-        currWristState = 0;
+        switch (buttonUsed)
+        {
+        case ButtonsR::arrow_buttons:
+            if (master.get_digital(DIGITAL_DOWN)) {
+                reset_state();
+            }
+            break;
+        case ButtonsR::Y_B_buttons:
+            if (master.get_digital(DIGITAL_Y)) {
+                reset_state();
+            }
+            break;
+        case ButtonsR::left_shoulder_buttons:
+            if (master.get_digital(DIGITAL_L1)) {
+                reset_state();
+            }
+            break;
+        case ButtonsR::right_shoulder_buttons:
+            if (master.get_digital(DIGITAL_R1)) {
+                reset_state();
+            }
+            break;
+        default:
+            break;
+        }
+        break;
+    case ControllerR::slave:
+
+        switch (buttonUsed)
+        {
+        case ButtonsR::arrow_buttons:
+            if (slave.get_digital(DIGITAL_DOWN)) {
+                reset_state();
+            }
+            break;
+        case ButtonsR::Y_B_buttons:
+            if (slave.get_digital(DIGITAL_Y)) {
+                reset_state();
+            }
+            break;
+        case ButtonsR::left_shoulder_buttons:
+            if (slave.get_digital(DIGITAL_L1)) {
+                reset_state();
+            }
+            break;
+        case ButtonsR::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() {
     currWristState = 0;
-    wristTarget = wristStates[currWristState];
+    if (!wristStates.empty()) {
+        wristTarget = wristStates[currWristState];
+    }
 }
 
 void WristMech::wrist_control() {
-    wristError = wristTarget - armRotation.get_position();
+    wristError = wristTarget - wristRotation.get_position();
     wristVelocity = get_wrist_kp() * wristError;
     wrist.move(wristVelocity);
 }
 
-int WristMech::get_wrist_state() {return wristStates[currWristState];}
+int WristMech::get_wrist_state() {
+    if (!wristStates.empty()) {
+        return wristStates[currWristState];
+    }
+}
 int WristMech::get_num_wrist_states() {return numWristStates;}
-int WristMech::get_wrist_kp() {return wristKP;}
+float WristMech::get_wrist_kp() {return wristKP;}
+
+void WristMech::set_wrist_kp(float _wristKP) {
+    wristKP = _wristKP;
+}

src/main.cpp

@@ -1,10 +1,5 @@
 #include "main.h"
-
-// Global Variables
-Pneumatics pistonControl;
-ArmMech armObj;
-LiftMech liftObj;
-WristMech wristObj;
+#include <iostream>
 
 // Chassis constructor
 ez::Drive chassis(
@@ -24,26 +19,19 @@ ez::Drive chassis(
  ez::tracking_wheel horiz_tracker(8, 2.75, 4.0);  // This tracking wheel is perpendicular to the drive wheels
 // 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);
+// Default Objects
+Pneumatics pistonControl;
+ArmMech armObj;
 
 // Lift constructor
-LiftMech liftSetup(
+LiftMech liftObj(
 
-  {0, 100, 200},
-
-  3);
+  {0, 63194, 94246, 139878}, 4);
 
 // Wrist constructor
-WristMech wristSetup(
+WristMech wristObj(
 
-  {0, 100},
-  
-  2);
+  {0, -14529}, 2);
 
 /**
  * Runs initialization code. This occurs as soon as the program is started.
@@ -57,7 +45,23 @@ void initialize() {
 
   pros::delay(500);  // Stop the user from doing anything while legacy ports configure
 
+  armObj.set_state_values(
+    {0, -5642}, 
+    {0, -22702}, 
+    {0, -19740}, 
+    {0, -17121}
+  );
+  liftObj.set_ranges(
+    {0, 63194},
+    {63194, 94246},
+    {94246, 139878},
+    {139878, 150000}
+  );
+
   armRotation.reset_position();
+  liftRotation.reset_position();
+  wristRotation.reset_position();
+
   pros::Task liftControlTasck([&]{
     while (true) {
       armObj.arm_control();
@@ -283,15 +287,28 @@ void opcontrol() {
     ez_template_extras();
 
     // Call custom drive functions here
-    pistonControl.control_piston(Pistons::claw_piston, ControlPeriod::driver, 
-      Controller::slave, Buttons::arrow_buttons);
+    pistonControl.control_piston(PistonsP::claw_piston, ControlPeriodP::driver, 
+      ControllerP::slave, ButtonsP::arrow_buttons);
+
+    //armObj.arm_driver_control(ControllerR::master, ButtonsR::left_shoulder_buttons);
+    //liftObj.lift_driver_control(ControllerR::master, ButtonsR::right_shoulder_buttons);
+    
+    armObj.arm_driver_control(ControllerR::slave, ButtonsR::left_shoulder_buttons);
+    liftObj.lift_driver_control(ControllerR::slave, ButtonsR::right_shoulder_buttons);
+    wristObj.wrist_driver_control(ControllerR::slave, ButtonsR::Y_B_buttons);
+    //wristObj.rotate_wrist(90);
     
-    // chassis.opcontrol_tank();  // Tank control
     chassis.opcontrol_arcade_standard(ez::SPLIT);   // Standard split arcade
+    // chassis.opcontrol_tank();  // Tank control
     // chassis.opcontrol_arcade_standard(ez::SINGLE);  // Standard single arcade
     // chassis.opcontrol_arcade_flipped(ez::SPLIT);    // Flipped split arcade
     // chassis.opcontrol_arcade_flipped(ez::SINGLE);   // Flipped single arcade
 
+    // Terminal debugging
+    std::cout << "Arm rotation sensor value: " << "\033[31m" << armRotation.get_position() << " cdg" << "mm\033[0m" << std::endl;
+    std::cout << "Lift rotation sensor value: " << "\033[32m" << liftRotation.get_position() << " cdg" << "mm\033[0m" << std::endl;
+    std::cout << "Wrist rotation sensor value: " << "\033[33m" << wristRotation.get_position() << " cdg" << "mm\033[0m" << std::endl;  
+
     pros::delay(ez::util::DELAY_TIME);  // This is used for timer calculations!  Keep this ez::util::DELAY_TIME
   }
 }