From 24ee947fd5cc7391c571a03bb8fce32a303c30cc Mon Sep 17 00:00:00 2001 From: Carter Date: Wed, 9 Sep 2026 13:29:07 -0400 Subject: [PATCH] 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. --- include/EZ-Template/pneumatics.hpp | 27 ++- .../rotation_mechanisms/rotation_calc.hpp | 71 ++++-- include/setup/initialize.hpp | 30 ++- src/EZ-Template/pneumatics.cpp | 28 +-- .../rotation_mechanisms/arm_rotation.cpp | 205 ++++++++++++------ .../rotation_mechanisms/lift_rotation.cpp | 67 +++--- .../rotation_mechanisms/wrist_rotation.cpp | 41 ++-- src/main.cpp | 63 +++--- 8 files changed, 340 insertions(+), 192 deletions(-) diff --git a/include/EZ-Template/pneumatics.hpp b/include/EZ-Template/pneumatics.hpp index d1e29b6..fd24e6f 100644 --- a/include/EZ-Template/pneumatics.hpp +++ b/include/EZ-Template/pneumatics.hpp @@ -6,24 +6,35 @@ #include "EZ-Template/piston.hpp" namespace ez { - - enum class Pistons { - claw_piston - }; - enum class ControlPeriod { + /** + * Enumeration to hold the current part of a match were in. + */ + enum class ControlPeriodP { driver, auton }; - enum class Controller { + /** + * Enumeration to hold the different controllers avalable. + */ + enum class ControllerP { master, slave }; - enum class Buttons { + /** + * 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{ @@ -34,7 +45,7 @@ namespace ez { Pneumatics(); - void control_piston(enum Pistons, enum ControlPeriod, enum Controller, enum Buttons); + void control_piston(enum PistonsP, enum ControlPeriodP, enum ControllerP, enum ButtonsP); bool get_piston_state(); void set_piston_state(bool value); diff --git a/include/EZ-Template/rotation_mechanisms/rotation_calc.hpp b/include/EZ-Template/rotation_mechanisms/rotation_calc.hpp index e848f92..f302b0e 100644 --- a/include/EZ-Template/rotation_mechanisms/rotation_calc.hpp +++ b/include/EZ-Template/rotation_mechanisms/rotation_calc.hpp @@ -2,28 +2,43 @@ #include #include "api.h" +//#include "main.h" #include "pros/adi.hpp" #include "pros/rotation.hpp" namespace ez{ - - enum class ControlPeriod { + /** + * Enumeration to hold the current part of a match were in. + */ + enum class ControlPeriodR { driver, auton }; - enum class Controller { + /** + * Enumeration to hold the different controllers avalable. + */ + enum class ControllerR { master, slave }; - enum class Buttons { + /** + * 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 levelOneArmStates; std::vector levelTwoArmStates; std::vector levelThreeArmStates; @@ -32,20 +47,20 @@ namespace ez{ float armKP; public: - std::vector selectedStates; - int targetPos; + std::vector 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(); + ArmMech(); // Defualt constructor - void arm_driver_control(enum Controller, enum Buttons); + void arm_driver_control(enum ControllerR, enum ButtonsR); void arm_auton_control(int pickState); void cycle_states(); void reset_state(); - void arm_control(); + void arm_control(); // Computes the velocity for the arm to move at. void set_arm_kp(float _armKP); void set_state_values(std::vector _levelOneArmStates, std::vector _levelTwoArmStates, std::vector _levelThreeArmStates, std::vector _levelFourArmStates); @@ -55,32 +70,37 @@ namespace ez{ float get_arm_kp(); }; + /** + * List mechanism class. + * + * Sets up all constructors, variables, and functions. + */ class LiftMech { private: - std::vector liftStates; - int numLiftStates; - float liftKP; + std::vector liftStates; // Array the lift uses to move. std::vector firstRange; std::vector secondRange; std::vector thirdRange; std::vector fourthRange; + int numLiftStates; + float liftKP; public: + int liftHeight; // Current height of the lift in centdegrees int currLiftState; int liftTarget; - int liftHeight; float liftError; float liftVelocity; - LiftMech(); - LiftMech(std::initializer_list _liftStates, int _numLiftStates); + LiftMech(); // Default constructor + LiftMech(std::initializer_list _liftStates, int _numLiftStates); // Constructor to initialze array and amount of values in array. - void lift_driver_control(enum Controller, enum Buttons); + void lift_driver_control(enum ControllerR, enum ButtonsR); void lift_auton_control(int pickState); void cycle_states(); void reset_state(); - void lift_control(); - void lift_range_finder(); + 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 _firstRange, std::vector _secondRange, std::vector _thirdRange, std::vector _fourthRange); @@ -90,9 +110,14 @@ namespace ez{ float get_lift_kp(); }; + /** + * Wrist mechanism class. + * + * Sets up all constructors, variables, and functions. + */ class WristMech { private: - std::vector wristStates; + std::vector wristStates; // Array the wrist uses to move int numWristStates; float wristKP; @@ -102,12 +127,12 @@ namespace ez{ float wristError; float wristVelocity; - WristMech(); - WristMech(std::initializer_list _wristStates, int _numWristStates); + WristMech(); // Default constuctor + WristMech(std::initializer_list _wristStates, int _numWristStates); // Constructor to initialze array and amount of values in array. - void wrist_driver_control(enum Controller, enum Buttons); + void wrist_driver_control(enum ControllerR, enum ButtonsR); - void rotate_wrist(int wristThreshold); + void rotate_wrist(int wristThreshold); // Rotates the wrist based where the arm is. void reset_state(); void wrist_control(); diff --git a/include/setup/initialize.hpp b/include/setup/initialize.hpp index 96d6a15..a9fa5f7 100644 --- a/include/setup/initialize.hpp +++ b/include/setup/initialize.hpp @@ -8,29 +8,39 @@ #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); /** - * Declare mechinism motors + * Declares mechinism motors and motor groups. */ -inline pros::MotorGroup lift({10, 11}); -inline pros::MotorGroup wrist({13, 14}); -inline pros::Motor arm({12}); +inline pros::MotorGroup lift({15, -19}); +inline pros::MotorGroup wrist({1, 9}); +inline pros::Motor arm({6}); /** - * Declare pistons here + * Declares piston mechanisms. */ inline ez::Piston claw({'G',true}); /** - * Declare sensors here + * Declare limit switches and other ADI sensors */ -inline pros::Rotation liftRotation({15}); -inline pros::Rotation armRotation({16}); -inline pros::Rotation wristRotation({17}); +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}); diff --git a/src/EZ-Template/pneumatics.cpp b/src/EZ-Template/pneumatics.cpp index 7e6896e..4a42225 100644 --- a/src/EZ-Template/pneumatics.cpp +++ b/src/EZ-Template/pneumatics.cpp @@ -5,22 +5,22 @@ using namespace ez; Pneumatics::Pneumatics(){} -void Pneumatics::control_piston(Pistons activePiston, ControlPeriod currPeriod, Controller currController, Buttons buttonUsed) { +void Pneumatics::control_piston(PistonsP activePiston, ControlPeriodP currPeriod, ControllerP currController, ButtonsP buttonUsed) { - if (currPeriod == ControlPeriod::driver) { + if (currPeriod == ControlPeriodP::driver) { switch (activePiston) { - case Pistons::claw_piston: + case PistonsP::claw_piston: /*More code can go here if the claw piston is selected*/ switch (currController) { - case Controller::master: + case ControllerP::master: /*More code can go here if we want to use the master controller*/ switch (buttonUsed) { - case Buttons::arrow_buttons: + case ButtonsP::arrow_buttons: if (master.get_digital(DIGITAL_RIGHT)) { claw.set(true); } @@ -28,7 +28,7 @@ void Pneumatics::control_piston(Pistons activePiston, ControlPeriod currPeriod, claw.set(false); } break; - case Buttons::Y_B_buttons: + case ButtonsP::Y_B_buttons: if (master.get_digital(DIGITAL_Y)) { claw.set(true); } @@ -36,7 +36,7 @@ void Pneumatics::control_piston(Pistons activePiston, ControlPeriod currPeriod, claw.set(false); } break; - case Buttons::left_shoulder_buttons: + case ButtonsP::left_shoulder_buttons: if (master.get_digital(DIGITAL_L1)) { claw.set(true); } @@ -44,7 +44,7 @@ void Pneumatics::control_piston(Pistons activePiston, ControlPeriod currPeriod, claw.set(false); } break; - case Buttons::right_shoulder_buttons: + case ButtonsP::right_shoulder_buttons: if (master.get_digital(DIGITAL_R1)) { claw.set(true); } @@ -56,11 +56,11 @@ void Pneumatics::control_piston(Pistons activePiston, ControlPeriod currPeriod, break; } break; - case Controller::slave: + case ControllerP::slave: switch (buttonUsed) { - case Buttons::arrow_buttons: + case ButtonsP::arrow_buttons: if (slave.get_digital(DIGITAL_RIGHT)) { claw.set(true); } @@ -68,7 +68,7 @@ void Pneumatics::control_piston(Pistons activePiston, ControlPeriod currPeriod, claw.set(false); } break; - case Buttons::Y_B_buttons: + case ButtonsP::Y_B_buttons: if (slave.get_digital(DIGITAL_Y)) { claw.set(true); } @@ -76,7 +76,7 @@ void Pneumatics::control_piston(Pistons activePiston, ControlPeriod currPeriod, claw.set(false); } break; - case Buttons::left_shoulder_buttons: + case ButtonsP::left_shoulder_buttons: if (slave.get_digital(DIGITAL_L1)) { claw.set(true); } @@ -84,7 +84,7 @@ void Pneumatics::control_piston(Pistons activePiston, ControlPeriod currPeriod, claw.set(false); } break; - case Buttons::right_shoulder_buttons: + case ButtonsP::right_shoulder_buttons: if (slave.get_digital(DIGITAL_R1)) { claw.set(true); } @@ -111,7 +111,7 @@ void Pneumatics::control_piston(Pistons activePiston, ControlPeriod currPeriod, else { switch (activePiston) { - case Pistons::claw_piston: + case PistonsP::claw_piston: /*code here*/ break; default: diff --git a/src/EZ-Template/rotation_mechanisms/arm_rotation.cpp b/src/EZ-Template/rotation_mechanisms/arm_rotation.cpp index 98579cf..ce65011 100644 --- a/src/EZ-Template/rotation_mechanisms/arm_rotation.cpp +++ b/src/EZ-Template/rotation_mechanisms/arm_rotation.cpp @@ -3,52 +3,69 @@ using namespace ez; -LiftMech liftObj; +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 } -void ArmMech::arm_driver_control(Controller currController, Buttons buttonUsed) { +/** + * 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 (currController) // Switch that check which controller was selected { - case Controller::master: - + case ControllerR::master: + // Code you want to run when the master controller is selcted switch (buttonUsed) { - case Buttons::arrow_buttons: - if (master.get_digital_new_press(DIGITAL_RIGHT)){ + 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_new_press(DIGITAL_DOWN)) { + else if (master.get_digital(DIGITAL_DOWN)) { arm.move(-127); } break; - case Buttons::Y_B_buttons: - if (master.get_digital_new_press(DIGITAL_Y)){ + 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_new_press(DIGITAL_B)) { + else if (master.get_digital(DIGITAL_B)) { arm.move(-127); } break; - case Buttons::left_shoulder_buttons: - if (master.get_digital_new_press(DIGITAL_L1)){ + 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_new_press(DIGITAL_L2)) { + else if (master.get_digital(DIGITAL_L2)) { arm.move(-127); } break; - case Buttons::right_shoulder_buttons: - if (master.get_digital_new_press(DIGITAL_R1)){ + 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_new_press(DIGITAL_R2)) { + else if (master.get_digital(DIGITAL_R2)) { arm.move(-127); } break; @@ -56,39 +73,43 @@ void ArmMech::arm_driver_control(Controller currController, Buttons buttonUsed) break; } break; - case Controller::slave: - + case ControllerR::slave: + // Code you want to run when the slave controller is selected switch (buttonUsed) { - case Buttons::arrow_buttons: - if (slave.get_digital_new_press(DIGITAL_RIGHT)){ + 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_new_press(DIGITAL_DOWN)) { + else if (slave.get_digital(DIGITAL_DOWN)) { reset_state(); } break; - case Buttons::Y_B_buttons: - if (slave.get_digital_new_press(DIGITAL_Y)){ + 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_new_press(DIGITAL_B)) { + else if (slave.get_digital(DIGITAL_B)) { reset_state(); } break; - case Buttons::left_shoulder_buttons: - if (slave.get_digital_new_press(DIGITAL_L1)){ + 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_new_press(DIGITAL_L2)) { + else if (slave.get_digital(DIGITAL_L2)) { reset_state(); } break; - case Buttons::right_shoulder_buttons: - if (slave.get_digital_new_press(DIGITAL_R1)){ + 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_new_press(DIGITAL_R2)) { + else if (slave.get_digital(DIGITAL_R2)) { reset_state(); } break; @@ -101,45 +122,51 @@ void ArmMech::arm_driver_control(Controller currController, Buttons buttonUsed) } } +/** + * 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) { - liftObj.lift_range_finder(); + liftRangeAccess.lift_range_finder(); // Returns a value 0 - 3 based on where the lift is currently switch (targetPos) { case 0: - selectedStates = levelOneArmStates; + selectedStates = levelOneArmStates; // Makes the main array equal the level one array if (pickState == 1) { - selectedStates[1]; + armTarget = selectedStates[1]; // Makes the target value the second in the array } else { - selectedStates[0]; + armTarget = selectedStates[0]; // Makes the target value the first in the array } break; case 1: - selectedStates = levelOneArmStates; + selectedStates = levelOneArmStates; // Make the main array equal the level two array if (pickState == 1) { - selectedStates[1]; + armTarget = selectedStates[1]; } else { - selectedStates[0]; + armTarget = selectedStates[0]; } break; case 2: - selectedStates = levelTwoArmStates; + selectedStates = levelTwoArmStates; // Makes the main array equal the level three array if (pickState == 1) { - selectedStates[1]; + armTarget = selectedStates[1]; } else { - selectedStates[0]; + armTarget = selectedStates[0]; } break; case 3: - selectedStates = levelThreeArmStates; + selectedStates = levelThreeArmStates; // Makes the main array equal the level four array if (pickState == 1) { - selectedStates[1]; + armTarget = selectedStates[1]; } else { - selectedStates[0]; + armTarget = selectedStates[0]; } break; default: @@ -148,55 +175,95 @@ void ArmMech::arm_auton_control(int pickState) { } +/** + * Function to cycle through each value in the array + */ void ArmMech::cycle_states() { - liftObj.lift_range_finder(); + liftRangeAccess.lift_range_finder(); // Returns a value 0 - 3 based on were the lift is currently - switch (targetPos) - { - case 0: - selectedStates = levelOneArmStates; - break; - case 1: - selectedStates = levelTwoArmStates; - break; - case 2: - selectedStates = levelThreeArmStates; - break; - case 3: - selectedStates = levelFourArmStates; - break; - default: - break; + // 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 ++; + currArmState ++; - if (currArmState == numArmStates) { + if (currArmState >= numArmStates) { currArmState = 0; } - armTarget = selectedStates[currArmState]; + 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; - armTarget = selectedStates[currArmState]; + + // 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(); - armVelocity = get_arm_kp() * armError; - arm.move(armVelocity); + 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 _levelOneArmStates, std::vector _levelTwoArmStates, std::vector _levelThreeArmStates, std::vector _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() {return selectedStates[currArmState];} +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;} \ No newline at end of file diff --git a/src/EZ-Template/rotation_mechanisms/lift_rotation.cpp b/src/EZ-Template/rotation_mechanisms/lift_rotation.cpp index 62a8bc7..891ad78 100644 --- a/src/EZ-Template/rotation_mechanisms/lift_rotation.cpp +++ b/src/EZ-Template/rotation_mechanisms/lift_rotation.cpp @@ -5,7 +5,13 @@ using namespace ez; ArmMech armObjRange; -LiftMech::LiftMech() {} +LiftMech::LiftMech() { + liftHeight = 0; + liftError = 0; + liftVelocity = 0; + + liftStates = {0, 0, 0, 0}; +} LiftMech::LiftMech(std::initializer_list _liftStates, int _numLiftStates) { liftStates = _liftStates; numLiftStates = _numLiftStates; @@ -14,14 +20,14 @@ LiftMech::LiftMech(std::initializer_list _liftStates, int _numLiftStates) { set_lift_kp(0.0173f); } -void LiftMech::lift_driver_control(Controller currController, Buttons buttonUsed) { +void LiftMech::lift_driver_control(ControllerR currController, ButtonsR buttonUsed) { switch (currController) { - case Controller::master: + case ControllerR::master: switch (buttonUsed) { - case Buttons::arrow_buttons: + case ButtonsR::arrow_buttons: if (master.get_digital(DIGITAL_RIGHT)) { lift.move(127); } @@ -29,7 +35,7 @@ void LiftMech::lift_driver_control(Controller currController, Buttons buttonUsed lift.move(-127); } break; - case Buttons::Y_B_buttons: + case ButtonsR::Y_B_buttons: if (master.get_digital(DIGITAL_Y)) { lift.move(127); } @@ -37,7 +43,7 @@ void LiftMech::lift_driver_control(Controller currController, Buttons buttonUsed lift.move(-127); } break; - case Buttons::left_shoulder_buttons: + case ButtonsR::left_shoulder_buttons: if (master.get_digital(DIGITAL_L1)) { lift.move(127); } @@ -45,7 +51,7 @@ void LiftMech::lift_driver_control(Controller currController, Buttons buttonUsed lift.move(-127); } break; - case Buttons::right_shoulder_buttons: + case ButtonsR::right_shoulder_buttons: if (master.get_digital(DIGITAL_R1)) { lift.move(127); } @@ -57,11 +63,11 @@ void LiftMech::lift_driver_control(Controller currController, Buttons buttonUsed break; } break; - case Controller::slave: + case ControllerR::slave: switch (buttonUsed) { - case Buttons::arrow_buttons: + case ButtonsR::arrow_buttons: if (slave.get_digital(DIGITAL_RIGHT)) { cycle_states(); } @@ -69,7 +75,7 @@ void LiftMech::lift_driver_control(Controller currController, Buttons buttonUsed reset_state(); } break; - case Buttons::Y_B_buttons: + case ButtonsR::Y_B_buttons: if (slave.get_digital(DIGITAL_Y)) { cycle_states(); } @@ -77,7 +83,7 @@ void LiftMech::lift_driver_control(Controller currController, Buttons buttonUsed reset_state(); } break; - case Buttons::left_shoulder_buttons: + case ButtonsR::left_shoulder_buttons: if (slave.get_digital(DIGITAL_L1)) { cycle_states(); } @@ -85,7 +91,7 @@ void LiftMech::lift_driver_control(Controller currController, Buttons buttonUsed reset_state(); } break; - case Buttons::right_shoulder_buttons: + case ButtonsR::right_shoulder_buttons: if (slave.get_digital(DIGITAL_R1)) { cycle_states(); } @@ -126,15 +132,20 @@ void LiftMech::lift_auton_control(int pickState) { void LiftMech::cycle_states() { currLiftState ++; - if (currLiftState == numLiftStates) { - currLiftState = 0; + if (!liftStates.empty()) { + if (currLiftState == numLiftStates) { + currLiftState = 0; + } + liftTarget = liftStates[currLiftState]; } - liftTarget = liftStates[currLiftState]; } void LiftMech::reset_state() { currLiftState = 0; - liftTarget = liftStates[currLiftState]; + + if (!liftStates.empty()) { + liftTarget = liftStates[currLiftState]; + } } void LiftMech::lift_control() { @@ -143,20 +154,20 @@ void LiftMech::lift_control() { lift.move(liftVelocity); } -void LiftMech::lift_range_finder() { +int LiftMech::lift_range_finder() { liftHeight = liftRotation.get_position(); - if (liftHeight >= firstRange[0] && liftHeight <= firstRange[1]) { - armObjRange.targetPos = 0; + if (firstRange.size() >= 2 && liftHeight >= firstRange[0] && liftHeight <= firstRange[1]) { + return armObjRange.targetPos = 0; } - else if (liftHeight >= secondRange[0] && liftHeight <= secondRange[1]) { - armObjRange.targetPos = 1; + else if (secondRange.size() >= 2 && liftHeight >= secondRange[0] && liftHeight <= secondRange[1]) { + return armObjRange.targetPos = 1; } - else if (liftHeight >= thirdRange[0] && liftHeight <= thirdRange[1]) { - armObjRange.targetPos = 2; + else if (thirdRange.size() >= 2 && liftHeight >= thirdRange[0] && liftHeight <= thirdRange[1]) { + return armObjRange.targetPos = 2; } - else if (liftHeight >= fourthRange[0] && liftHeight <= fourthRange[1]) { - armObjRange.targetPos = 3; + else if (fourthRange.size() >= 2 && liftHeight >= fourthRange[0] && liftHeight <= fourthRange[1]) { + return armObjRange.targetPos = 3; } } @@ -171,6 +182,10 @@ void LiftMech::set_ranges(std::vector _firstRange, std::vector _second fourthRange = _fourthRange; } -int LiftMech::get_lift_state() {return liftStates[currLiftState];} +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;} \ No newline at end of file diff --git a/src/EZ-Template/rotation_mechanisms/wrist_rotation.cpp b/src/EZ-Template/rotation_mechanisms/wrist_rotation.cpp index 54b4c88..d9966ee 100644 --- a/src/EZ-Template/rotation_mechanisms/wrist_rotation.cpp +++ b/src/EZ-Template/rotation_mechanisms/wrist_rotation.cpp @@ -3,7 +3,12 @@ using namespace ez; -WristMech::WristMech(){} +WristMech::WristMech() { + wristError = 0; + wristVelocity = 0; + + wristStates = {0, 0}; +} WristMech::WristMech(std::initializer_list _wristStates, int _numWristStates) { wristStates = _wristStates; numWristStates = _numWristStates; @@ -12,29 +17,29 @@ WristMech::WristMech(std::initializer_list _wristStates, int _numWristState set_wrist_kp(0.0173f); } -void WristMech::wrist_driver_control(Controller currController, Buttons buttonUsed) { +void WristMech::wrist_driver_control(ControllerR currController, ButtonsR buttonUsed) { switch (currController) { - case Controller::master: + case ControllerR::master: switch (buttonUsed) { - case Buttons::arrow_buttons: + case ButtonsR::arrow_buttons: if (master.get_digital(DIGITAL_DOWN)) { reset_state(); } break; - case Buttons::Y_B_buttons: + case ButtonsR::Y_B_buttons: if (master.get_digital(DIGITAL_Y)) { reset_state(); } break; - case Buttons::left_shoulder_buttons: + case ButtonsR::left_shoulder_buttons: if (master.get_digital(DIGITAL_L1)) { reset_state(); } break; - case Buttons::right_shoulder_buttons: + case ButtonsR::right_shoulder_buttons: if (master.get_digital(DIGITAL_R1)) { reset_state(); } @@ -43,26 +48,26 @@ void WristMech::wrist_driver_control(Controller currController, Buttons buttonUs break; } break; - case Controller::slave: + case ControllerR::slave: switch (buttonUsed) { - case Buttons::arrow_buttons: + case ButtonsR::arrow_buttons: if (slave.get_digital(DIGITAL_DOWN)) { reset_state(); } break; - case Buttons::Y_B_buttons: + case ButtonsR::Y_B_buttons: if (slave.get_digital(DIGITAL_Y)) { reset_state(); } break; - case Buttons::left_shoulder_buttons: + case ButtonsR::left_shoulder_buttons: if (slave.get_digital(DIGITAL_L1)) { reset_state(); } break; - case Buttons::right_shoulder_buttons: + case ButtonsR::right_shoulder_buttons: if (slave.get_digital(DIGITAL_R1)) { reset_state(); } @@ -87,16 +92,22 @@ void WristMech::rotate_wrist(int wristThreshold) { void WristMech::reset_state() { currWristState = 0; - wristTarget = wristStates[currWristState]; + if (!wristStates.empty()) { + wristTarget = wristStates[currWristState]; + } } void WristMech::wrist_control() { - wristError = wristTarget - armRotation.get_position(); + wristError = wristTarget - wristRotation.get_position(); wristVelocity = get_wrist_kp() * wristError; wrist.move(wristVelocity); } -int WristMech::get_wrist_state() {return wristStates[currWristState];} +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;} diff --git a/src/main.cpp b/src/main.cpp index 747eaad..0191a8e 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,10 +1,5 @@ #include "main.h" - -// Global Variables -Pneumatics pistonControl; -ArmMech armObj; -LiftMech liftObj; -WristMech wristObj; +#include // Chassis constructor ez::Drive chassis( @@ -24,22 +19,19 @@ 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(); +// Default Objects +Pneumatics pistonControl; +ArmMech armObj; // Lift constructor -LiftMech liftSetup( +LiftMech liftObj( - {0, 100, 200, 300}, - - 4); + {0, 63194, 94246, 139878}, 4); // Wrist constructor -WristMech wristSetup( +WristMech wristObj( - {0, 100}, - - 2); + {0, -14529}, 2); /** * Runs initialization code. This occurs as soon as the program is started. @@ -53,7 +45,18 @@ 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}); + 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(); @@ -66,7 +69,7 @@ void initialize() { 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 @@ -284,22 +287,28 @@ void opcontrol() { ez_template_extras(); // Call custom drive functions here - pistonControl.control_piston(Pistons::claw_piston, ControlPeriod::driver, - Controller::slave, Buttons::arrow_buttons); + pistonControl.control_piston(PistonsP::claw_piston, ControlPeriodP::driver, + ControllerP::slave, ButtonsP::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(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); - 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_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 + 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 } }