Made a new pneumatics class and implemented it in driver control, also made a few different classes for the rotation sensors. Got all the rotation classes implemented with connection to the physical rotation sensors and the motors. Added another controller to be implemented later.

include/EZ-Template/color_sorter.hpp

@@ -22,9 +22,6 @@ namespace ez {
     class color_sorter {
         public:
         
-
-        color_sorter();
-
         /**
         *
         * \param sensorNum

include/EZ-Template/pneumatics.hpp

@@ -0,0 +1,43 @@
+#pragma once
+
+#include "api.h"
+#include "pros/adi.hpp"
+#include "pros/serial.hpp"
+#include "EZ-Template/piston.hpp"
+
+namespace ez {
+
+    enum class Pistons {
+        claw_piston
+    };
+    enum class ControlPeriod {
+        driver,
+        auton
+    };
+    enum class Controller {
+        master,
+        slave
+    };
+    enum class Buttons {
+        arrow_buttons,
+        Y_B_buttons,
+        left_shoulder_buttons,
+        right_shoulder_buttons
+    };
+
+    class Pneumatics{
+
+        private:
+        bool pistonState;
+
+        public:
+
+        Pneumatics();
+
+        void control_piston(enum Pistons, enum ControlPeriod, enum Controller, enum Buttons);
+
+        bool get_piston_state();
+        void set_piston_state(bool value);
+
+    };
+};

include/EZ-Template/rotation_mechanisms/rotation_calc.hpp

@@ -0,0 +1,80 @@
+#pragma once
+
+#include "api.h"
+#include "pros/adi.hpp"
+#include "pros/rotation.hpp"
+
+namespace ez{
+
+    class ArmMech{
+        private:
+        std::vector<int> armStates;
+        int numArmStates;
+        float armKP;
+
+        public:
+        int currArmState;
+        int armTarget;
+        float armError;
+        float armVelocity;
+
+        ArmMech();
+        ArmMech(std::initializer_list<int> armStates, int numArmStates);
+
+        void cycle_states();
+        void reset_state();
+        void arm_control();
+
+        int get_arm_state();
+        int get_num_arm_states();
+        int get_arm_kp();
+    };
+
+    class LiftMech {
+        private:
+        std::vector<int> liftStates;
+        int numLiftStates;
+        float liftKP;
+
+        public:
+        int currLiftState;
+        int liftTarget;
+        float liftError;
+        float liftVelocity;
+
+        LiftMech();
+        LiftMech(std::initializer_list<int> liftStates, int numLiftStates);
+
+        void cycle_states();
+        void reset_state();
+        void lift_control();
+
+        int get_lift_state();
+        int get_num_lift_states();
+        int get_lift_kp();
+    };
+
+    class WristMech {
+        private:
+        std::vector<int> wristStates;
+        int numWristStates;
+        float wristKP;
+
+        public:
+        int currWristState;
+        int wristTarget;
+        float wristError;
+        float wristVelocity;
+
+        WristMech();
+        WristMech(std::initializer_list<int> wristStates, int numWristStates);
+
+        void cycle_states();
+        void reset_state();
+        void wrist_control();
+
+        int get_wrist_state();
+        int get_num_wrist_states();
+        int get_wrist_kp();
+    };
+};

include/EZ-Template/util.hpp

@@ -18,11 +18,6 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/.
 
 using namespace okapi::literals;
 
-/**
- * Controller.
- */
-extern pros::Controller master;
-
 namespace ez {
 
 /**

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

@@ -47,6 +47,8 @@
 // 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

@@ -7,73 +7,32 @@
 #ifndef CONFIG_H
 #define CONFIG_H
 
-extern bool gotRed;
-extern bool gotBlue;
-extern double currentColor;
+/**
+ * 
+ */
+inline pros::Controller master;
+inline pros::Controller slave;
 
-extern Drive chassis;
+/**
+ * Declare mechinism motors
+ */
+inline pros::MotorGroup lift({10, 11});
+inline pros::MotorGroup wrist({13, 14});
+inline pros::Motor arm({12});
 
-// Declares the left side motors
-extern pros::Motor FrontLeft;
-extern pros::Motor MiddleLeft;
-extern pros::Motor BackLeft;
+/**
+ * Declare pistons here
+ */
+inline ez::Piston claw({'A',true});
 
-// Declares the right side motors
-extern pros::Motor FrontRight;
-extern pros::Motor MiddleRight;
-extern pros::Motor BackRight;
+/**
+ * Declare sensors here
+ */
+inline pros::Rotation liftRotation({15});
+inline pros::Rotation armRotation({16});
+inline pros::Rotation wristRotatio({17});
 
-// Declares the left and right  drive motor groups.
-extern pros::MotorGroup RightSide;
-extern pros::MotorGroup LeftSide;
-
-// Declares all the drive motors as a group.
-extern pros::MotorGroup FullDrive;
-
-// Declaring the Intake motors.
-inline pros::Motor stage1({14});
-inline pros::Motor stage2({16});
-inline pros::Motor stage3({12});
-
-//
-inline ez::Piston ramp({'H',true});
-
-// Declaring the piston used to guide the balls into the basket.
-inline ez::Piston deScorer({'G',false});
-
-// Declaring the piston used to get the balls out of the tube.
-inline ez::Piston matchLoader({'F',false});
-
-//
-inline pros::Optical colorSensorOne({20});
-inline pros::Optical colorSensorTwo({11});
-inline pros::Optical colorSensorThree({18});
-
-//
-inline pros::Distance frontDistance({5});
-
-//inline pros::MotorGroup fillingGroup({20, 17, 18});
-//inline pros::MotorGroup scoringGroup({20, 17, 18, 19});
-
-// Function used for the Intake control
-void IntakePresets();
-void ToggleButtons();
-
-// Function used for the color sorter
-void ColorDisplay();
-void CheckRed(std::string Sensor, std::string controlPeriod, bool loop);
-void CheckBlue(std::string Sensor, std::string controlPeriod, bool loop);
-void ColorDetection(std::string wantedColor, bool checkForColor);
-
-// Auton voids
-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();
+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

@@ -3,6 +3,7 @@
 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);

src/EZ-Template/pneumatics.cpp

@@ -0,0 +1,119 @@
+#include "main.h"
+#include "EZ-Template/pneumatics.hpp"
+
+using namespace ez;
+
+void Pneumatics::control_piston(Pistons activePiston, ControlPeriod currPeriod, Controller currController, Buttons buttonUsed) {
+
+    if (currPeriod == ControlPeriod::driver) {
+        switch (activePiston)
+        {
+        case Pistons::claw_piston:
+            /*More code can go here if the claw piston is selected*/
+
+            switch (currController)
+            {
+            case Controller::master:
+                /*More code can go here if we want to use the master controller*/
+
+                switch (buttonUsed)
+                {
+                case Buttons::arrow_buttons:
+                    if (master.get_digital(DIGITAL_LEFT)) {
+                        claw.set(true);
+                    }
+                    else if (master.get_digital(DIGITAL_RIGHT)) {
+                        claw.set(false);
+                    }
+                    break;
+                case Buttons::Y_B_buttons:
+                    if (master.get_digital(DIGITAL_Y)) {
+                        claw.set(true);
+                    }
+                    else if (master.get_digital(DIGITAL_B)) {
+                        claw.set(false);
+                    }
+                    break;
+                case Buttons::left_shoulder_buttons:
+                    if (master.get_digital(DIGITAL_L1)) {
+                        claw.set(true);
+                    }
+                    else if (master.get_digital(DIGITAL_L2)) {
+                        claw.set(false);
+                    }
+                    break;
+                case Buttons::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 Controller::slave:
+
+                switch (buttonUsed)
+                {
+                case Buttons::arrow_buttons:
+                    if (slave.get_digital(DIGITAL_LEFT)) {
+                        claw.set(true);
+                    }
+                    else if (slave.get_digital(DIGITAL_RIGHT)) {
+                        claw.set(false);
+                    }
+                    break;
+                case Buttons::Y_B_buttons:
+                    if (slave.get_digital(DIGITAL_Y)) {
+                        claw.set(true);
+                    }
+                    else if (slave.get_digital(DIGITAL_B)) {
+                        claw.set(false);
+                    }
+                    break;
+                case Buttons::left_shoulder_buttons:
+                    if (slave.get_digital(DIGITAL_L1)) {
+                        claw.set(true);
+                    }
+                    else if (slave.get_digital(DIGITAL_L2)) {
+                        claw.set(false);
+                    }
+                    break;
+                case Buttons::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 Pistons::claw_piston:
+            /*code here*/
+            break;
+        default:
+            break;
+        }
+    }
+}

src/EZ-Template/rotation_mechanisms/arm_rotation.cpp

@@ -0,0 +1,36 @@
+#include "main.h"
+#include "EZ-Template/rotation_mechanisms/rotation_calc.hpp"
+
+using namespace ez;
+
+ArmMech::ArmMech(std::initializer_list<int> _armStates, int _numArmStates) {
+    armStates = _armStates;
+    numArmStates = _numArmStates;
+    currArmState = 0;
+    armTarget = 0;
+    armKP = 0.0173f;
+}
+
+void ArmMech::cycle_states() {
+    currArmState ++;
+
+    if (currArmState == numArmStates) {
+        currArmState = 0;
+    }
+    armTarget = armStates[currArmState];
+}
+
+void ArmMech::reset_state() {
+    currArmState = 0;
+    armTarget = armStates[currArmState];
+}
+
+void ArmMech::arm_control() {
+    armError = armTarget - armRotation.get_position();
+    armVelocity = get_arm_kp() * armError;
+    arm.move(armVelocity);
+}
+
+int ArmMech::get_arm_state() {return armStates[currArmState];}
+int ArmMech::get_num_arm_states() {return numArmStates;}
+int ArmMech::get_arm_kp() {return armKP;}

src/EZ-Template/rotation_mechanisms/lift_rotation.cpp

@@ -0,0 +1,36 @@
+#include "main.h"
+#include "EZ-Template/rotation_mechanisms/rotation_calc.hpp"
+
+using namespace ez;
+
+LiftMech::LiftMech(std::initializer_list<int> _liftStates, int _numLiftStates) {
+    liftStates = _liftStates;
+    numLiftStates = _numLiftStates;
+    currLiftState = 0;
+    liftTarget = 0;
+    liftKP = 0.0173f;
+}
+
+void LiftMech::cycle_states() {
+    currLiftState ++;
+
+    if (currLiftState == numLiftStates) {
+        currLiftState = 0;
+    }
+    liftTarget = liftStates[currLiftState];
+}
+
+void LiftMech::reset_state() {
+    currLiftState = 0;
+    liftTarget = liftStates[currLiftState];
+}
+
+void LiftMech::lift_control() {
+    liftError = liftTarget - liftRotation.get_position();
+    liftVelocity = get_lift_kp() * liftError;
+    lift.move(liftVelocity);
+}
+
+int LiftMech::get_lift_state() {return liftStates[currLiftState];}
+int LiftMech::get_num_lift_states() {return numLiftStates;}
+int LiftMech::get_lift_kp() {return liftKP;}

src/EZ-Template/rotation_mechanisms/wrist_rotation.cpp

@@ -0,0 +1,36 @@
+#include "main.h"
+#include "EZ-Template/rotation_mechanisms/rotation_calc.hpp"
+
+using namespace ez;
+
+WristMech::WristMech(std::initializer_list<int> _wristStates, int _numWristStates) {
+    wristStates = _wristStates;
+    numWristStates = _numWristStates;
+    currWristState = 0;
+    wristTarget = 0;
+    wristKP = 0.0173f;
+}
+
+void WristMech::cycle_states() {
+    currWristState ++;
+
+    if (currWristState == numWristStates) {
+        currWristState = 0;
+    }
+    wristTarget = wristStates[currWristState];
+}
+
+void WristMech::reset_state() {
+    currWristState = 0;
+    wristTarget = wristStates[currWristState];
+}
+
+void WristMech::wrist_control() {
+    wristError = wristTarget - armRotation.get_position();
+    wristVelocity = get_wrist_kp() * wristError;
+    wrist.move(wristVelocity);
+}
+
+int WristMech::get_wrist_state() {return wristStates[currWristState];}
+int WristMech::get_num_wrist_states() {return numWristStates;}
+int WristMech::get_wrist_kp() {return wristKP;}

src/main.cpp

@@ -1,15 +1,16 @@
 #include "main.h"
 
-/////
-// For installation, upgrading, documentations, and tutorials, check out our website!
-// https://ez-robotics.github.io/EZ-Template/
-/////
+// Global Variables
+Pneumatics pistonControl;
+ArmMech armObj;
+LiftMech liftObj;
+WristMech wristObj;
 
 // 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!)
-    {11, 15, 16, 17},  // Right Chassis Ports (negative port will reverse it!)
+    {-1, -2},  // Left Chassis Ports (negative port will reverse it!)
+    {3, 4},  // Right Chassis Ports (negative port will reverse it!)
 
     21,      // IMU Port
     3.25,   // Wheel Diameter (Remember, 4" wheels without screw holes are actually 4.125!)
@@ -23,6 +24,27 @@ 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);
+
+// Lift constructor
+LiftMech liftSetup(
+
+  {0, 100, 200},
+
+  3);
+
+// Wrist constructor
+WristMech wristSetup(
+
+  {0, 100},
+  
+  2);
+
 /**
  * Runs initialization code. This occurs as soon as the program is started.
  *
@@ -35,6 +57,16 @@ void initialize() {
 
   pros::delay(500);  // Stop the user from doing anything while legacy ports configure
 
+  armRotation.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
@@ -250,7 +282,9 @@ void opcontrol() {
     // Gives you some extras to make EZ-Template ezier
     ez_template_extras();
 
-    // Place to call custom functions for driver control
+    // Call custom drive functions here
+    pistonControl.control_piston(Pistons::claw_piston, ControlPeriod::driver, 
+      Controller::master, Buttons::arrow_buttons);
 
     // chassis.opcontrol_tank();  // Tank control
     chassis.opcontrol_arcade_standard(ez::SPLIT);   // Standard split arcade
@@ -258,10 +292,6 @@ void opcontrol() {
     // chassis.opcontrol_arcade_flipped(ez::SPLIT);    // Flipped split arcade
     // chassis.opcontrol_arcade_flipped(ez::SINGLE);   // Flipped single arcade
 
-    // . . .
-    // Put more user control code here!
-    // . . .
-
     pros::delay(ez::util::DELAY_TIME);  // This is used for timer calculations!  Keep this ez::util::DELAY_TIME
   }
 }