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.

src/EZ-Template/color_sorter.cpp

@@ -3,24 +3,25 @@
 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;
-        }
+  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;
-        }
-        
-      }
     }
+    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/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
   }
 }