From 3f29767125e762d850740c4cb238a0df4b9701aa Mon Sep 17 00:00:00 2001 From: Alex Date: Wed, 8 Dec 2021 20:04:23 -0700 Subject: [PATCH 1/6] Change drive motors to coast --- src/main.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index c5de4bf4..33706f5c 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -137,10 +137,10 @@ void initSensors() { */ void initMotors() { // Motor brake modes - frontLeftMotor.set_brake_mode(pros::motor_brake_mode_e::E_MOTOR_BRAKE_HOLD); - frontRightMotor.set_brake_mode(pros::motor_brake_mode_e::E_MOTOR_BRAKE_HOLD); - backLeftMotor.set_brake_mode(pros::motor_brake_mode_e::E_MOTOR_BRAKE_HOLD); - backRightMotor.set_brake_mode(pros::motor_brake_mode_e::E_MOTOR_BRAKE_HOLD); + frontLeftMotor.set_brake_mode(pros::motor_brake_mode_e::E_MOTOR_BRAKE_COAST); + frontRightMotor.set_brake_mode(pros::motor_brake_mode_e::E_MOTOR_BRAKE_COAST); + backLeftMotor.set_brake_mode(pros::motor_brake_mode_e::E_MOTOR_BRAKE_COAST); + backRightMotor.set_brake_mode(pros::motor_brake_mode_e::E_MOTOR_BRAKE_COAST); backGrabber.set_brake_mode(pros::motor_brake_mode_e::E_MOTOR_BRAKE_HOLD); leftLift.set_brake_mode(pros::motor_brake_mode_e::E_MOTOR_BRAKE_HOLD); rightLift.set_brake_mode(pros::motor_brake_mode_e::E_MOTOR_BRAKE_HOLD); From 58dc3244fd2f0802ee25f2d39c6a5e222de973ad Mon Sep 17 00:00:00 2001 From: Alex Date: Thu, 9 Dec 2021 11:25:07 -0700 Subject: [PATCH 2/6] Auton selector not working --- include/autonSelector.hpp | 5 +++-- include/defines.h | 2 +- src/autonSelector.cpp | 6 ------ src/main.cpp | 8 ++++---- 4 files changed, 8 insertions(+), 13 deletions(-) diff --git a/include/autonSelector.hpp b/include/autonSelector.hpp index 70b9fc0a..4d75b9bb 100644 --- a/include/autonSelector.hpp +++ b/include/autonSelector.hpp @@ -14,9 +14,10 @@ static const char *textMap[AUTON_SELECTOR_COUNT]; static bool guiFinnished; /** - * A graphical interface for selecting a auton. Uses LVGL, so make sure that is included. Also a singleton. + * A graphical interface for selecting a auton. Uses LVGL, so make sure that is included. */ -class autonSelector { +class autonSelector +{ private: // The function map, used for graphical and execution. diff --git a/include/defines.h b/include/defines.h index db78b56a..d0ef5737 100644 --- a/include/defines.h +++ b/include/defines.h @@ -1,4 +1,4 @@ #pragma once -#define AUTON_SELECTOR_COUNT 3 +#define AUTON_SELECTOR_COUNT 2 diff --git a/src/autonSelector.cpp b/src/autonSelector.cpp index bca32fba..f187282d 100644 --- a/src/autonSelector.cpp +++ b/src/autonSelector.cpp @@ -30,12 +30,6 @@ int autonSelector::choose() { this->draw(); this->addCallbacks(); - while (!guiFinnished) { - pros::Task::delay(20); - } - - this->del(); - // Return the selection. return userSelection; } diff --git a/src/main.cpp b/src/main.cpp index f3c0d23e..893e0f65 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -263,7 +263,7 @@ void initSelector() { autonomousSel->setSelection(0); // Start the task - // pros::Task selectorTask(runSelector, "Auton Selector"); + pros::Task selectorTask(runSelector, "Auton Selector"); } /** @@ -369,10 +369,10 @@ void initialize() { initSensors(); initMotors(); initDrivetrain(); + autoPaths(); initController(); - initSelector(); initLogger(); - autoPaths(); + initSelector(); } /** @@ -399,7 +399,7 @@ void disabled() { * Starts when connected to the field */ void competition_initialize() { - //autonomousSel->choose(); + // autonomousSel->choose(); } From fc5e4b6e32b4cd2c22f909c2bd1d3a98dca50fa0 Mon Sep 17 00:00:00 2001 From: Alex Date: Thu, 9 Dec 2021 14:43:44 -0700 Subject: [PATCH 3/6] Create controller gui --- include/auton/auton.hpp | 42 ++++++++++ include/auton/autonSelector.hpp | 75 +++++++++++++++++ include/autonSelector.hpp | 137 -------------------------------- include/main.h | 5 +- src/auton/auton.cpp | 24 ++++++ src/auton/autonSelector.cpp | 54 +++++++++++++ src/autonSelector.cpp | 61 -------------- src/driver/controller.cpp | 5 +- src/main.cpp | 32 +++----- 9 files changed, 212 insertions(+), 223 deletions(-) create mode 100644 include/auton/auton.hpp create mode 100644 include/auton/autonSelector.hpp delete mode 100644 include/autonSelector.hpp create mode 100644 src/auton/auton.cpp create mode 100644 src/auton/autonSelector.cpp delete mode 100644 src/autonSelector.cpp diff --git a/include/auton/auton.hpp b/include/auton/auton.hpp new file mode 100644 index 00000000..4bd7aa88 --- /dev/null +++ b/include/auton/auton.hpp @@ -0,0 +1,42 @@ +#pragma once + +#include + + +namespace Pronounce { + + typedef int (*AutonFunction)(); + + int nullAutonFunc(); + + class Auton { + private: + std::string name; + AutonFunction func; + public: + Auton(); + Auton(std::string name, AutonFunction func); + + int run(); + + std::string getName() { + return name; + } + + void setName(std::string name) { + this->name = name; + } + + AutonFunction getFunc() { + return func; + } + + void setFunc(AutonFunction func) { + this->func = func; + } + + ~Auton(); + }; +} // namespace Pronounce + + diff --git a/include/auton/autonSelector.hpp b/include/auton/autonSelector.hpp new file mode 100644 index 00000000..08fd7738 --- /dev/null +++ b/include/auton/autonSelector.hpp @@ -0,0 +1,75 @@ +#pragma once + +#include "api.h" +#include "auton.hpp" +#include "driver/controller.hpp" +#include + +namespace Pronounce { + class AutonSelector { + private: + std::vector autons; + Auton defaultAuton; + + Pronounce::Controller* controller; + + int autonIndex = 0; + public: + AutonSelector(); + AutonSelector(std::vector autons, Auton defaultAuton); + AutonSelector(std::vector autons, Auton defaultAuton, Pronounce::Controller* controller); + + void choose(); + + int addAuton(Auton auton) { + autons.push_back(auton); + return autons.size() - 1; + } + + int run() { + if (autonIndex >= autons.size()) { + return defaultAuton.run(); + } + return autons[autonIndex].run(); + } + + Auton getAuton(int index) { + if (index < 0 || index >= autons.size()) { + return defaultAuton; + } + return autons[index]; + } + + std::vector getAutons() { + return autons; + } + + void setAutons(std::vector autons) { + this->autons = autons; + } + + Auton getDefaultAuton() { + return defaultAuton; + } + + void setDefaultAuton(Auton defaultAuton) { + this->defaultAuton = defaultAuton; + } + + void setController(Pronounce::Controller* controller) { + this->controller = controller; + } + + int getAutonIndex() { + return autonIndex; + } + + void setAutonIndex(int autonIndex) { + this->autonIndex = autonIndex; + } + + ~AutonSelector(); + }; +} + + diff --git a/include/autonSelector.hpp b/include/autonSelector.hpp deleted file mode 100644 index 4d75b9bb..00000000 --- a/include/autonSelector.hpp +++ /dev/null @@ -1,137 +0,0 @@ -#pragma once - -#include "api.h" -#include "defines.h" - -typedef int (*autonFunction)(); - -int nullAutonFunc(); - -// Current User Selection, Static for the moment -// Because it needed a static function call -static int userSelection; -static const char *textMap[AUTON_SELECTOR_COUNT]; -static bool guiFinnished; - -/** - * A graphical interface for selecting a auton. Uses LVGL, so make sure that is included. - */ -class autonSelector -{ -private: - - // The function map, used for graphical and execution. - autonFunction functionMap[AUTON_SELECTOR_COUNT]; - autonFunction preRun = nullAutonFunc; - autonFunction postAuton = nullAutonFunc; - - - /* - LVGL Widgets - */ - - // Pointer to Button Grid - lv_obj_t* buttonMap; - - // How big the buttons are - lv_obj_t* buttonMapRegion; - - /** - * Delete all objects, clear up screen and memory - */ - void del(); - - void draw(); - - void addCallbacks(); - -public: - /** - * Create an auton selector without autons configured. - */ - autonSelector(); - - /** - * Create an auton selector without autons configured. - */ - autonSelector(char* textMap[]); - - /** - * Create an auton selector with all the auton functions already declared - * !
Not implemented yet - * - * @param textMap The map of different text. - */ - autonSelector(char* textMap[], lv_obj_t* buttonMapRegion); - - /** - * Run the user configuration with rendering. - * - * @return The selected auton - */ - int choose(); - - /** - * Run the user selection - */ - int runSelection(); - - /** - * Function to handle button presses - */ - void buttonPressed(int position) { - userSelection = position; - } - - /** - * Replace the current function at a coordinate. - * - * @param side The side the robot is on - * @param position The position the robot is on - * @param function The Function to replace it with - */ - void setFunction(int position, autonFunction function); - - void setPreRun(autonFunction function) { - this->preRun = function; - } - - void setPostAuton(autonFunction function) { - this->postAuton = function; - } - - /** - * Set the user selection - * - * @param side Side to change to - * @param position Position to change to - */ - void setSelection(int position) { - userSelection = position; - } - - /** - * Get the current user selection - * - * @return A struct containing both objects. - */ - int getSelection() { return userSelection; } - - /** - * Temporary static solution, changes the userSelection - * - */ - static lv_res_t pressHandler(lv_obj_t *btnm, const char *txt) { - - for (int i = 0; i < AUTON_SELECTOR_COUNT; i ++) { - if (strcmp(txt, textMap[i]) == 0) { - userSelection = i; - break; - } - } - - guiFinnished = true; - - return LV_RES_OK; - } -}; \ No newline at end of file diff --git a/include/main.h b/include/main.h index 8b56d86f..5a027de4 100644 --- a/include/main.h +++ b/include/main.h @@ -48,7 +48,10 @@ */ #include "defines.h" -#include "autonSelector.hpp" + +// Auton +#include "auton/auton.hpp" +#include "auton/autonSelector.hpp" // Chassis #include "chassis/drivetrain.hpp" diff --git a/src/auton/auton.cpp b/src/auton/auton.cpp new file mode 100644 index 00000000..7bb061a5 --- /dev/null +++ b/src/auton/auton.cpp @@ -0,0 +1,24 @@ +#include "auton.hpp" + +namespace Pronounce { + int nullAutonFunc() { + return 0; + } + + Auton::Auton(/* args */) { + this->name = ""; + this->func = nullAutonFunc; + } + + Auton::Auton(std::string name, AutonFunction func) { + this->name = name; + this->func = func; + } + + int Auton::run() { + return func(); + } + + Auton::~Auton() { + } +} // namespace Pronounce diff --git a/src/auton/autonSelector.cpp b/src/auton/autonSelector.cpp new file mode 100644 index 00000000..821bd181 --- /dev/null +++ b/src/auton/autonSelector.cpp @@ -0,0 +1,54 @@ +#include "autonSelector.hpp" + +namespace Pronounce { + AutonSelector::AutonSelector() { + controller = new Pronounce::Controller(pros::E_CONTROLLER_MASTER); + } + + AutonSelector::AutonSelector(std::vector autons, Auton defaultAuton) : AutonSelector() { + this->autons = autons; + this->defaultAuton = defaultAuton; + } + + AutonSelector::AutonSelector(std::vector autons, Auton defaultAuton, Pronounce::Controller* controller) { + this->autons = autons; + this->defaultAuton = defaultAuton; + this->controller = controller; + } + + void AutonSelector::choose() { + bool done = false; + + controller->setIsRendering(false); + + while (!done) { + if (controller->get_digital(pros::E_CONTROLLER_DIGITAL_A)) { + done = true; + } + + if (controller->get_digital_new_press(pros::E_CONTROLLER_DIGITAL_UP)) { + autonIndex++; + if (autonIndex >= autons.size()) { + autonIndex = 0; + } + } + + if (controller->get_digital_new_press(pros::E_CONTROLLER_DIGITAL_DOWN)) { + autonIndex--; + if (autonIndex < 0) { + autonIndex = autons.size() - 1; + } + } + + pros::lcd::print(0, "Auton: %s", autons[autonIndex].getName().c_str()); + + pros::Task::delay(20); + } + + controller->setIsRendering(true); + } + + AutonSelector::~AutonSelector() { + } +} // namespace Pronounce + diff --git a/src/autonSelector.cpp b/src/autonSelector.cpp deleted file mode 100644 index f187282d..00000000 --- a/src/autonSelector.cpp +++ /dev/null @@ -1,61 +0,0 @@ -#include "autonSelector.hpp" - -int nullAutonFunc() { - return 0; -} - -autonSelector::autonSelector() { - // Nothing -} - -autonSelector::autonSelector(char* inTextMap[AUTON_SELECTOR_COUNT]) { - for (int i = 0; i < AUTON_SELECTOR_COUNT; i ++) { - textMap[i] = inTextMap[i]; - } -} - -autonSelector::autonSelector(char* inTextMap[AUTON_SELECTOR_COUNT], lv_obj_t* buttonMapRegion) { - for (int i = 0; i < AUTON_SELECTOR_COUNT; i ++) { - textMap[i] = inTextMap[i]; - } - this->buttonMapRegion = buttonMapRegion; -} - -int autonSelector::choose() { - - // Wait until button is pressed. - guiFinnished = false; - - // Draw the items - this->draw(); - this->addCallbacks(); - - // Return the selection. - return userSelection; -} - -int autonSelector::runSelection() { - this->preRun(); - int result = this->functionMap[userSelection](); - this->postAuton(); - return result; -} - -void autonSelector::setFunction(int position, autonFunction function) { - this->functionMap[position] = function; -} - -void autonSelector::del() { - lv_obj_del(buttonMap); -} - -void autonSelector::draw() { - buttonMap = lv_btnm_create(buttonMapRegion, NULL); - lv_btnm_set_map(buttonMap, textMap); - lv_obj_set_size(buttonMap, lv_obj_get_width(buttonMapRegion), - lv_obj_get_height(buttonMapRegion)); -} - -void autonSelector::addCallbacks() { - lv_btnm_set_action(buttonMap, pressHandler); -} diff --git a/src/driver/controller.cpp b/src/driver/controller.cpp index 9fbc1399..d8d55c48 100644 --- a/src/driver/controller.cpp +++ b/src/driver/controller.cpp @@ -77,7 +77,10 @@ namespace Pronounce { } void Controller::renderFunc() { - while (this->continueRendering) { + while (true) { + if(!continueRendering) { + break; + } this->render(); // Prevent wasted resources diff --git a/src/main.cpp b/src/main.cpp index 893e0f65..a2cc4c32 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,7 +1,5 @@ #include "main.h" -// Auton Selector object -autonSelector* autonomousSel = nullptr; // Controllers Pronounce::Controller master(pros::E_CONTROLLER_MASTER); @@ -36,6 +34,9 @@ MecanumDrivetrain drivetrain(&frontLeftMotor, &frontRightMotor, &backLeftMotor, Pronounce::PurePursuit purePursuit(&drivetrain, 10); +// Autonomous Selector +Pronounce::AutonSelector autonomousSelector; + bool relativeMovement = false; bool driveOdomEnabled = true; @@ -238,31 +239,17 @@ void initController() { // Run selector as task void runSelector() { - autonomousSel->choose(); + autonomousSelector.choose(); } /** * Initialize the Auton Selector */ void initSelector() { - // Create a button descriptor string array w/ no repeat "\224" - static char* btnm_map[] = { (char*)"Test", (char*)"\n", - (char*)"Right steal right", - (char*)"" }; - - autonomousSel = new autonSelector(btnm_map, lv_scr_act()); - - // Set pre and post run - autonomousSel->setPreRun(preAutonRun); - autonomousSel->setPostAuton(postAuton); - - // Set functions - autonomousSel->setFunction(0, testAuton); - autonomousSel->setFunction(1, rightStealRight); - - autonomousSel->setSelection(0); + autonomousSelector.addAuton(Auton("Right steal right", rightStealRight)); + autonomousSelector.addAuton(Auton("Test", testAuton)); + autonomousSelector.setDefaultAuton(Auton("Right steal right", rightStealRight)); - // Start the task pros::Task selectorTask(runSelector, "Auton Selector"); } @@ -399,7 +386,7 @@ void disabled() { * Starts when connected to the field */ void competition_initialize() { - // autonomousSel->choose(); + // autonomousSelector.choose(); } @@ -408,8 +395,7 @@ void competition_initialize() { */ void autonomous() { // This calls the user selection, all the functions prototypes are in - // autonRoutines.hpp and the implementation is autonRoutines.cpp - autonomousSel->runSelection(); + // autonRoutines.hpp and the implementation is autonRoutines.cp } From 6166bbbe5925f05093c9b2ff792aa6a8ee9fe980 Mon Sep 17 00:00:00 2001 From: Alex Date: Fri, 10 Dec 2021 17:33:48 -0700 Subject: [PATCH 4/6] Right steal right --- include/auton/autonSelector.hpp | 28 +++++- include/driver/button.hpp | 10 +- include/driver/motorButton.hpp | 20 ++++ src/auton/autonSelector.cpp | 14 ++- src/driver/motorButton.cpp | 6 ++ src/main.cpp | 168 ++++++++++++++++---------------- 6 files changed, 159 insertions(+), 87 deletions(-) diff --git a/include/auton/autonSelector.hpp b/include/auton/autonSelector.hpp index 08fd7738..70de07eb 100644 --- a/include/auton/autonSelector.hpp +++ b/include/auton/autonSelector.hpp @@ -11,6 +11,9 @@ namespace Pronounce { std::vector autons; Auton defaultAuton; + Auton preAuton; + Auton postAuton; + Pronounce::Controller* controller; int autonIndex = 0; @@ -27,13 +30,18 @@ namespace Pronounce { } int run() { + preAuton.run(); + int result = 0; if (autonIndex >= autons.size()) { - return defaultAuton.run(); + result = defaultAuton.run(); } - return autons[autonIndex].run(); + result = autons[autonIndex].run(); + postAuton.run(); + return result; } Auton getAuton(int index) { + int result = 0; if (index < 0 || index >= autons.size()) { return defaultAuton; } @@ -56,6 +64,22 @@ namespace Pronounce { this->defaultAuton = defaultAuton; } + Auton getPreAuton() { + return preAuton; + } + + void setPreAuton(Auton preAuton) { + this->preAuton = preAuton; + } + + Auton getPostAuton() { + return postAuton; + } + + void setPostAuton(Auton postAuton) { + this->postAuton = postAuton; + } + void setController(Pronounce::Controller* controller) { this->controller = controller; } diff --git a/include/driver/button.hpp b/include/driver/button.hpp index ef444c39..f1b2728d 100644 --- a/include/driver/button.hpp +++ b/include/driver/button.hpp @@ -20,7 +20,7 @@ namespace Pronounce { pros::controller_digital_e_t positiveButton, negativeButton; bool singleToggle = false; - bool retainOnNeutral = true; + bool retainOnNeutral = false; public: Button(pros::Controller* cotroller); @@ -62,6 +62,14 @@ namespace Pronounce { this->retainOnNeutral = retainOnNeutral; } + bool getAutonomous() { + return this->autonomous; + } + + void setAutonomous(bool autonomous) { + this->autonomous = autonomous; + } + ~Button(); }; } // namespace Pronounce diff --git a/include/driver/motorButton.hpp b/include/driver/motorButton.hpp index 970de3aa..47204b41 100644 --- a/include/driver/motorButton.hpp +++ b/include/driver/motorButton.hpp @@ -10,10 +10,14 @@ namespace Pronounce { private: bool goToImmediately = false; + bool autonomousButton = false; + int positiveAuthority = 0; int neutralAuthority = 0; int negativeAuthority = 0; + int autonomousAuthority = 0; + int min = 0; int max = 0; pros::Motor* motor; @@ -39,6 +43,22 @@ namespace Pronounce { this->goToImmediately = goToImmediately; } + int getAutonomousAuthority() { + return this->autonomousAuthority; + } + + void setAutonomousAuthority(int autonomousAuthority) { + this->autonomousAuthority = autonomousAuthority; + } + + bool getAutonomousButton() { + return this->autonomousButton; + } + + void setAutonomousButton(bool autonomousButton) { + this->autonomousButton = autonomousButton; + } + ~MotorButton(); }; diff --git a/src/auton/autonSelector.cpp b/src/auton/autonSelector.cpp index 821bd181..5105478f 100644 --- a/src/auton/autonSelector.cpp +++ b/src/auton/autonSelector.cpp @@ -3,6 +3,9 @@ namespace Pronounce { AutonSelector::AutonSelector() { controller = new Pronounce::Controller(pros::E_CONTROLLER_MASTER); + preAuton = Auton(); + postAuton = Auton(); + defaultAuton = Auton(); } AutonSelector::AutonSelector(std::vector autons, Auton defaultAuton) : AutonSelector() { @@ -21,7 +24,11 @@ namespace Pronounce { controller->setIsRendering(false); + printf("Auton selector\n"); + while (!done) { + controller->clear(); + if (controller->get_digital(pros::E_CONTROLLER_DIGITAL_A)) { done = true; } @@ -40,11 +47,14 @@ namespace Pronounce { } } - pros::lcd::print(0, "Auton: %s", autons[autonIndex].getName().c_str()); + controller->set_text(0, 0, autons[autonIndex].getName()); + + pros::Task::delay(50); - pros::Task::delay(20); } + printf("Auton selected: %s", autons[autonIndex].getName().c_str()); + controller->setIsRendering(true); } diff --git a/src/driver/motorButton.cpp b/src/driver/motorButton.cpp index 3631634c..c6bb8ff2 100644 --- a/src/driver/motorButton.cpp +++ b/src/driver/motorButton.cpp @@ -25,6 +25,12 @@ namespace Pronounce { return; } + if (this->getAutonomous() && !this->autonomousButton) { + this->motor->move_absolute(this->autonomousAuthority, abs(this->positiveAuthority)); + printf("MotorButton: autonomous authority: %d\n", this->autonomousAuthority); + return; + } + switch (this->getButtonStatus()) { case NEGATIVE: if (goToImmediately) { diff --git a/src/main.cpp b/src/main.cpp index a2cc4c32..fe6893d9 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -28,12 +28,19 @@ Pronounce::MotorOdom wheel2(&frontRightMotor, 2); Pronounce::MotorOdom wheel3(&backLeftMotor, 2); Pronounce::MotorOdom wheel4(&backRightMotor, 2); -Pronounce::MecanumOdometry odometry(&wheel1, &wheel2, &wheel3, &wheel4, &imu, 14/2, 10.5/2); +Pronounce::MecanumOdometry odometry(&wheel1, &wheel2, &wheel3, &wheel4, &imu, 14 / 2, 10.5 / 2); MecanumDrivetrain drivetrain(&frontLeftMotor, &frontRightMotor, &backLeftMotor, &backRightMotor, &imu, &odometry); Pronounce::PurePursuit purePursuit(&drivetrain, 10); + +MotorButton leftLiftButton(&master, &leftLift, DIGITAL_L1, DIGITAL_L2, 200, 0, -200, 0, 0); +MotorButton rightLiftButton(&master, &rightLift, DIGITAL_L1, DIGITAL_L2, 200, 0, -200, 0, 0); +MotorButton backGrabberButton(&master, &backGrabber, DIGITAL_R1, DIGITAL_R2, 200, 200, 200, 0, 450 * 3); + +SolenoidButton frontGrabberButton(&master, DIGITAL_A, DIGITAL_B); + // Autonomous Selector Pronounce::AutonSelector autonomousSelector; @@ -50,8 +57,8 @@ int testPathIndex; int rightHomeToGoalNeutralIndex; int rightNeutralToMidNeutralIndex; int midNeutralToRightAllianceIndex; -int rightAllianceToRightRingIndex; -int rightRingToLeftHomeZoneIndex; +int midNeutralToMidHomeZoneIndex; +int rightNeutralToRightHomeIndex; /** * @brief Runs during auton period before auton @@ -65,6 +72,13 @@ int preAutonRun() { purePursuit.setEnabled(true); + frontGrabberButton.setAutonomous(true); + backGrabberButton.setAutonomous(true); + leftLiftButton.setAutonomous(true); + rightLiftButton.setAutonomous(true); + + backGrabberButton.setAutonomousButton(true); + return 0; } @@ -73,68 +87,42 @@ int preAutonRun() { * */ int rightStealRight() { - odometry.reset(new Position(105.7, 8)); + odometry.reset(new Position(105.7, 16)); + + backGrabberButton.setButtonStatus(ButtonStatus::POSITIVE); + frontGrabberButton.setButtonStatus(ButtonStatus::NEUTRAL); purePursuit.setCurrentPathIndex(rightHomeToGoalNeutralIndex); purePursuit.setFollowing(true); // Wait until it is done - while (purePursuit.isDone(0.5)) { + while (!purePursuit.isDone(0.5)) { pros::Task::delay(50); } // Collect front goal - frontGrabber.set_value(true); + frontGrabberButton.setButtonStatus(ButtonStatus::POSITIVE); + leftLiftButton.setAutonomousAuthority(360); + rightLiftButton.setAutonomousAuthority(360); purePursuit.setCurrentPathIndex(rightNeutralToMidNeutralIndex); purePursuit.setFollowing(true); purePursuit.setTurnTarget(3.14); - while (odometry.getPosition()->getY() < 46.8) { - pros::Task::delay(50); - } - - // Let go of front goal and get ready to collect again - frontGrabber.set_value(false); - purePursuit.setTurnTarget(0); - - // Wait until it is done - while (purePursuit.isDone(0.5)) { - pros::Task::delay(50); - } - - // Collect front goal - frontGrabber.set_value(true); - - purePursuit.setCurrentPathIndex(midNeutralToRightAllianceIndex); - purePursuit.setFollowing(true); - purePursuit.setTurnTarget(2.355); - // Wait until it is done - while (purePursuit.isDone(0.5)) { + while (!purePursuit.isDone(0.5)) { pros::Task::delay(50); } - backGrabber.move_velocity(200); - - purePursuit.setTurnTarget(0); + backGrabberButton.setButtonStatus(ButtonStatus::NEUTRAL); pros::Task::delay(500); - purePursuit.setCurrentPathIndex(rightAllianceToRightRingIndex); - purePursuit.setFollowing(true); - purePursuit.setTurnTarget(0); - - // Wait until it is done - while (purePursuit.isDone(0.5)) { - pros::Task::delay(50); - } - - purePursuit.setCurrentPathIndex(rightRingToLeftHomeZoneIndex); + purePursuit.setCurrentPathIndex(midNeutralToMidHomeZoneIndex); purePursuit.setFollowing(true); purePursuit.setTurnTarget(-M_PI_2); // Wait until it is done - while (purePursuit.isDone(0.5)) { + while (!purePursuit.isDone(0.5)) { pros::Task::delay(50); } @@ -147,15 +135,15 @@ int rightStealRight() { * @return 0 */ int testAuton() { + + printf("Test Auton\n"); + odometry.reset(new Position()); purePursuit.setCurrentPathIndex(testPathIndex); purePursuit.setFollowing(true); - // Wait until it is done - while (purePursuit.isDone(0.5)) { - pros::Task::delay(50); - } + pros::Task::delay(10000); return 0; } @@ -163,6 +151,11 @@ int testAuton() { int postAuton() { purePursuit.setFollowing(false); purePursuit.setEnabled(false); + frontGrabberButton.setAutonomous(false); + backGrabberButton.setAutonomous(false); + leftLiftButton.setAutonomous(false); + rightLiftButton.setAutonomous(false); + return 0; } @@ -199,6 +192,17 @@ void initSensors() { } } +void updateMotors() { + while (1) { + frontGrabberButton.update(); + backGrabberButton.update(); + leftLiftButton.update(); + rightLiftButton.update(); + + pros::Task::delay(20); + } +} + /** * Initialize all motors */ @@ -211,6 +215,8 @@ void initMotors() { backGrabber.set_brake_mode(pros::motor_brake_mode_e::E_MOTOR_BRAKE_HOLD); leftLift.set_brake_mode(pros::motor_brake_mode_e::E_MOTOR_BRAKE_HOLD); rightLift.set_brake_mode(pros::motor_brake_mode_e::E_MOTOR_BRAKE_HOLD); + + pros::Task updateButtons(updateMotors, "Update buttons"); } void initDrivetrain() { @@ -249,8 +255,11 @@ void initSelector() { autonomousSelector.addAuton(Auton("Right steal right", rightStealRight)); autonomousSelector.addAuton(Auton("Test", testAuton)); autonomousSelector.setDefaultAuton(Auton("Right steal right", rightStealRight)); + autonomousSelector.setPreAuton(Auton("Pre auton", preAutonRun)); + autonomousSelector.setPreAuton(Auton("Post auton", postAuton)); pros::Task selectorTask(runSelector, "Auton Selector"); + } /** @@ -269,7 +278,7 @@ void initLogger() { void autoPaths() { // Default pure pursuit profile - PurePursuitProfile defaultProfile(new PID(20, 0.0, 0.0), new PID(30, 0.0, 0.0), 10.0); + PurePursuitProfile defaultProfile(new PID(20, 0.0, -1.0), new PID(100, 0.0, 40.0), 10.0); purePursuit.getPurePursuitProfileManager().setDefaultProfile(defaultProfile); // Test path @@ -284,45 +293,43 @@ void autoPaths() { testPathIndex = purePursuit.addPath(testPath); + Path rightNeutralToRightHomeZone; + + rightNeutralToRightHomeZone.addPoint(105.7, 61); + rightNeutralToRightHomeZone.addPoint(105.7, 16); + + rightNeutralToRightHomeIndex = purePursuit.addPath(rightNeutralToRightHomeZone); + // Right Steal Right Path rightHomeToGoalNeutral; - rightHomeToGoalNeutral.addPoint(105.7, 8); - rightHomeToGoalNeutral.addPoint(105.7, 60); + rightHomeToGoalNeutral.addPoint(105.7, 16); + rightHomeToGoalNeutral.addPoint(105.7, 62); rightHomeToGoalNeutralIndex = purePursuit.addPath(rightHomeToGoalNeutral); Path rightNeutralToMidNeutral; - rightNeutralToMidNeutral.addPoint(105.7, 60); - rightNeutralToMidNeutral.addPoint(82.3, 40); - rightNeutralToMidNeutral.addPoint(70.3, 60); + rightNeutralToMidNeutral.addPoint(105.7, 62); + rightNeutralToMidNeutral.addPoint(75.3, 40); + rightNeutralToMidNeutral.addPoint(60.3, 65); rightNeutralToMidNeutralIndex = purePursuit.addPath(rightNeutralToMidNeutral); Path midNeutralToRightAlliance; - midNeutralToRightAlliance.addPoint(70.3, 60); + midNeutralToRightAlliance.addPoint(70.3, 65); midNeutralToRightAlliance.addPoint(120.1, 36); midNeutralToRightAllianceIndex = purePursuit.addPath(midNeutralToRightAlliance); - Path rightAllianceToRightRing; - - rightAllianceToRightRing.addPoint(120.1, 36); - rightAllianceToRightRing.addPoint(117.5, 46.8); - rightAllianceToRightRing.addPoint(117.5, 70.3); - rightAllianceToRightRing.addPoint(117.5, 70.3); - - rightAllianceToRightRingIndex = purePursuit.addPath(rightAllianceToRightRing); + Path midNeutralToMidHomeZone; - Path rightRingToLeftHomeZone; + midNeutralToMidHomeZone.addPoint(70.3, 70.3); + midNeutralToMidHomeZone.addPoint(70.3, 35); - rightRingToLeftHomeZone.addPoint(117.5, 70.3); - rightRingToLeftHomeZone.addPoint(105, 35); - rightRingToLeftHomeZone.addPoint(35, 35); + midNeutralToMidHomeZoneIndex = purePursuit.addPath(midNeutralToMidHomeZone); - rightRingToLeftHomeZoneIndex = purePursuit.addPath(rightRingToLeftHomeZone); } /** @@ -359,7 +366,14 @@ void initialize() { autoPaths(); initController(); initLogger(); - initSelector(); + // initSelector(); + + + backGrabberButton.setSingleToggle(true); + backGrabberButton.setGoToImmediately(true); + + frontGrabberButton.setSolenoid(&frontGrabber); + frontGrabberButton.setSingleToggle(true); } /** @@ -396,6 +410,10 @@ void competition_initialize() { void autonomous() { // This calls the user selection, all the functions prototypes are in // autonRoutines.hpp and the implementation is autonRoutines.cp + // autonomousSelector.run(); + preAutonRun(); + rightStealRight(); + postAuton(); } @@ -407,6 +425,8 @@ void opcontrol() { printf("OpControl"); lv_obj_clean(lv_scr_act()); + postAuton(); + //lv_obj_t* infoLabel = lv_label_create(lv_scr_act(), NULL); // lv_label_set_text(infoLabel, ""); @@ -415,16 +435,6 @@ void opcontrol() { RunningAverage leftYAvg; RunningAverage rightXAvg; - MotorButton leftLiftButton(&master, &leftLift, DIGITAL_L1, DIGITAL_L2, 127, 0, -127, 0, 0); - MotorButton rightLiftButton(&master, &rightLift, DIGITAL_L1, DIGITAL_L2, 127, 0, -127, 0, 0); - MotorButton backGrabberButton(&master, &backGrabber, DIGITAL_R1, DIGITAL_R2, 200, 200, 200, 0, 450*3); - backGrabberButton.setSingleToggle(true); - backGrabberButton.setGoToImmediately(true); - - SolenoidButton frontGrabberButton(&master, DIGITAL_A, DIGITAL_B); - frontGrabberButton.setSolenoid(&frontGrabber); - frontGrabberButton.setSingleToggle(true); - // Driver Control Loop while (true) { @@ -451,16 +461,10 @@ void opcontrol() { frontGrabberButton.setButtonStatus(Pronounce::ButtonStatus::POSITIVE); } - leftLiftButton.update(); - rightLiftButton.update(); - frontGrabberButton.update(); - backGrabberButton.update(); if (master.get_digital_new_press(DIGITAL_X)) { odometry.reset(new Position()); } - odometry.update(); - // Prevent wasted resources pros::delay(10); } From 84f7b2e0c28f7cb89adb60feabf3ea55497b0908 Mon Sep 17 00:00:00 2001 From: Alex Date: Fri, 10 Dec 2021 18:52:08 -0700 Subject: [PATCH 5/6] Right awp right --- src/main.cpp | 42 ++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 42 insertions(+) diff --git a/src/main.cpp b/src/main.cpp index fe6893d9..b820f52e 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -59,6 +59,8 @@ int rightNeutralToMidNeutralIndex; int midNeutralToRightAllianceIndex; int midNeutralToMidHomeZoneIndex; int rightNeutralToRightHomeIndex; +int farRightHomeZoneToRightAllianceIndex; +int rightAllianceToRightHomeZoneIndex; /** * @brief Runs during auton period before auton @@ -102,6 +104,7 @@ int rightStealRight() { // Collect front goal frontGrabberButton.setButtonStatus(ButtonStatus::POSITIVE); + pros::Task::delay(200); leftLiftButton.setAutonomousAuthority(360); rightLiftButton.setAutonomousAuthority(360); @@ -129,6 +132,32 @@ int rightStealRight() { return 0; } +int rightAwpRight() { + odometry.reset(new Position(129.9, 16)); + + purePursuit.setCurrentPathIndex(farRightHomeZoneToRightAllianceIndex); + purePursuit.setFollowing(true); + + // Wait until it is done + while (!purePursuit.isDone(0.5)) { + pros::Task::delay(50); + } + + // Collect front goal + frontGrabberButton.setButtonStatus(ButtonStatus::POSITIVE); + pros::Task::delay(200); + leftLiftButton.setAutonomousAuthority(360); + rightLiftButton.setAutonomousAuthority(360); + + purePursuit.setCurrentPathIndex(rightAllianceToRightHomeZoneIndex); + purePursuit.setFollowing(true); + + // Wait until it is done + while (!purePursuit.isDone(0.5)) { + pros::Task::delay(50); + } +} + /** * @brief Test auton * @@ -330,6 +359,19 @@ void autoPaths() { midNeutralToMidHomeZoneIndex = purePursuit.addPath(midNeutralToMidHomeZone); + Path farRightHomeZoneToRightAlliance; + + farRightHomeZoneToRightAlliance.addPoint(127.9, 16); + farRightHomeZoneToRightAlliance.addPoint(127.9, 24); + + farRightHomeZoneToRightAllianceIndex = purePursuit.addPath(farRightHomeZoneToRightAlliance); + + Path rightAllianceToRightHomeZone; + + rightAllianceToRightHomeZone.addPoint(127.9, 24); + rightAllianceToRightHomeZone.addPoint(105.7, 16); + + rightAllianceToRightHomeZoneIndex = purePursuit.addPath(rightAllianceToRightHomeZone); } /** From 9861eda2df9342bd0564b6fb32a1869a8c325c50 Mon Sep 17 00:00:00 2001 From: Alex Date: Fri, 10 Dec 2021 19:02:52 -0700 Subject: [PATCH 6/6] Working, may add on later --- src/main.cpp | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/src/main.cpp b/src/main.cpp index b820f52e..80ba0caf 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -142,7 +142,7 @@ int rightAwpRight() { while (!purePursuit.isDone(0.5)) { pros::Task::delay(50); } - + // Collect front goal frontGrabberButton.setButtonStatus(ButtonStatus::POSITIVE); pros::Task::delay(200); @@ -156,6 +156,17 @@ int rightAwpRight() { while (!purePursuit.isDone(0.5)) { pros::Task::delay(50); } + + return 0; +} + +int leftAwpLeft() { + // Collect front goal + frontGrabberButton.setButtonStatus(ButtonStatus::POSITIVE); + pros::Task::delay(1000); + frontGrabberButton.setButtonStatus(ButtonStatus::NEUTRAL); + + return 0; } /** @@ -180,7 +191,7 @@ int testAuton() { int postAuton() { purePursuit.setFollowing(false); purePursuit.setEnabled(false); - frontGrabberButton.setAutonomous(false); + frontGrabberButton.setAutonomous(false); backGrabberButton.setAutonomous(false); leftLiftButton.setAutonomous(false); rightLiftButton.setAutonomous(false); @@ -454,7 +465,7 @@ void autonomous() { // autonRoutines.hpp and the implementation is autonRoutines.cp // autonomousSelector.run(); preAutonRun(); - rightStealRight(); + leftAwpLeft(); postAuton(); }