From 6d79b61626e642a84c344fcea375944974ad2589 Mon Sep 17 00:00:00 2001 From: Carter Date: Sat, 5 Sep 2026 19:01:14 -0400 Subject: [PATCH] Added a way to control the arm, lift, and wrist mechanisms. 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. --- .../rotation_mechanisms/rotation_calc.hpp | 62 ++++-- include/setup/initialize.hpp | 2 +- .../rotation_mechanisms/arm_rotation.cpp | 183 +++++++++++++++++- .../rotation_mechanisms/lift_rotation.cpp | 143 +++++++++++++- .../rotation_mechanisms/wrist_rotation.cpp | 82 +++++++- src/main.cpp | 22 ++- 6 files changed, 457 insertions(+), 37 deletions(-) diff --git a/include/EZ-Template/rotation_mechanisms/rotation_calc.hpp b/include/EZ-Template/rotation_mechanisms/rotation_calc.hpp index 9aa959d..e848f92 100644 --- a/include/EZ-Template/rotation_mechanisms/rotation_calc.hpp +++ b/include/EZ-Template/rotation_mechanisms/rotation_calc.hpp @@ -1,33 +1,58 @@ #pragma once +#include #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 armStates; - int numArmStates; + std::vector levelOneArmStates; + std::vector levelTwoArmStates; + std::vector levelThreeArmStates; + std::vector levelFourArmStates; + int numArmStates = 2; float armKP; public: + std::vector selectedStates; + int targetPos; int currArmState; int armTarget; float armError; float armVelocity; - + ArmMech(); - ArmMech(std::initializer_list 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 _levelOneArmStates, std::vector _levelTwoArmStates, std::vector _levelThreeArmStates, std::vector _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 liftStates; int numLiftStates; float liftKP; + std::vector firstRange; + std::vector secondRange; + std::vector thirdRange; + std::vector fourthRange; public: int currLiftState; int liftTarget; + int liftHeight; float liftError; float liftVelocity; - + LiftMech(); - LiftMech(std::initializer_list liftStates, int numLiftStates); + LiftMech(std::initializer_list _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 _firstRange, std::vector _secondRange, std::vector _thirdRange, std::vector _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 wristStates, int numWristStates); + WristMech(std::initializer_list _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); }; }; \ No newline at end of file diff --git a/include/setup/initialize.hpp b/include/setup/initialize.hpp index 67c87ff..96d6a15 100644 --- a/include/setup/initialize.hpp +++ b/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}); diff --git a/src/EZ-Template/rotation_mechanisms/arm_rotation.cpp b/src/EZ-Template/rotation_mechanisms/arm_rotation.cpp index b3df61c..98579cf 100644 --- a/src/EZ-Template/rotation_mechanisms/arm_rotation.cpp +++ b/src/EZ-Template/rotation_mechanisms/arm_rotation.cpp @@ -3,27 +3,182 @@ using namespace ez; -ArmMech::ArmMech() {} -ArmMech::ArmMech(std::initializer_list _armStates, int _numArmStates) { - armStates = _armStates; - numArmStates = _numArmStates; +LiftMech liftObj; + +ArmMech::ArmMech() { 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 _levelOneArmStates, std::vector _levelTwoArmStates, std::vector _levelThreeArmStates, std::vector _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;} \ 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 0196997..62a8bc7 100644 --- a/src/EZ-Template/rotation_mechanisms/lift_rotation.cpp +++ b/src/EZ-Template/rotation_mechanisms/lift_rotation.cpp @@ -3,13 +3,124 @@ using namespace ez; +ArmMech armObjRange; + LiftMech::LiftMech() {} LiftMech::LiftMech(std::initializer_list _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 _firstRange, std::vector _secondRange, std::vector _thirdRange, std::vector _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;} \ No newline at end of file +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 d52bfc6..54b4c88 100644 --- a/src/EZ-Template/rotation_mechanisms/wrist_rotation.cpp +++ b/src/EZ-Template/rotation_mechanisms/wrist_rotation.cpp @@ -9,16 +9,80 @@ WristMech::WristMech(std::initializer_list _wristStates, int _numWristState numWristStates = _numWristStates; currWristState = 0; wristTarget = 0; - wristKP = 0.0173f; + set_wrist_kp(0.0173f); } -void WristMech::cycle_states() { - currWristState ++; +void WristMech::wrist_driver_control(Controller currController, Buttons buttonUsed) { + switch (currController) + { + case Controller::master: - if (currWristState == numWristStates) { - currWristState = 0; + switch (buttonUsed) + { + 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;} \ No newline at end of file +float WristMech::get_wrist_kp() {return wristKP;} + +void WristMech::set_wrist_kp(float _wristKP) { + wristKP = _wristKP; +} \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 47ee5b6..747eaad 100644 --- a/src/main.cpp +++ b/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( - - {0, 300}, - - 2); +ArmMech armSetup(); // 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