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.

README.md

@@ -1,6 +1,121 @@
-![](https://img.shields.io/github/downloads/EZ-Robotics/EZ-Template/total.svg)
+# 11254Y Robot Code
-![](https://github.com/EZ-Robotics/EZ-Template/workflows/Build/badge.svg)
+
-[![License: MPL 2.0](https://img.shields.io/badge/License-MPL%202.0-brightgreen.svg)](https://opensource.org/licenses/MPL-2.0)
+This repository contains the software for team 11254Y's competition robot. The project is written in C++ using **PROS** and is built on the **uncompiled/source version of EZ-Template**.
+
+The goal of this project is to provide a modular structure for controlling the robot's drivetrain and mechanisms while keeping individual mechanisms easy to reuse and modify.
+
+## Technologies
+
+* **C++**
+* **PROS**
+* **EZ-Template** — source/uncompiled version
+* **VEX V5**
+
+## Project Structure
+
+The project is organized similarly to the structure used by EZ-Template. Mechanism systems are separated into their own header and source files so they can be developed and reused independently.
+
+### EZ-Template
+
+EZ-Template provides the foundation for the robot code, including systems such as:
+
+* Drivetrain control
+* Autonomous routines
+* Autonomous selector
+* PID control
+* General robot utilities
+
+The source version of EZ-Template is included directly in the project rather than using it only as a precompiled library. This allows the template's code to be modified and extended as needed.
+Using the uncompiled version of EZ-Template also allows us the learn how the template was made and that lets us use the template more effectivly.
+
+## Custom Systems
+
+I have added several custom mechanism-control systems following the same general structure and organization used by EZ-Template.
+The costum control systems I've added are:
+*Adjustable Color sensor class
+*Adaptive Pneumatics controller
+*Rotation sensor classes
+
+### Color Sorter
+
+As of now the color sorter dosen't do anything on the robot but is in place if we ever need it.
+
+It is designed to handle the logic required to detect and sort objects based on their color while integrating with the rest of the robot's control system.
+
+### Pneumatic Control
+
+The pneumatic control system provides an organized way to control the robot's pneumatic mechanisms.
+
+Instead of handling individual pneumatic outputs throughout the main robot code, pneumatic mechanisms can be controlled through their own class and functions.
+
+### Rotation Mechanisms
+
+The rotation-control system is designed for mechanisms that use a VEX V5 Rotation Sensor to determine their position.
+
+Mechanisms can define multiple preset positions and cycle between those positions. The system also provides control functions for moving and maintaining the mechanism at its desired position.
+
+For example, a mechanism can have states such as:
+
+```text
+State 0 → 0°
+State 1 → 26.5°
+State 2 → 45°
+```
+
+The mechanism can then cycle between these states without requiring the state-management code to be rewritten for every mechanism.
+
+## Design Philosophy
+
+One of the main goals of this project is **modularity**.
+
+Rather than putting all robot functionality into `main.cpp`, individual systems are implemented as classes with their own `.hpp` and `.cpp` files.
+
+The header files contain the class declarations, while the `.cpp` files contain the implementation.
+
+This allows mechanisms to be instantiated and controlled from the main robot program without putting their implementation directly into `main.cpp`.
+This method is much cleaner and easier to understand.
+
+## Using a Mechanism
+
+A mechanism can be created as an object and then controlled through its member functions.
+
+For example:
+
+```cpp
+ArmMech arm_control;
+```
+
+Once the object exists, its functions can be called from the robot code:
+
+```cpp
+arm_control.cycle_states();
+arm_control.reset_state();
+arm_control.arm_control();
+```
+
+This keeps the main robot code focused on **what the robot should do** instead of how each individual mechanism works internally.
+
+## Development
+
+This project is actively developed for VEX Robotics competition use. The code may change as the robot's mechanical design, autonomous routines, and control systems are improved.
+
+Because the project uses the source version of EZ-Template, changes can be made directly to the template and custom systems can be integrated alongside it.
+
+## Credits
+
+### PROS
+
+This project uses PROS as the development environment and operating system for the VEX V5 brain.
+
+### EZ-Template
+
+EZ-Template is used as the foundation for the drivetrain and competition-code structure. Custom systems in this project are implemented using a similar organization and design approach.
+
+## License
+
+This project is intended for personal and educational use. If you reuse portions of this code, please give appropriate credit to the original authors of EZ-Template and PROS where applicable.
+
+
 
 ⚡️ **coding made ez** ⚡️
 

include/EZ-Template/api.hpp

@@ -9,6 +9,7 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/.
 #include "EZ-Template/PID.hpp"
 #include "EZ-Template/auton.hpp"
 #include "EZ-Template/auton_selector.hpp"
+#include "EZ-Template/color_sorter.hpp"
 #include "EZ-Template/drive/drive.hpp"
 #include "EZ-Template/piston.hpp"
 #include "EZ-Template/sdcard.hpp"

include/EZ-Template/color_sorter.hpp

@@ -0,0 +1,42 @@
+#pragma once
+
+#include "api.h"
+#include "pros/adi.hpp"
+#include "pros/colors.hpp"
+
+namespace ez {
+    
+    enum wanted_color {
+        red,
+        blue,
+        yellow,
+        green,
+        purple
+    };
+
+    enum control_period {
+        driver,
+        auton
+    };
+
+    class color_sorter {
+        public:
+        
+        /**
+        *
+        * \param sensorNum
+        *        the sensor we want to use for the reading
+        * \param period
+        *        the current control period where in
+        * \param selected_color
+        *        the color we want to look for
+        * \param looped
+        *        do we want to continusly check for that color
+        */
+        color_sorter(int sensorNum, control_period period, wanted_color selected_color, bool looped);
+        
+        void sensor(int sensorAmount);
+
+        void DisplayColor();
+    };
+};

include/EZ-Template/pneumatics.hpp

@@ -0,0 +1,54 @@
+#pragma once
+
+#include "api.h"
+#include "pros/adi.hpp"
+#include "pros/serial.hpp"
+#include "EZ-Template/piston.hpp"
+
+namespace ez {
+     /**
+     * Enumeration to hold the current part of a match were in.
+     */
+    enum class ControlPeriodP {
+        driver,
+        auton
+    };
+    /**
+     * Enumeration to hold the different controllers avalable. 
+     */
+    enum class ControllerP {
+        master,
+        slave
+    };
+    /**
+     * 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{
+
+        private:
+        bool pistonState;
+
+        public:
+
+        Pneumatics();
+
+        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

@@ -0,0 +1,145 @@
+#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:
+        // 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(); // 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(); // 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();
+        float get_arm_kp();
+    };
+
+    /**
+     * List mechanism class.
+     * 
+     * Sets up all constructors, variables, and functions.
+     */
+    class LiftMech {
+        private:
+        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(); // 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(); // 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();
+        float get_lift_kp();
+    };
+
+    /**
+     * Wrist mechanism class.
+     * 
+     * Sets up all constructors, variables, and functions.
+     */
+    class WristMech {
+        private:
+        std::vector<int> wristStates; // Array the wrist uses to move
+        int numWristStates;
+        float wristKP;
+
+        public:
+        int currWristState;
+        int wristTarget;
+        float wristError;
+        float wristVelocity;
+
+        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 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();
+        float get_wrist_kp();
+
+        void set_wrist_kp(float _wristKP);
+    };
+};

include/EZ-Template/util.hpp

@@ -18,17 +18,12 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
 using namespace okapi::literals;
 
-/**
- * Controller.
- */
-extern pros::Controller master;
-
 namespace ez {
 
 /**
- * Prints our branding all over your pros terminal.
+ * Prints your chooses logo in the terminal
  */
-void ez_template_print();
+void logo_print();
 
 /**
  * Prints to the brain screen in one string.

include/autons.hpp

@@ -2,6 +2,22 @@
 
 void default_constants();
 
+/**
+ * Personal auton routes
+ */
+void leftMain(std::string side);
+void rightMain(std::string side);
+void Skills();
+void BlueRight();
+void BlueLeft();
+void RedRight();
+void RedLeft();
+void testMain(std::string side);
+void Test();
+
+/**
+ * Template auton routes
+ */
 void drive_example();
 void turn_example();
 void drive_and_turn();

include/main.h

@@ -42,10 +42,13 @@
 // #include "okapi/api.hpp"
 // #include "pros/api_legacy.h"
 #include "EZ-Template/api.hpp"
+#include "setup/initialize.hpp"
 
 // More includes here...
 #include "autons.hpp"
 #include "subsystems.hpp"
+#include "EZ-Template/rotation_mechanisms/rotation_calc.hpp"
+#include "EZ-Template/pneumatics.hpp"
 
 /**
  * If you find doing pros::Motor() to be tedious and you'd prefer just to do

include/setup/initialize.hpp

@@ -0,0 +1,48 @@
+#include "main.h"
+#include "EZ-Template/api.hpp"
+#include "api.h"
+
+#pragma once
+
+#ifndef CONFIG_H
+#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);
+
+/**
+ * Declares mechinism motors and motor groups.
+ */
+inline pros::MotorGroup lift({15, -19});
+inline pros::MotorGroup wrist({1, 9});
+inline pros::Motor arm({6});
+
+/**
+ * Declares piston mechanisms.
+ */
+inline ez::Piston claw({'G',true});
+
+/**
+ * Declare limit switches and other ADI sensors
+ */
+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});
+
+#endif

project.pros

@@ -1,7 +1,7 @@
 {
   "py/object": "pros.conductor.project.Project",
   "py/state": {
-    "project_name": "EZ-Template",
+    "project_name": "Over-Ride",
     "target": "v5",
     "templates": {
       "kernel": {
@@ -400,9 +400,10 @@
     },
     "upload_options": {
       "compress_bin": true,
-      "description": "roboticsisez.com",
+      "description": "",
       "remote_name": "EZ-Template",
-      "slot": 1
+      "slot": 1,
+      "icon": "pizza"
     },
     "use_early_access": false
   }

src/EZ-Template/color_sorter.cpp

@@ -0,0 +1,27 @@
+#include "main.h"
+
+using namespace ez;
+
+color_sorter::color_sorter(int sensorNum, control_period period, wanted_color selected_color, bool looped) {
+
+  while (looped == true) {
+    if (sensorNum == 1 && period == auton) {
+      pros::delay(100);
+      while (colorSensorOne.get_hue() <= 20 || colorSensorOne.get_hue() >= 340) {
+        //Code you want to execute goes here
+
+        looped = false;
+      }
+
+    }
+    else if (sensorNum == 2 && period== auton) {
+      pros::delay(100);
+      while (colorSensorTwo.get_hue() >= 160 && colorSensorTwo.get_hue() <= 280) {
+        //Code you want to execute goes here
+
+        looped = false;
+      }
+        
+    }
+  }
+}

src/EZ-Template/drive/pid_tuner.cpp

@@ -4,6 +4,7 @@ License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
 */
 
+#include "setup/initialize.hpp"
 #include "EZ-Template/drive/drive.hpp"
 #include "EZ-Template/sdcard.hpp"
 #include "EZ-Template/util.hpp"

src/EZ-Template/drive/user_input.cpp

@@ -4,6 +4,7 @@ License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
 */
 
+#include "setup/initialize.hpp"
 #include "EZ-Template/PID.hpp"
 #include "EZ-Template/drive/drive.hpp"
 #include "pros/misc.h"

src/EZ-Template/pneumatics.cpp

@@ -0,0 +1,121 @@
+#include "main.h"
+#include "EZ-Template/pneumatics.hpp"
+
+using namespace ez;
+
+Pneumatics::Pneumatics(){}
+
+void Pneumatics::control_piston(PistonsP activePiston, ControlPeriodP currPeriod, ControllerP currController, ButtonsP buttonUsed) {
+
+    if (currPeriod == ControlPeriodP::driver) {
+        switch (activePiston)
+        {
+        case PistonsP::claw_piston:
+            /*More code can go here if the claw piston is selected*/
+
+            switch (currController)
+            {
+            case ControllerP::master:
+                /*More code can go here if we want to use the master controller*/
+
+                switch (buttonUsed)
+                {
+                case ButtonsP::arrow_buttons:
+                    if (master.get_digital(DIGITAL_RIGHT)) {
+                        claw.set(true);
+                    }
+                    else if (master.get_digital(DIGITAL_DOWN)) {
+                        claw.set(false);
+                    }
+                    break;
+                case ButtonsP::Y_B_buttons:
+                    if (master.get_digital(DIGITAL_Y)) {
+                        claw.set(true);
+                    }
+                    else if (master.get_digital(DIGITAL_B)) {
+                        claw.set(false);
+                    }
+                    break;
+                case ButtonsP::left_shoulder_buttons:
+                    if (master.get_digital(DIGITAL_L1)) {
+                        claw.set(true);
+                    }
+                    else if (master.get_digital(DIGITAL_L2)) {
+                        claw.set(false);
+                    }
+                    break;
+                case ButtonsP::right_shoulder_buttons:
+                    if (master.get_digital(DIGITAL_R1)) {
+                        claw.set(true);
+                    }
+                    else if (master.get_digital(DIGITAL_R2)) {
+                        claw.set(false);
+                    }
+                    break;
+                default:
+                    break;
+                }
+                break;
+            case ControllerP::slave:
+
+                switch (buttonUsed)
+                {
+                case ButtonsP::arrow_buttons:
+                    if (slave.get_digital(DIGITAL_RIGHT)) {
+                        claw.set(true);
+                    }
+                    else if (slave.get_digital(DIGITAL_DOWN)) {
+                        claw.set(false);
+                    }
+                    break;
+                case ButtonsP::Y_B_buttons:
+                    if (slave.get_digital(DIGITAL_Y)) {
+                        claw.set(true);
+                    }
+                    else if (slave.get_digital(DIGITAL_B)) {
+                        claw.set(false);
+                    }
+                    break;
+                case ButtonsP::left_shoulder_buttons:
+                    if (slave.get_digital(DIGITAL_L1)) {
+                        claw.set(true);
+                    }
+                    else if (slave.get_digital(DIGITAL_L2)) {
+                        claw.set(false);
+                    }
+                    break;
+                case ButtonsP::right_shoulder_buttons:
+                    if (slave.get_digital(DIGITAL_R1)) {
+                        claw.set(true);
+                    }
+                    else if (slave.get_digital(DIGITAL_R2)) {
+                        claw.set(false);
+                    }
+                    break;
+                default:
+                    break;
+                }
+                break;
+            /*This is were more cases can go for the controller switch*/
+
+            default:
+                break;
+            }
+            break;
+        /*This is were more cases can go fot the piston selected switch*/
+        default:
+            break;
+        }
+
+    }
+    else {
+        switch (activePiston)
+        {
+        case PistonsP::claw_piston:
+            /*code here*/
+            break;
+        default:
+            break;
+        }
+    }
+}

src/EZ-Template/rotation_mechanisms/arm_rotation.cpp

@@ -0,0 +1,269 @@
+#include "main.h"
+#include "EZ-Template/rotation_mechanisms/rotation_calc.hpp"
+
+using namespace ez;
+
+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
+}
+
+/**
+ * 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) {
+        currArmState = 0;
+    }
+    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;
+
+    // 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(); // 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() {
+    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

@@ -0,0 +1,191 @@
+#include "main.h"
+#include "EZ-Template/rotation_mechanisms/rotation_calc.hpp"
+
+using namespace ez;
+
+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;
+    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 (!liftStates.empty()) {
+        if (currLiftState == numLiftStates) {
+            currLiftState = 0;
+        }
+        liftTarget = liftStates[currLiftState];
+    }
+}
+
+void LiftMech::reset_state() {
+    currLiftState = 0;
+
+    if (!liftStates.empty()) {
+        liftTarget = liftStates[currLiftState];
+    }
+}
+
+void LiftMech::lift_control() {
+    liftError = liftTarget - liftRotation.get_position();
+    liftVelocity = get_lift_kp() * liftError;
+    lift.move(liftVelocity);
+}
+
+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;}
+float LiftMech::get_lift_kp() {return liftKP;}

src/EZ-Template/rotation_mechanisms/wrist_rotation.cpp

@@ -0,0 +1,116 @@
+#include "main.h"
+#include "EZ-Template/rotation_mechanisms/rotation_calc.hpp"
+
+using namespace ez;
+
+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;
+    set_wrist_kp(0.0173f);
+}
+
+void WristMech::wrist_driver_control(ControllerR currController, ButtonsR buttonUsed) {
+    switch (currController)
+    {
+    case ControllerR::master:
+
+        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];
+    }
+}
+
+void WristMech::reset_state() {
+    currWristState = 0;
+    if (!wristStates.empty()) {
+        wristTarget = wristStates[currWristState];
+    }
+}
+
+void WristMech::wrist_control() {
+    wristError = wristTarget - wristRotation.get_position();
+    wristVelocity = get_wrist_kp() * wristError;
+    wrist.move(wristVelocity);
+}
+
+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;}
+
+void WristMech::set_wrist_kp(float _wristKP) {
+    wristKP = _wristKP;
+}

src/EZ-Template/util.cpp

@@ -14,18 +14,15 @@ pros::Controller master(pros::E_CONTROLLER_MASTER);
 namespace ez {
 int mode = DISABLE;
 
-void ez_template_print() {
+void logo_print() {
   std::cout << R"(
 
+       _________   ________ ____  __
+      <  <  /__ \ / ____/ // /\ \/ /
+      / // /__/ //___ \/ // /_ \  / 
+     / // // __/____/ /__  __/ / /  
+    /_//_//____/_____/  /_/   /_/  
 
-    _____ ______   _____                    _       _
-   |  ___|___  /  |_   _|                  | |     | |
-   | |__    / /_____| | ___ _ __ ___  _ __ | | __ _| |_ ___
-   |  __|  / /______| |/ _ \ '_ ` _ \| '_ \| |/ _` | __/ _ \
-   | |___./ /___    | |  __/ | | | | | |_) | | (_| | ||  __/
-   \____/\_____/    \_/\___|_| |_| |_| .__/|_|\__,_|\__\___|
-                                     | |
-                                     |_|
 )" << '\n';
 }
 std::string get_last_word(std::string text) {

src/autons.cpp

@@ -48,6 +48,11 @@ void default_constants() {
   chassis.pid_angle_behavior_set(ez::shortest);  // Changes the default behavior for turning, this defaults it to the shortest path there
 }
 
+///
+// Custom auton routes
+//
+
+
 ///
 // Drive Example
 ///

src/main.cpp

@@ -1,28 +1,38 @@
 #include "main.h"
-
+#include <iostream>
-/////
-// For installation, upgrading, documentations, and tutorials, check out our website!
-// https://ez-robotics.github.io/EZ-Template/
-/////
 
 // Chassis constructor
 ez::Drive chassis(
     // These are your drive motors, the first motor is used for sensing!
-    {-5, -6, -7, -8},  // Left Chassis Ports (negative port will reverse it!)
+    {12, 14},  // Left Chassis Ports (negative port will reverse it!)
-    {11, 15, 16, 17},  // Right Chassis Ports (negative port will reverse it!)
+    {-11, -13},  // Right Chassis Ports (negative port will reverse it!)
 
     21,      // IMU Port
-    4.125,   // Wheel Diameter (Remember, 4" wheels without screw holes are actually 4.125!)
+    3.25,   // Wheel Diameter (Remember, 4" wheels without screw holes are actually 4.125!)
-    420.0);  // Wheel RPM = cartridge * (motor gear / wheel gear)
+    360.0);  // Wheel RPM = cartridge * (motor gear / wheel gear)
 
 // Uncomment the trackers you're using here!
 // - `8` and `9` are smart ports (making these negative will reverse the sensor)
 //  - you should get positive values on the encoders going FORWARD and RIGHT
 // - `2.75` is the wheel diameter
 // - `4.0` is the distance from the center of the wheel to the center of the robot
-// ez::tracking_wheel horiz_tracker(8, 2.75, 4.0);  // This tracking wheel is perpendicular to the drive wheels
+ 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
 
+// Default Objects
+Pneumatics pistonControl;
+ArmMech armObj;
+
+// Lift constructor
+LiftMech liftObj(
+
+  {0, 63194, 94246, 139878}, 4);
+
+// Wrist constructor
+WristMech wristObj(
+
+  {0, -14529}, 2);
+
 /**
  * Runs initialization code. This occurs as soon as the program is started.
  *
@@ -30,11 +40,37 @@ ez::Drive chassis(
  * to keep execution time for this mode under a few seconds.
  */
 void initialize() {
-  // Print our branding over your terminal :D
+  // Prints your logo in the terminal
-  ez::ez_template_print();
+  ez::logo_print();
 
   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();
+      liftObj.lift_control();
+      wristObj.wrist_control();
+      pros::delay(10);
+    }
+  });
+
   // Look at your horizontal tracking wheel and decide if it's in front of the midline of your robot or behind it
   //  - change `back` to `front` if the tracking wheel is in front of the midline
   //  - ignore this if you aren't using a horizontal tracker
@@ -58,7 +94,10 @@ void initialize() {
 
   // Autonomous Selector using LLEMU
   ez::as::auton_selector.autons_add({
-      {"Drive\n\nDrive forward and come back", drive_example},
+      //Custom autonomouse routes go here.
+
+
+      /*{"Drive\n\nDrive forward and come back", drive_example},
       {"Turn\n\nTurn 3 times.", turn_example},
       {"Drive and Turn\n\nDrive forward, turn, come back", drive_and_turn},
       {"Drive and Turn\n\nSlow down during drive", wait_until_change_speed},
@@ -71,7 +110,7 @@ void initialize() {
       {"Pure Pursuit Wait Until\n\nGo to (24, 24) but start running an intake once the robot passes (12, 24)", odom_pure_pursuit_wait_until_example},
       {"Boomerang\n\nGo to (0, 24, 45) then come back to (0, 0, 0)", odom_boomerang_example},
       {"Boomerang Pure Pursuit\n\nGo to (0, 24, 45) on the way to (24, 24) then come back to (0, 0, 0)", odom_boomerang_injected_pure_pursuit_example},
-      {"Measure Offsets\n\nThis will turn the robot a bunch of times and calculate your offsets for your tracking wheels.", measure_offsets},
+      {"Measure Offsets\n\nThis will turn the robot a bunch of times and calculate your offsets for your tracking wheels.", measure_offsets},*/
   });
 
   // Initialize chassis and auton selector
@@ -169,10 +208,10 @@ void ez_screen_task() {
                            1);  // Don't override the top Page line
 
           // Display all trackers that are being used
-          screen_print_tracker(chassis.odom_tracker_left, "l", 4);
+          screen_print_tracker(chassis.odom_tracker_left, "L", 4);
-          screen_print_tracker(chassis.odom_tracker_right, "r", 5);
+          screen_print_tracker(chassis.odom_tracker_right, "R", 5);
-          screen_print_tracker(chassis.odom_tracker_back, "b", 6);
+          screen_print_tracker(chassis.odom_tracker_back, "B", 6);
-          screen_print_tracker(chassis.odom_tracker_front, "f", 7);
+          screen_print_tracker(chassis.odom_tracker_front, "F", 7);
         }
       }
     }
@@ -241,21 +280,34 @@ void ez_template_extras() {
  */
 void opcontrol() {
   // This is preference to what you like to drive on
-  chassis.drive_brake_set(MOTOR_BRAKE_COAST);
+  chassis.drive_brake_set(MOTOR_BRAKE_HOLD);
 
   while (true) {
     // Gives you some extras to make EZ-Template ezier
     ez_template_extras();
 
-    chassis.opcontrol_tank();  // Tank control
+    // Call custom drive functions here
-    // chassis.opcontrol_arcade_standard(ez::SPLIT);   // Standard split arcade
+    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_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
-    // Put more user control code here!
+    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
   }