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;}