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

@@ -2,28 +2,43 @@
 
 #include <vector>
 #include "api.h"
+//#include "main.h"
 #include "pros/adi.hpp"
 #include "pros/rotation.hpp"
 
 namespace ez{
-
-    enum class ControlPeriod {
+    /**
+     * Enumeration to hold the current part of a match were in.
+     */
+    enum class ControlPeriodR {
         driver,
         auton
     };
-    enum class Controller {
+    /**
+     * Enumeration to hold the different controllers avalable. 
+     */
+    enum class ControllerR {
         master,
         slave
     };
-    enum class Buttons {
+    /**
+     * 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:
+        // Arrays to hold different values for the arm.
         std::vector<int> levelOneArmStates;
         std::vector<int> levelTwoArmStates;
         std::vector<int> levelThreeArmStates;
@@ -32,20 +47,20 @@ namespace ez{
         float armKP;
 
         public:
-        std::vector<int> selectedStates;
-        int targetPos;
+        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(); // Defualt constructor
 
-        void arm_driver_control(enum Controller, enum Buttons);
+        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);
@@ -55,32 +70,37 @@ namespace ez{
         float get_arm_kp();
     };
 
+    /**
+     * List mechanism class.
+     * 
+     * Sets up all constructors, variables, and functions.
+     */
     class LiftMech {
         private:
-        std::vector<int> liftStates;
-        int numLiftStates;
-        float liftKP;
+        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;
-        int liftHeight;
         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 Controller, enum Buttons);
+        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_range_finder();
+        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);
@@ -90,9 +110,14 @@ namespace ez{
         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;
 
@@ -102,12 +127,12 @@ 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 wrist_driver_control(enum Controller, enum Buttons);
+        void wrist_driver_control(enum ControllerR, enum ButtonsR);
 
-        void rotate_wrist(int wristThreshold);
+        void rotate_wrist(int wristThreshold); // Rotates the wrist based where the arm is.
         void reset_state();
         void wrist_control();
 

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 wristRotation({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,52 +3,69 @@
 
 using namespace ez;
 
-LiftMech liftObj;
+LiftMech liftRangeAccess; // Lift object to use lift variables and functions
 
+/**
+ * Default constructor
+ */
 ArmMech::ArmMech() {
     currArmState = 0;
     armTarget = 0;
     targetPos = 0;
     set_arm_kp(0.0173f);
+
+    selectedStates = {0, 0}; // Sets default array values
 }
 
-void ArmMech::arm_driver_control(Controller currController, Buttons buttonUsed) {
+/**
+ * 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 (currController) // Switch that check which controller was selected
     {
-    case Controller::master:
-            
+    case ControllerR::master:
+        // Code you want to run when the master controller is selcted
         switch (buttonUsed)
         {
-        case Buttons::arrow_buttons:
-            if (master.get_digital_new_press(DIGITAL_RIGHT)){
+        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_new_press(DIGITAL_DOWN)) {
+            else if (master.get_digital(DIGITAL_DOWN)) {
                 arm.move(-127);
             }
             break;
-        case Buttons::Y_B_buttons:
-            if (master.get_digital_new_press(DIGITAL_Y)){
+        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_new_press(DIGITAL_B)) {
+            else if (master.get_digital(DIGITAL_B)) {
                 arm.move(-127);
             }
             break;
-        case Buttons::left_shoulder_buttons:
-            if (master.get_digital_new_press(DIGITAL_L1)){
+        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_new_press(DIGITAL_L2)) {
+            else if (master.get_digital(DIGITAL_L2)) {
                 arm.move(-127);
             }
             break;
-        case Buttons::right_shoulder_buttons:
-            if (master.get_digital_new_press(DIGITAL_R1)){
+        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_new_press(DIGITAL_R2)) {
+            else if (master.get_digital(DIGITAL_R2)) {
                 arm.move(-127);
             }
             break;
@@ -56,39 +73,43 @@ void ArmMech::arm_driver_control(Controller currController, Buttons buttonUsed)
             break;
         }
         break;
-    case Controller::slave:
-            
+    case ControllerR::slave:
+        // Code you want to run when the slave controller is selected    
         switch (buttonUsed)
         {
-        case Buttons::arrow_buttons:
-            if (slave.get_digital_new_press(DIGITAL_RIGHT)){
+        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_new_press(DIGITAL_DOWN)) {
+            else if (slave.get_digital(DIGITAL_DOWN)) {
                 reset_state();
             }
             break;
-        case Buttons::Y_B_buttons:
-            if (slave.get_digital_new_press(DIGITAL_Y)){
+        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_new_press(DIGITAL_B)) {
+            else if (slave.get_digital(DIGITAL_B)) {
                 reset_state();
             }
             break;
-        case Buttons::left_shoulder_buttons:
-            if (slave.get_digital_new_press(DIGITAL_L1)){
+        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_new_press(DIGITAL_L2)) {
+            else if (slave.get_digital(DIGITAL_L2)) {
                 reset_state();
             }
             break;
-        case Buttons::right_shoulder_buttons:
-            if (slave.get_digital_new_press(DIGITAL_R1)){
+        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_new_press(DIGITAL_R2)) {
+            else if (slave.get_digital(DIGITAL_R2)) {
                 reset_state();
             }
             break;
@@ -101,45 +122,51 @@ void ArmMech::arm_driver_control(Controller currController, Buttons buttonUsed)
     }
 }
 
+/**
+ * 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) {
-    liftObj.lift_range_finder();
+    liftRangeAccess.lift_range_finder(); // Returns a value 0 - 3 based on where the lift is currently
 
     switch (targetPos)
     {
     case 0:
-        selectedStates = levelOneArmStates;
+        selectedStates = levelOneArmStates; // Makes the main array equal the level one array
         if (pickState == 1) {
-            selectedStates[1];
+            armTarget = selectedStates[1]; // Makes the target value the second in the array
         }
         else {
-            selectedStates[0];
+            armTarget = selectedStates[0]; // Makes the target value the first in the array
         }
         break;
     case 1:
-        selectedStates = levelOneArmStates;
+        selectedStates = levelOneArmStates; // Make the main array equal the level two array
         if (pickState == 1) {
-            selectedStates[1];
+            armTarget = selectedStates[1];
         }
         else {
-            selectedStates[0];
+            armTarget = selectedStates[0];
         }
         break;
     case 2:
-        selectedStates = levelTwoArmStates;
+        selectedStates = levelTwoArmStates; // Makes the main array equal the level three array
         if (pickState == 1) {
-            selectedStates[1];
+            armTarget = selectedStates[1];
         }
         else {
-            selectedStates[0];
+            armTarget = selectedStates[0];
         }
         break;
     case 3:
-        selectedStates = levelThreeArmStates;
+        selectedStates = levelThreeArmStates; // Makes the main array equal the level four array
         if (pickState == 1) {
-            selectedStates[1];
+            armTarget = selectedStates[1];
         }
         else {
-            selectedStates[0];
+            armTarget = selectedStates[0];
         }
         break;
     default:
@@ -148,55 +175,95 @@ void ArmMech::arm_auton_control(int pickState) {
 
 }
 
+/**
+ * Function to cycle through each value in the array
+ */
 void ArmMech::cycle_states() {
-    liftObj.lift_range_finder();
+    liftRangeAccess.lift_range_finder(); // Returns a value 0 - 3 based on were the lift is currently
 
-    switch (targetPos)
-    {
-    case 0:
-        selectedStates = levelOneArmStates;
-        break;
-    case 1:
-        selectedStates = levelTwoArmStates;
-        break;
-    case 2:
-        selectedStates = levelThreeArmStates;
-        break;
-    case 3:
-        selectedStates = levelFourArmStates;
-        break;
-    default:
-        break;
+    // 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 = selectedStates[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 = selectedStates[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
 }
 
+/**
+ * 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() {return selectedStates[currArmState];}
+int ArmMech::get_arm_state() {
+    if (selectedStates.empty()) {
+        return 0;
+    }
+    return selectedStates[currArmState];
+}
 int ArmMech::get_num_arm_states() {return numArmStates;}
 float ArmMech::get_arm_kp() {return armKP;}

src/EZ-Template/rotation_mechanisms/lift_rotation.cpp

@@ -5,7 +5,13 @@ using namespace ez;
 
 ArmMech armObjRange;
 
-LiftMech::LiftMech() {}
+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;
@@ -14,14 +20,14 @@ LiftMech::LiftMech(std::initializer_list<int> _liftStates, int _numLiftStates) {
     set_lift_kp(0.0173f);
 }
 
-void LiftMech::lift_driver_control(Controller currController, Buttons buttonUsed) {
+void LiftMech::lift_driver_control(ControllerR currController, ButtonsR buttonUsed) {
     switch (currController)
     {
-    case Controller::master:
+    case ControllerR::master:
 
         switch (buttonUsed)
         {
-        case Buttons::arrow_buttons:
+        case ButtonsR::arrow_buttons:
             if (master.get_digital(DIGITAL_RIGHT)) {
                 lift.move(127);
             }
@@ -29,7 +35,7 @@ void LiftMech::lift_driver_control(Controller currController, Buttons buttonUsed
                 lift.move(-127);
             }
             break;
-        case Buttons::Y_B_buttons:
+        case ButtonsR::Y_B_buttons:
             if (master.get_digital(DIGITAL_Y)) {
                 lift.move(127);
             }
@@ -37,7 +43,7 @@ void LiftMech::lift_driver_control(Controller currController, Buttons buttonUsed
                 lift.move(-127);
             }
             break;
-        case Buttons::left_shoulder_buttons:
+        case ButtonsR::left_shoulder_buttons:
             if (master.get_digital(DIGITAL_L1)) {
                 lift.move(127);
             }
@@ -45,7 +51,7 @@ void LiftMech::lift_driver_control(Controller currController, Buttons buttonUsed
                 lift.move(-127);
             }
             break;
-        case Buttons::right_shoulder_buttons:
+        case ButtonsR::right_shoulder_buttons:
             if (master.get_digital(DIGITAL_R1)) {
                 lift.move(127);
             }
@@ -57,11 +63,11 @@ void LiftMech::lift_driver_control(Controller currController, Buttons buttonUsed
             break;
         }
         break;
-    case Controller::slave:
+    case ControllerR::slave:
 
         switch (buttonUsed)
         {
-        case Buttons::arrow_buttons:
+        case ButtonsR::arrow_buttons:
             if (slave.get_digital(DIGITAL_RIGHT)) {
                 cycle_states();
             }
@@ -69,7 +75,7 @@ void LiftMech::lift_driver_control(Controller currController, Buttons buttonUsed
                 reset_state();
             }
             break;
-        case Buttons::Y_B_buttons:
+        case ButtonsR::Y_B_buttons:
             if (slave.get_digital(DIGITAL_Y)) {
                 cycle_states();
             }
@@ -77,7 +83,7 @@ void LiftMech::lift_driver_control(Controller currController, Buttons buttonUsed
                 reset_state();
             }
             break;
-        case Buttons::left_shoulder_buttons:
+        case ButtonsR::left_shoulder_buttons:
             if (slave.get_digital(DIGITAL_L1)) {
                 cycle_states();
             }
@@ -85,7 +91,7 @@ void LiftMech::lift_driver_control(Controller currController, Buttons buttonUsed
                 reset_state();
             }
             break;
-        case Buttons::right_shoulder_buttons:
+        case ButtonsR::right_shoulder_buttons:
             if (slave.get_digital(DIGITAL_R1)) {
                 cycle_states();
             }
@@ -126,15 +132,20 @@ void LiftMech::lift_auton_control(int pickState) {
 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() {
@@ -143,20 +154,20 @@ void LiftMech::lift_control() {
     lift.move(liftVelocity);
 }
 
-void LiftMech::lift_range_finder() {
+int LiftMech::lift_range_finder() {
     liftHeight = liftRotation.get_position();
 
-    if (liftHeight >= firstRange[0] && liftHeight <= firstRange[1]) {
-        armObjRange.targetPos = 0;
+    if (firstRange.size() >= 2 && liftHeight >= firstRange[0] && liftHeight <= firstRange[1]) {
+        return armObjRange.targetPos = 0;
     }
-    else if (liftHeight >= secondRange[0] && liftHeight <= secondRange[1]) {
-        armObjRange.targetPos = 1;
+    else if (secondRange.size() >= 2 && liftHeight >= secondRange[0] && liftHeight <= secondRange[1]) {
+        return armObjRange.targetPos = 1;
     }
-    else if (liftHeight >= thirdRange[0] && liftHeight <= thirdRange[1]) {
-        armObjRange.targetPos = 2;
+    else if (thirdRange.size() >= 2 && liftHeight >= thirdRange[0] && liftHeight <= thirdRange[1]) {
+        return armObjRange.targetPos = 2;
     }
-    else if (liftHeight >= fourthRange[0] && liftHeight <= fourthRange[1]) {
-        armObjRange.targetPos = 3;
+    else if (fourthRange.size() >= 2 && liftHeight >= fourthRange[0] && liftHeight <= fourthRange[1]) {
+        return armObjRange.targetPos = 3;
     }
 }
 
@@ -171,6 +182,10 @@ void LiftMech::set_ranges(std::vector<int> _firstRange, std::vector<int> _second
     fourthRange = _fourthRange;
 }
 
-int LiftMech::get_lift_state() {return liftStates[currLiftState];}
+int LiftMech::get_lift_state() {
+    if (!liftStates.empty()) {
+        return liftStates[currLiftState];
+    }
+}
 int LiftMech::get_num_lift_states() {return numLiftStates;}
 float LiftMech::get_lift_kp() {return liftKP;}

src/EZ-Template/rotation_mechanisms/wrist_rotation.cpp

@@ -3,7 +3,12 @@
 
 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;
@@ -12,29 +17,29 @@ WristMech::WristMech(std::initializer_list<int> _wristStates, int _numWristState
     set_wrist_kp(0.0173f);
 }
 
-void WristMech::wrist_driver_control(Controller currController, Buttons buttonUsed) {
+void WristMech::wrist_driver_control(ControllerR currController, ButtonsR buttonUsed) {
     switch (currController)
     {
-    case Controller::master:
+    case ControllerR::master:
 
         switch (buttonUsed)
         {
-        case Buttons::arrow_buttons:
+        case ButtonsR::arrow_buttons:
             if (master.get_digital(DIGITAL_DOWN)) {
                 reset_state();
             }
             break;
-        case Buttons::Y_B_buttons:
+        case ButtonsR::Y_B_buttons:
             if (master.get_digital(DIGITAL_Y)) {
                 reset_state();
             }
             break;
-        case Buttons::left_shoulder_buttons:
+        case ButtonsR::left_shoulder_buttons:
             if (master.get_digital(DIGITAL_L1)) {
                 reset_state();
             }
             break;
-        case Buttons::right_shoulder_buttons:
+        case ButtonsR::right_shoulder_buttons:
             if (master.get_digital(DIGITAL_R1)) {
                 reset_state();
             }
@@ -43,26 +48,26 @@ void WristMech::wrist_driver_control(Controller currController, Buttons buttonUs
             break;
         }
         break;
-    case Controller::slave:
+    case ControllerR::slave:
 
         switch (buttonUsed)
         {
-        case Buttons::arrow_buttons:
+        case ButtonsR::arrow_buttons:
             if (slave.get_digital(DIGITAL_DOWN)) {
                 reset_state();
             }
             break;
-        case Buttons::Y_B_buttons:
+        case ButtonsR::Y_B_buttons:
             if (slave.get_digital(DIGITAL_Y)) {
                 reset_state();
             }
             break;
-        case Buttons::left_shoulder_buttons:
+        case ButtonsR::left_shoulder_buttons:
             if (slave.get_digital(DIGITAL_L1)) {
                 reset_state();
             }
             break;
-        case Buttons::right_shoulder_buttons:
+        case ButtonsR::right_shoulder_buttons:
             if (slave.get_digital(DIGITAL_R1)) {
                 reset_state();
             }
@@ -87,16 +92,22 @@ void WristMech::rotate_wrist(int wristThreshold) {
 
 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;}
 float WristMech::get_wrist_kp() {return 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,22 +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();
+// Default Objects
+Pneumatics pistonControl;
+ArmMech armObj;
 
 // Lift constructor
-LiftMech liftSetup(
+LiftMech liftObj(
 
-  {0, 100, 200, 300},
-
-  4);
+  {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.
@@ -53,7 +45,18 @@ 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});
+  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();
@@ -284,22 +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(Controller::master, Buttons::left_shoulder_buttons);
-    liftObj.lift_driver_control(Controller::master, Buttons::right_shoulder_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);
     
-    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_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
   }
 }