diff --git a/include/EZ-Template/color_sorter.hpp b/include/EZ-Template/color_sorter.hpp index 22e787e..78798e7 100644 --- a/include/EZ-Template/color_sorter.hpp +++ b/include/EZ-Template/color_sorter.hpp @@ -22,9 +22,6 @@ namespace ez { class color_sorter { public: - - color_sorter(); - /** * * \param sensorNum diff --git a/include/EZ-Template/pneumatics.hpp b/include/EZ-Template/pneumatics.hpp new file mode 100644 index 0000000..d1e29b6 --- /dev/null +++ b/include/EZ-Template/pneumatics.hpp @@ -0,0 +1,43 @@ +#pragma once + +#include "api.h" +#include "pros/adi.hpp" +#include "pros/serial.hpp" +#include "EZ-Template/piston.hpp" + +namespace ez { + + enum class Pistons { + claw_piston + }; + 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 Pneumatics{ + + private: + bool pistonState; + + public: + + Pneumatics(); + + void control_piston(enum Pistons, enum ControlPeriod, enum Controller, enum Buttons); + + bool get_piston_state(); + void set_piston_state(bool value); + + }; +}; \ No newline at end of file diff --git a/include/EZ-Template/rotation_mechanisms/rotation_calc.hpp b/include/EZ-Template/rotation_mechanisms/rotation_calc.hpp new file mode 100644 index 0000000..9aa959d --- /dev/null +++ b/include/EZ-Template/rotation_mechanisms/rotation_calc.hpp @@ -0,0 +1,80 @@ +#pragma once + +#include "api.h" +#include "pros/adi.hpp" +#include "pros/rotation.hpp" + +namespace ez{ + + class ArmMech{ + private: + std::vector armStates; + int numArmStates; + float armKP; + + public: + int currArmState; + int armTarget; + float armError; + float armVelocity; + + ArmMech(); + ArmMech(std::initializer_list armStates, int numArmStates); + + void cycle_states(); + void reset_state(); + void arm_control(); + + int get_arm_state(); + int get_num_arm_states(); + int get_arm_kp(); + }; + + class LiftMech { + private: + std::vector liftStates; + int numLiftStates; + float liftKP; + + public: + int currLiftState; + int liftTarget; + float liftError; + float liftVelocity; + + LiftMech(); + LiftMech(std::initializer_list liftStates, int numLiftStates); + + void cycle_states(); + void reset_state(); + void lift_control(); + + int get_lift_state(); + int get_num_lift_states(); + int get_lift_kp(); + }; + + class WristMech { + private: + std::vector wristStates; + int numWristStates; + float wristKP; + + public: + int currWristState; + int wristTarget; + float wristError; + float wristVelocity; + + WristMech(); + WristMech(std::initializer_list wristStates, int numWristStates); + + void cycle_states(); + void reset_state(); + void wrist_control(); + + int get_wrist_state(); + int get_num_wrist_states(); + int get_wrist_kp(); + }; +}; \ No newline at end of file diff --git a/include/EZ-Template/util.hpp b/include/EZ-Template/util.hpp index 0f8f552..2322b85 100644 --- a/include/EZ-Template/util.hpp +++ b/include/EZ-Template/util.hpp @@ -18,11 +18,6 @@ file, You can obtain one at http://mozilla.org/MPL/2.0/. using namespace okapi::literals; -/** - * Controller. - */ -extern pros::Controller master; - namespace ez { /** diff --git a/include/autons.hpp b/include/autons.hpp index 99ae3ab..46c1ce8 100644 --- a/include/autons.hpp +++ b/include/autons.hpp @@ -2,6 +2,22 @@ void default_constants(); +/** + * Personal auton routes + */ +void leftMain(std::string side); +void rightMain(std::string side); +void Skills(); +void BlueRight(); +void BlueLeft(); +void RedRight(); +void RedLeft(); +void testMain(std::string side); +void Test(); + +/** + * Template auton routes + */ void drive_example(); void turn_example(); void drive_and_turn(); diff --git a/include/main.h b/include/main.h index 7f02021..7ed0cc5 100644 --- a/include/main.h +++ b/include/main.h @@ -47,6 +47,8 @@ // More includes here... #include "autons.hpp" #include "subsystems.hpp" +#include "EZ-Template/rotation_mechanisms/rotation_calc.hpp" +#include "EZ-Template/pneumatics.hpp" /** * If you find doing pros::Motor() to be tedious and you'd prefer just to do diff --git a/include/setup/initialize.hpp b/include/setup/initialize.hpp index dcb62d2..724cbdd 100644 --- a/include/setup/initialize.hpp +++ b/include/setup/initialize.hpp @@ -7,73 +7,32 @@ #ifndef CONFIG_H #define CONFIG_H -extern bool gotRed; -extern bool gotBlue; -extern double currentColor; +/** + * + */ +inline pros::Controller master; +inline pros::Controller slave; -extern Drive chassis; +/** + * Declare mechinism motors + */ +inline pros::MotorGroup lift({10, 11}); +inline pros::MotorGroup wrist({13, 14}); +inline pros::Motor arm({12}); -// Declares the left side motors -extern pros::Motor FrontLeft; -extern pros::Motor MiddleLeft; -extern pros::Motor BackLeft; +/** + * Declare pistons here + */ +inline ez::Piston claw({'A',true}); -// Declares the right side motors -extern pros::Motor FrontRight; -extern pros::Motor MiddleRight; -extern pros::Motor BackRight; +/** + * Declare sensors here + */ +inline pros::Rotation liftRotation({15}); +inline pros::Rotation armRotation({16}); +inline pros::Rotation wristRotatio({17}); -// Declares the left and right drive motor groups. -extern pros::MotorGroup RightSide; -extern pros::MotorGroup LeftSide; - -// Declares all the drive motors as a group. -extern pros::MotorGroup FullDrive; - -// Declaring the Intake motors. -inline pros::Motor stage1({14}); -inline pros::Motor stage2({16}); -inline pros::Motor stage3({12}); - -// -inline ez::Piston ramp({'H',true}); - -// Declaring the piston used to guide the balls into the basket. -inline ez::Piston deScorer({'G',false}); - -// Declaring the piston used to get the balls out of the tube. -inline ez::Piston matchLoader({'F',false}); - -// -inline pros::Optical colorSensorOne({20}); -inline pros::Optical colorSensorTwo({11}); -inline pros::Optical colorSensorThree({18}); - -// -inline pros::Distance frontDistance({5}); - -//inline pros::MotorGroup fillingGroup({20, 17, 18}); -//inline pros::MotorGroup scoringGroup({20, 17, 18, 19}); - -// Function used for the Intake control -void IntakePresets(); -void ToggleButtons(); - -// Function used for the color sorter -void ColorDisplay(); -void CheckRed(std::string Sensor, std::string controlPeriod, bool loop); -void CheckBlue(std::string Sensor, std::string controlPeriod, bool loop); -void ColorDetection(std::string wantedColor, bool checkForColor); - -// Auton voids -void leftMain(std::string side); -void rightMain(std::string side); -void Skills(); -void BlueRight(); -void BlueLeft(); -void RedRight(); -void RedLeft(); -void testMain(std::string side); -void Test(); +inline pros::Optical colorSensorOne({18}); +inline pros::Optical colorSensorTwo({19}); #endif \ No newline at end of file diff --git a/project.pros b/project.pros index f265380..e57942d 100644 --- a/project.pros +++ b/project.pros @@ -1,7 +1,7 @@ { "py/object": "pros.conductor.project.Project", "py/state": { - "project_name": "EZ-Template", + "project_name": "Over-Ride", "target": "v5", "templates": { "kernel": { @@ -400,9 +400,10 @@ }, "upload_options": { "compress_bin": true, - "description": "roboticsisez.com", + "description": "", "remote_name": "EZ-Template", - "slot": 1 + "slot": 1, + "icon": "pizza" }, "use_early_access": false } diff --git a/src/EZ-Template/color_sorter.cpp b/src/EZ-Template/color_sorter.cpp index 28d3aff..367a790 100644 --- a/src/EZ-Template/color_sorter.cpp +++ b/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; + } + + } + } } \ No newline at end of file diff --git a/src/EZ-Template/pneumatics.cpp b/src/EZ-Template/pneumatics.cpp new file mode 100644 index 0000000..259e7d8 --- /dev/null +++ b/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; + } + } +} \ No newline at end of file diff --git a/src/EZ-Template/rotation_mechanisms/arm_rotation.cpp b/src/EZ-Template/rotation_mechanisms/arm_rotation.cpp new file mode 100644 index 0000000..325a9de --- /dev/null +++ b/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 _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;} diff --git a/src/EZ-Template/rotation_mechanisms/lift_rotation.cpp b/src/EZ-Template/rotation_mechanisms/lift_rotation.cpp new file mode 100644 index 0000000..b30f1e5 --- /dev/null +++ b/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 _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;} \ 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 new file mode 100644 index 0000000..2d23e06 --- /dev/null +++ b/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 _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;} \ No newline at end of file diff --git a/src/main.cpp b/src/main.cpp index 0e49b50..d301b0d 100644 --- a/src/main.cpp +++ b/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 } }