Merge branch 'main' of http://100.75.58.78:3000/carter/VEX-OverRide

Made the arm check the height of the lift and move to a different state based on the height of the lift
and made the wrist move a specific amount only when the arm passes a threshold.
Added new functions that allow me to quickly change the controller and buttons being used to control
different mechanisms.
Added a new function specificly for autonomouse which allows me to select the desired positon of every
mechanism.
Changed the name of almost every function and enumeration to better match the rest of the program.

include/EZ-Template/rotation_mechanisms/rotation_calc.hpp

@@ -1,33 +1,58 @@
 #pragma once
 
+#include <vector>
 #include "api.h"
 #include "pros/adi.hpp"
 #include "pros/rotation.hpp"
 
 namespace ez{
 
+    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 ArmMech{
         private:
-        std::vector<int> armStates;
+        std::vector<int> levelOneArmStates;
-        int numArmStates;
+        std::vector<int> levelTwoArmStates;
+        std::vector<int> levelThreeArmStates;
+        std::vector<int> levelFourArmStates;
+        int numArmStates = 2;
         float armKP;
 
         public:
+        std::vector<int> selectedStates;
+        int targetPos;
         int currArmState;
         int armTarget;
         float armError;
         float armVelocity;
-
+        
         ArmMech();
-        ArmMech(std::initializer_list<int> armStates, int numArmStates);
 
+        void arm_driver_control(enum Controller, enum Buttons);
+        void arm_auton_control(int pickState);
         void cycle_states();
         void reset_state();
         void arm_control();
 
+        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();
-        int get_arm_kp();
+        float get_arm_kp();
     };
 
     class LiftMech {
@@ -35,23 +60,34 @@ namespace ez{
         std::vector<int> liftStates;
         int numLiftStates;
         float liftKP;
+        std::vector<int> firstRange;
+        std::vector<int> secondRange; 
+        std::vector<int> thirdRange; 
+        std::vector<int> fourthRange;
 
         public:
         int currLiftState;
         int liftTarget;
+        int liftHeight;
         float liftError;
         float liftVelocity;
-
+    
         LiftMech();
-        LiftMech(std::initializer_list<int> liftStates, int numLiftStates);
+        LiftMech(std::initializer_list<int> _liftStates, int _numLiftStates);
 
+        void lift_driver_control(enum Controller, enum Buttons);
+        void lift_auton_control(int pickState);
         void cycle_states();
         void reset_state();
         void lift_control();
+        void lift_range_finder();
+
+        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();
-        int get_lift_kp();
+        float get_lift_kp();
     };
 
     class WristMech {
@@ -67,14 +103,18 @@ namespace ez{
         float wristVelocity;
 
         WristMech();
-        WristMech(std::initializer_list<int> wristStates, int numWristStates);
+        WristMech(std::initializer_list<int> _wristStates, int _numWristStates);
 
-        void cycle_states();
+        void wrist_driver_control(enum Controller, enum Buttons);
+
+        void rotate_wrist(int wristThreshold);
         void reset_state();
         void wrist_control();
 
         int get_wrist_state();
         int get_num_wrist_states();
-        int get_wrist_kp();
+        float get_wrist_kp();
+
+        void set_wrist_kp(float _wristKP);
     };
 };

include/setup/initialize.hpp

@@ -30,7 +30,7 @@ inline ez::Piston claw({'G',true});
  */
 inline pros::Rotation liftRotation({15});
 inline pros::Rotation armRotation({16});
-inline pros::Rotation wristRotatio({17});
+inline pros::Rotation wristRotation({17});
 
 inline pros::Optical colorSensorOne({18});
 inline pros::Optical colorSensorTwo({19});

src/EZ-Template/rotation_mechanisms/arm_rotation.cpp

@@ -3,27 +3,182 @@
 
 using namespace ez;
 
-ArmMech::ArmMech() {}
+LiftMech liftObj;
-ArmMech::ArmMech(std::initializer_list<int> _armStates, int _numArmStates) {
+
-    armStates = _armStates;
+ArmMech::ArmMech() {
-    numArmStates = _numArmStates;
     currArmState = 0;
     armTarget = 0;
-    armKP = 0.0173f;
+    targetPos = 0;
+    set_arm_kp(0.0173f);
+}
+
+void ArmMech::arm_driver_control(Controller currController, Buttons buttonUsed) {
+
+    switch (currController)
+    {
+    case Controller::master:
+            
+        switch (buttonUsed)
+        {
+        case Buttons::arrow_buttons:
+            if (master.get_digital_new_press(DIGITAL_RIGHT)){
+                arm.move(127);
+            }
+            else if (master.get_digital_new_press(DIGITAL_DOWN)) {
+                arm.move(-127);
+            }
+            break;
+        case Buttons::Y_B_buttons:
+            if (master.get_digital_new_press(DIGITAL_Y)){
+                arm.move(127);
+            }
+            else if (master.get_digital_new_press(DIGITAL_B)) {
+                arm.move(-127);
+            }
+            break;
+        case Buttons::left_shoulder_buttons:
+            if (master.get_digital_new_press(DIGITAL_L1)){
+                arm.move(127);
+            }
+            else if (master.get_digital_new_press(DIGITAL_L2)) {
+                arm.move(-127);
+            }
+            break;
+        case Buttons::right_shoulder_buttons:
+            if (master.get_digital_new_press(DIGITAL_R1)){
+                arm.move(127);
+            }
+            else if (master.get_digital_new_press(DIGITAL_R2)) {
+                arm.move(-127);
+            }
+            break;
+        default:
+            break;
+        }
+        break;
+    case Controller::slave:
+            
+        switch (buttonUsed)
+        {
+        case Buttons::arrow_buttons:
+            if (slave.get_digital_new_press(DIGITAL_RIGHT)){
+                cycle_states();
+            }
+            else if (slave.get_digital_new_press(DIGITAL_DOWN)) {
+                reset_state();
+            }
+            break;
+        case Buttons::Y_B_buttons:
+            if (slave.get_digital_new_press(DIGITAL_Y)){
+                cycle_states();
+            }
+            else if (slave.get_digital_new_press(DIGITAL_B)) {
+                reset_state();
+            }
+            break;
+        case Buttons::left_shoulder_buttons:
+            if (slave.get_digital_new_press(DIGITAL_L1)){
+                cycle_states();
+            }
+            else if (slave.get_digital_new_press(DIGITAL_L2)) {
+                reset_state();
+            }
+            break;
+        case Buttons::right_shoulder_buttons:
+            if (slave.get_digital_new_press(DIGITAL_R1)){
+                cycle_states();
+            }
+            else if (slave.get_digital_new_press(DIGITAL_R2)) {
+                reset_state();
+            }
+            break;
+        default:
+            break;
+        }
+        break;
+    default:
+        break;
+    }
+}
+
+void ArmMech::arm_auton_control(int pickState) {
+    liftObj.lift_range_finder();
+
+    switch (targetPos)
+    {
+    case 0:
+        selectedStates = levelOneArmStates;
+        if (pickState == 1) {
+            selectedStates[1];
+        }
+        else {
+            selectedStates[0];
+        }
+        break;
+    case 1:
+        selectedStates = levelOneArmStates;
+        if (pickState == 1) {
+            selectedStates[1];
+        }
+        else {
+            selectedStates[0];
+        }
+        break;
+    case 2:
+        selectedStates = levelTwoArmStates;
+        if (pickState == 1) {
+            selectedStates[1];
+        }
+        else {
+            selectedStates[0];
+        }
+        break;
+    case 3:
+        selectedStates = levelThreeArmStates;
+        if (pickState == 1) {
+            selectedStates[1];
+        }
+        else {
+            selectedStates[0];
+        }
+        break;
+    default:
+        break;
+    }
+
 }
 
 void ArmMech::cycle_states() {
+    liftObj.lift_range_finder();
+
+    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 = armStates[currArmState];
+    armTarget = selectedStates[currArmState];
 }
 
 void ArmMech::reset_state() {
     currArmState = 0;
-    armTarget = armStates[currArmState];
+    armTarget = selectedStates[currArmState];
 }
 
 void ArmMech::arm_control() {
@@ -32,6 +187,16 @@ void ArmMech::arm_control() {
     arm.move(armVelocity);
 }
 
-int ArmMech::get_arm_state() {return armStates[currArmState];}
+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;
+}
+void ArmMech::set_arm_kp(float _armKP) {
+    armKP = _armKP;
+}
+
+int ArmMech::get_arm_state() {return selectedStates[currArmState];}
 int ArmMech::get_num_arm_states() {return numArmStates;}
-int ArmMech::get_arm_kp() {return armKP;}
+float ArmMech::get_arm_kp() {return armKP;}

src/EZ-Template/rotation_mechanisms/lift_rotation.cpp

@@ -3,13 +3,124 @@
 
 using namespace ez;
 
+ArmMech armObjRange;
+
 LiftMech::LiftMech() {}
 LiftMech::LiftMech(std::initializer_list<int> _liftStates, int _numLiftStates) {
     liftStates = _liftStates;
     numLiftStates = _numLiftStates;
     currLiftState = 0;
     liftTarget = 0;
-    liftKP = 0.0173f;
+    set_lift_kp(0.0173f);
+}
+
+void LiftMech::lift_driver_control(Controller currController, Buttons buttonUsed) {
+    switch (currController)
+    {
+    case Controller::master:
+
+        switch (buttonUsed)
+        {
+        case Buttons::arrow_buttons:
+            if (master.get_digital(DIGITAL_RIGHT)) {
+                lift.move(127);
+            }
+            else if (master.get_digital(DIGITAL_DOWN)) {
+                lift.move(-127);
+            }
+            break;
+        case Buttons::Y_B_buttons:
+            if (master.get_digital(DIGITAL_Y)) {
+                lift.move(127);
+            }
+            else if (master.get_digital(DIGITAL_B)) {
+                lift.move(-127);
+            }
+            break;
+        case Buttons::left_shoulder_buttons:
+            if (master.get_digital(DIGITAL_L1)) {
+                lift.move(127);
+            }
+            else if (master.get_digital(DIGITAL_L2)) {
+                lift.move(-127);
+            }
+            break;
+        case Buttons::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 Controller::slave:
+
+        switch (buttonUsed)
+        {
+        case Buttons::arrow_buttons:
+            if (slave.get_digital(DIGITAL_RIGHT)) {
+                cycle_states();
+            }
+            else if (slave.get_digital(DIGITAL_DOWN)) {
+                reset_state();
+            }
+            break;
+        case Buttons::Y_B_buttons:
+            if (slave.get_digital(DIGITAL_Y)) {
+                cycle_states();
+            }
+            else if (slave.get_digital(DIGITAL_B)) {
+                reset_state();
+            }
+            break;
+        case Buttons::left_shoulder_buttons:
+            if (slave.get_digital(DIGITAL_L1)) {
+                cycle_states();
+            }
+            else if (slave.get_digital(DIGITAL_L2)) {
+                reset_state();
+            }
+            break;
+        case Buttons::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() {
@@ -32,6 +143,34 @@ void LiftMech::lift_control() {
     lift.move(liftVelocity);
 }
 
+void LiftMech::lift_range_finder() {
+    liftHeight = liftRotation.get_position();
+
+    if (liftHeight >= firstRange[0] && liftHeight <= firstRange[1]) {
+        armObjRange.targetPos = 0;
+    }
+    else if (liftHeight >= secondRange[0] && liftHeight <= secondRange[1]) {
+        armObjRange.targetPos = 1;
+    }
+    else if (liftHeight >= thirdRange[0] && liftHeight <= thirdRange[1]) {
+        armObjRange.targetPos = 2;
+    }
+    else if (liftHeight >= fourthRange[0] && liftHeight <= fourthRange[1]) {
+        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() {return liftStates[currLiftState];}
 int LiftMech::get_num_lift_states() {return numLiftStates;}
-int LiftMech::get_lift_kp() {return liftKP;}
+float LiftMech::get_lift_kp() {return liftKP;}

src/EZ-Template/rotation_mechanisms/wrist_rotation.cpp

@@ -9,16 +9,80 @@ WristMech::WristMech(std::initializer_list<int> _wristStates, int _numWristState
     numWristStates = _numWristStates;
     currWristState = 0;
     wristTarget = 0;
-    wristKP = 0.0173f;
+    set_wrist_kp(0.0173f);
 }
 
-void WristMech::cycle_states() {
+void WristMech::wrist_driver_control(Controller currController, Buttons buttonUsed) {
-    currWristState ++;
+    switch (currController)
+    {
+    case Controller::master:
 
-    if (currWristState == numWristStates) {
+        switch (buttonUsed)
-        currWristState = 0;
+        {
+        case Buttons::arrow_buttons:
+            if (master.get_digital(DIGITAL_DOWN)) {
+                reset_state();
+            }
+            break;
+        case Buttons::Y_B_buttons:
+            if (master.get_digital(DIGITAL_Y)) {
+                reset_state();
+            }
+            break;
+        case Buttons::left_shoulder_buttons:
+            if (master.get_digital(DIGITAL_L1)) {
+                reset_state();
+            }
+            break;
+        case Buttons::right_shoulder_buttons:
+            if (master.get_digital(DIGITAL_R1)) {
+                reset_state();
+            }
+            break;
+        default:
+            break;
+        }
+        break;
+    case Controller::slave:
+
+        switch (buttonUsed)
+        {
+        case Buttons::arrow_buttons:
+            if (slave.get_digital(DIGITAL_DOWN)) {
+                reset_state();
+            }
+            break;
+        case Buttons::Y_B_buttons:
+            if (slave.get_digital(DIGITAL_Y)) {
+                reset_state();
+            }
+            break;
+        case Buttons::left_shoulder_buttons:
+            if (slave.get_digital(DIGITAL_L1)) {
+                reset_state();
+            }
+            break;
+        case Buttons::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];
     }
-    wristTarget = wristStates[currWristState];
 }
 
 void WristMech::reset_state() {
@@ -34,4 +98,8 @@ void WristMech::wrist_control() {
 
 int WristMech::get_wrist_state() {return wristStates[currWristState];}
 int WristMech::get_num_wrist_states() {return numWristStates;}
-int WristMech::get_wrist_kp() {return wristKP;}
+float WristMech::get_wrist_kp() {return wristKP;}
+
+void WristMech::set_wrist_kp(float _wristKP) {
+    wristKP = _wristKP;
+}

src/main.cpp

@@ -25,18 +25,14 @@ ez::Drive chassis(
 // ez::tracking_wheel vert_tracker(9, 2.75, 4.0);   // This tracking wheel is parallel to the drive wheels
 
 // Arm constructor
-ArmMech armSetup(
+ArmMech armSetup();
-    
-  {0, 300}, 
-  
-  2);
 
 // Lift constructor
 LiftMech liftSetup(
 
-  {0, 100, 200},
+  {0, 100, 200, 300},
 
-  3);
+  4);
 
 // Wrist constructor
 WristMech wristSetup(
@@ -57,7 +53,12 @@ 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});
+
   armRotation.reset_position();
+  liftRotation.reset_position();
+  wristRotation.reset_position();
+
   pros::Task liftControlTasck([&]{
     while (true) {
       armObj.arm_control();
@@ -286,6 +287,13 @@ void opcontrol() {
     pistonControl.control_piston(Pistons::claw_piston, ControlPeriod::driver, 
       Controller::slave, Buttons::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(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_arcade_standard(ez::SINGLE);  // Standard single arcade