diff --git a/.devcontainer/devcontainer.json b/.devcontainer/devcontainer.json index ab87be9..52968ab 100644 --- a/.devcontainer/devcontainer.json +++ b/.devcontainer/devcontainer.json @@ -1,3 +1,3 @@ { - "postCreateCommand": "chmod 777 .devcontainer/postCreateCommand.bash && .devcontainer/postCreateCommand.bash" -} \ No newline at end of file + "postCreateCommand": "chmod 777 .devcontainer/postCreateCommand.bash && .devcontainer/postCreateCommand.bash && source ~/.bashrc" +} diff --git a/.devcontainer/postCreateCommand.bash b/.devcontainer/postCreateCommand.bash index 0499fec..b0dbb37 100755 --- a/.devcontainer/postCreateCommand.bash +++ b/.devcontainer/postCreateCommand.bash @@ -6,4 +6,3 @@ curl https://armkeil.blob.core.windows.net/developer/Files/downloads/gnu-rm/10.3 tar -xjvf gcc-arm-none-eabi-10.3-2021.10-x86_64-linux.tar.bz2 echo "PATH=$PATH:/home/codespace/gcc-arm-none-eabi-10.3-2021.10/bin" >> ~/.bashrc popd -source ~/.bashrc \ No newline at end of file diff --git a/.github/workflows/clang-format.yml b/.github/workflows/clang-format.yml new file mode 100644 index 0000000..29121bb --- /dev/null +++ b/.github/workflows/clang-format.yml @@ -0,0 +1,21 @@ +name: test-clang-format + +on: + push: + branches: + - '*' + pull_request: + workflow_dispatch: + + +jobs: + build: + runs-on: ubuntu-latest + + steps: + - uses: actions/checkout@v2 + - uses: DoozyX/clang-format-lint-action@v0.18 + with: + source: './src/gamepad ./include/gamepad' + extensions: 'hpp,cpp' + clangFormatVersion: 18 diff --git a/.github/workflows/pr-comment.yml b/.github/workflows/pr-comment.yml index f579cdd..f4d610b 100644 --- a/.github/workflows/pr-comment.yml +++ b/.github/workflows/pr-comment.yml @@ -26,7 +26,7 @@ jobs: const old_marker = new RegExp(marker.replace("\r\n", "\r?\n")).exec(old_body)?.[0] ?? marker; - body = old_body.split(old_marker)[0] + marker + body; + body = (old_body ?? "").split(old_marker)[0] + marker + body; await github.request('PATCH /repos/{owner}/{repo}/pulls/{pull_number}', { owner: owner, repo: repo, diff --git a/.github/workflows/pros-build.yml b/.github/workflows/pros-build.yml index abb219b..88566b5 100644 --- a/.github/workflows/pros-build.yml +++ b/.github/workflows/pros-build.yml @@ -13,6 +13,6 @@ jobs: steps: - uses: actions/checkout@v4 - - uses: LemLib/pros-build@v1.0.0 + - uses: LemLib/pros-build@v2.0.2 with: library-path: gamepad diff --git a/include/gamepad/api.hpp b/include/gamepad/api.hpp new file mode 100644 index 0000000..835385a --- /dev/null +++ b/include/gamepad/api.hpp @@ -0,0 +1,4 @@ +#pragma once + +#include "gamepad/event_handler.hpp" // IWYU pragma: export +#include "gamepad/controller.hpp" // IWYU pragma: export \ No newline at end of file diff --git a/include/gamepad/controller.hpp b/include/gamepad/controller.hpp new file mode 100644 index 0000000..df78d52 --- /dev/null +++ b/include/gamepad/controller.hpp @@ -0,0 +1,274 @@ +#pragma once + +#include "pros/misc.h" +#include +#include +#include +#ifndef PROS_USE_SIMPLE_NAMES +#define PROS_USE_SIMPLE_NAMES +#endif + +#include "event_handler.hpp" +#include "pros/misc.hpp" +#include "pros/rtos.hpp" + +namespace Gamepad { + +enum EventType { + ON_PRESS, + ON_LONG_PRESS, + ON_RELEASE, + ON_SHORT_RELEASE, +}; + +class Button { + friend class Controller; + public: + /// Whether the button has just been pressed + bool rising_edge = false; + /// Whether the button has just been released + bool falling_edge = false; + /// Whether the button is currently held down + bool is_pressed = false; + /// How long the button has been held down + uint32_t time_held = 0; + /// How long the button has been released + uint32_t time_released = 0; + /// How long the threshold should be for the longPress and shortRelease events + uint32_t long_press_threshold = 500; + /** + * @brief Register a function to run when the button is pressed. + * + * @param listenerName The name of the listener, this must be a unique name + * @param func The function to run when the button is pressed, the function MUST NOT block + * @return true The listener was successfully registered + * @return false The listener was not successfully registered (there is already a listener with this name) + * + * @b Example: + * @code {.cpp} + * // Use a function... + * Gamepad::master.Down.onPress("downPress1", downPress1); + * // ...or a lambda + * Gamepad::master.Up.onPress("upPress1", []() { std::cout << "I was pressed!" << std::endl; }); + * @endcode + */ + bool onPress(std::string listenerName, std::function func) const; + /** + * @brief Register a function to run when the button is long pressed. + * + * By default, onLongPress will fire when the button has been held down for + * 500ms or more, this threshold can be adjusted by changing long_press_threshold. + * + * @warning When using this event along with onPress, both the onPress + * and onlongPress listeners may fire together. + * + * @param listenerName The name of the listener, this must be a unique name + * @param func The function to run when the button is long pressed, the function MUST NOT block + * @return true The listener was successfully registered + * @return false The listener was not successfully registered (there is already a listener with this name) + * + * @b Example: + * @code {.cpp} + * // Use a function... + * Gamepad::master.Left.onLongPress("fireCatapult", fireCatapult); + * // ...or a lambda + * Gamepad::master.Right.onLongPress("print_right", []() { std::cout << "Right button was long pressed!" << + * std::endl; }); + * @endcode + */ + bool onLongPress(std::string listenerName, std::function func) const; + /** + * @brief Register a function to run when the button is released. + * + * @param listenerName The name of the listener, this must be a unique name + * @param func The function to run when the button is released, the function MUST NOT block + * @return true The listener was successfully registered + * @return false The listener was not successfully registered (there is already a listener with this name) + * + * @b Example: + * @code {.cpp} + * // Use a function... + * Gamepad::master.X.onRelease("stopFlywheel", stopFlywheel); + * // ...or a lambda + * Gamepad::master.Y.onRelease("stopIntake", []() { intake.move(0); }); + * @endcode + */ + bool onRelease(std::string listenerName, std::function func) const; + /** + * @brief Register a function to run when the button is short released. + * + * By default, shortRelease will fire when the button has been released before 500ms, this threshold can be + * adjusted by changing long_press_threshold. + * + * @note This event will most likely be used along with the longPress event. + * + * @param listenerName The name of the listener, this must be a unique name + * @param func The function to run when the button is short released, the function MUST NOT block + * @return true The listener was successfully registered + * @return false The listener was not successfully registered (there is already a listener with this name) + * + * @b Example: + * @code {.cpp} + * // Use a function... + * Gamepad::master.A.onShortRelease("raiseLiftOneLevel", raiseLiftOneLevel); + * // ...or a lambda + * Gamepad::master.B.onShortRelease("intakeOnePicce", []() { intake.move_relative(600, 100); }); + * @endcode + */ + bool onShortRelease(std::string listenerName, std::function func) const; + /** + * @brief Register a function to run for a given event. + * + * @param event Which event to register the listener on. + * @param listenerName The name of the listener, this must be a unique name + * @param func The function to run for the given event, the function MUST NOT block + * @return true The listener was successfully registered + * @return false The listener was not successfully registered (there is already a listener with this name) + * + * @b Example: + * @code {.cpp} + * // Use a function... + * Gamepad::master.L1.addListener(Gamepad::ON_PRESS, "start_spin", startSpin); + * // ...or a lambda + * Gamepad::master.L1.addListener(Gamepad::ON_RELEASE, "stop_spin", []() { motor1.brake(); }); + * @endcode + */ + bool addListener(EventType event, std::string listenerName, std::function func) const; + /** + * @brief Removes a listener from the button + * @warning Usage of this function is discouraged. + * + * @param listenerName The name of the listener to remove + * @return true The specified listener was successfully removed + * @return false The specified listener could not be removed + * + * @b Example: + * @code {.cpp} + * // Add an event listener... + * Gamepad::master.L1.addListener(Gamepad::ON_PRESS, "do_something", doSomething); + * // ...and now get rid of it + * Gamepad::master.L1.removeListener("do_something"); + * @endcode + */ + bool removeListener(std::string listenerName) const; + + /** + * @brief Returns a value indicating whether the button is currently being held. + * + * @return true The button is currently pressed + * @return false The button is not currently pressed + */ + explicit operator bool() const { return is_pressed; } + private: + /** + * @brief Updates the button and runs any event handlers, if necessary + * + * @param is_held Whether or not the button is currently held down + */ + void update(bool is_held); + /// he last time the update function was called + uint32_t last_update_time = pros::millis(); + /// The last time the long press event was fired + uint32_t last_long_press_time = 0; + mutable _impl::EventHandler onPressEvent {}; + mutable _impl::EventHandler onLongPressEvent {}; + mutable _impl::EventHandler onReleaseEvent {}; + mutable _impl::EventHandler onShortReleaseEvent {}; +}; + +class Controller { + public: + /** + * @brief Updates the state of the gamepad (all joysticks and buttons), and also runs + * any registered listeners. + * + * @note This function should be called at the beginning of every loop iteration. + * + * @b Example: + * @code {.cpp} + * while (true) { + * Gamepad::master.update(); + * // do robot control stuff here... + * pros::delay(25); + * } + * @endcode + * + */ + void update(); + /** + * @brief Get the state of a button on the controller. + * + * @param button Which button to return + * + * @b Example: + * @code {.cpp} + * if(Gamepad::master[DIGITAL_L1]) { + * // do something here... + * } + * @endcode + * + */ + const Button& operator[](pros::controller_digital_e_t button); + /** + * @brief Get the value of a joystick axis on the controller. + * + * @param joystick Which joystick axis to return + * + * @b Example: + * @code {.cpp} + * // control a motor with a joystick + * intake.move(Gamepad::master[ANALOG_RIGHT_Y]); + * @endcode + * + */ + float operator[](pros::controller_analog_e_t joystick); + const Button& L1 {m_L1}; + const Button& L2 {m_L2}; + const Button& R1 {m_R1}; + const Button& R2 {m_R2}; + const Button& Up {m_Up}; + const Button& Down {m_Down}; + const Button& Left {m_Left}; + const Button& Right {m_Right}; + const Button& X {m_X}; + const Button& B {m_B}; + const Button& Y {m_Y}; + const Button& A {m_A}; + const float& LeftX = m_LeftX; + const float& LeftY = m_LeftY; + const float& RightX = m_RightX; + const float& RightY = m_RightY; + /// The master controller, same as @ref Gamepad::master + static Controller master; + /// The partner controller, same as @ref Gamepad::partner + static Controller partner; + private: + Controller(pros::controller_id_e_t id) + : controller(id) {} + + Button m_L1 {}, m_L2 {}, m_R1 {}, m_R2 {}, m_Up {}, m_Down {}, m_Left {}, m_Right {}, m_X {}, m_B {}, m_Y {}, + m_A {}; + float m_LeftX = 0, m_LeftY = 0, m_RightX = 0, m_RightY = 0; + Button Fake {}; + /** + * @brief Gets a unique name for a listener that will not conflict with user listener names. + * + * @important: when using the function, you must register the listener by + * directly calling add_listener on the EventHandler, do NOT use onPress/addListener,etc. + * + * @return std::string A unique listener name + */ + static std::string unique_name(); + static Button Controller::*button_to_ptr(pros::controller_digital_e_t button); + void updateButton(pros::controller_digital_e_t button_id); + pros::Controller controller; +}; + +inline Controller Controller::master {pros::E_CONTROLLER_MASTER}; +inline Controller Controller::partner {pros::E_CONTROLLER_PARTNER}; +/// The master controller +inline Controller& master = Controller::master; +/// The partner controller +inline Controller& partner = Controller::partner; + +} // namespace Gamepad diff --git a/include/gamepad/event_handler.hpp b/include/gamepad/event_handler.hpp new file mode 100644 index 0000000..76ab6cc --- /dev/null +++ b/include/gamepad/event_handler.hpp @@ -0,0 +1,81 @@ +#pragma once + +#include +#include +#include +#include + +#include "gamepad/recursive_mutex.hpp" + +namespace Gamepad::_impl { + +/** + * @brief Event handling class with thread safety that supports adding, removing, and running listeners + * + * @tparam Key the key type for (un)registering listener (this type MUST support operator== and operator!=) + * @tparam Args the types of the parameters that each listener is passed + */ +template class EventHandler { + public: + using Listener = std::function; + + /** + * @brief Add a listener to the list of listeners + * + * @param key The listener key (this must be a unique key value) + * @param func The function to run when this event is fired + * @return true The listener was successfully added + * @return false The listener was NOT successfully added (there is already a listener with the same key) + */ + bool add_listener(Key key, Listener func) { + std::lock_guard lock(mutex); + if (std::find(keys.begin(), keys.end(), key) != keys.end()) return false; + keys.push_back(key); + listeners.push_back(func); + return true; + } + + /** + * @brief Remove a listener from the list of listeners + * + * @param key The listener key (this must be a unique key value) + * @return true The listener was successfully removed + * @return false The listener was NOT successfully removed (there is no listener with the same key) + */ + bool remove_listener(Key key) { + std::lock_guard lock(mutex); + auto i = std::find(keys.begin(), keys.end(), key); + if (i != keys.end()) { + keys.erase(i); + listeners.erase(listeners.begin() + (i - keys.begin())); + return true; + } + return false; + } + + /** + * @brief Whther or not there are any listeners registered + * + * @return true There are listeners registered + * @return false There are no listeners registered + */ + bool is_empty() { + std::lock_guard lock(mutex); + return listeners.empty(); + } + + /** + * @brief Runs each listener registered + * + * @param args The parameters to pass to each listener + */ + void fire(Args... args) { + std::lock_guard lock(mutex); + for (auto listener : listeners) { listener(args...); } + } + private: + std::vector keys {}; + std::vector listeners {}; + Gamepad::_impl::RecursiveMutex mutex {}; +}; +} // namespace Gamepad::_impl diff --git a/include/gamepad/recursive_mutex.hpp b/include/gamepad/recursive_mutex.hpp new file mode 100644 index 0000000..d892f2c --- /dev/null +++ b/include/gamepad/recursive_mutex.hpp @@ -0,0 +1,62 @@ +#include "pros/apix.h" +#include "pros/rtos.h" + +namespace Gamepad::_impl { + +class RecursiveMutex { + public: + /** + * @brief Construct a new recursive mutex + * + */ + RecursiveMutex() + : mutex(pros::c::mutex_recursive_create()) {} + + /** + * @brief Locks the recursive mutex, optionally bailing out after a timeout + * + * @param timeout How long to wait for the mutex before baling out + * @return true The mutex was successfully acquired + * @return false The mutex was not successfully acquired + */ + bool take(std::uint32_t timeout = TIMEOUT_MAX) { return pros::c::mutex_recursive_take(mutex, timeout); } + + /** + * @brief Locks the mutex, waiting indefinetely until the mutex is acquired + * + */ + void lock() { + while (!this->take()) pros::delay(2); + } + + /** + * @brief Attempts to lock the mutex without blocking the current thread + * + * @return true The mutex was successfully acquired + * @return false The mutex was not successfully acquired + */ + bool try_lock() { return this->take(0); } + + /** + * @brief Unlocks the mutex + * + * @return true The mutex was successfully released + * @return false The mutex was not successfully released + */ + bool give() { return pros::c::mutex_recursive_give(mutex); } + + /** + * @brief Unlocks the mutex, equivalent to \ref give() + * + */ + void unlock() { this->give(); } + + /** + * @brief Destroy the recursive mutex and free any allocated memory + */ + ~RecursiveMutex() { pros::c::mutex_delete(mutex); } + private: + pros::mutex_t mutex; +}; + +} // namespace Gamepad::_impl \ No newline at end of file diff --git a/include/gamepad/todo.hpp b/include/gamepad/todo.hpp new file mode 100644 index 0000000..c1e0b7b --- /dev/null +++ b/include/gamepad/todo.hpp @@ -0,0 +1,5 @@ +#pragma once + +#define DO_PRAGMA(x) _Pragma(#x) +#define TODO(x) DO_PRAGMA(message("TODO - " #x)) +#define FIXME(x) DO_PRAGMA(warning("FIXME - " #x)) \ No newline at end of file diff --git a/src/gamepad/controller.cpp b/src/gamepad/controller.cpp new file mode 100644 index 0000000..720c1f4 --- /dev/null +++ b/src/gamepad/controller.cpp @@ -0,0 +1,125 @@ +#include "gamepad/controller.hpp" +#include "gamepad/todo.hpp" +#include "pros/misc.h" +#include + +namespace Gamepad { +bool Button::onPress(std::string listenerName, std::function func) const { + return this->onPressEvent.add_listener(std::move(listenerName) + "_user", std::move(func)); +} + +bool Button::onLongPress(std::string listenerName, std::function func) const { + return this->onLongPressEvent.add_listener(std::move(listenerName) + "_user", std::move(func)); +} + +bool Button::onRelease(std::string listenerName, std::function func) const { + return this->onReleaseEvent.add_listener(std::move(listenerName) + "_user", std::move(func)); +} + +bool Button::onShortRelease(std::string listenerName, std::function func) const { + return this->onShortReleaseEvent.add_listener(std::move(listenerName) + "_user", std::move(func)); +} + +bool Button::addListener(EventType event, std::string listenerName, std::function func) const { + switch (event) { + case Gamepad::EventType::ON_PRESS: return this->onPress(std::move(listenerName), std::move(func)); + case Gamepad::EventType::ON_LONG_PRESS: return this->onLongPress(std::move(listenerName), std::move(func)); + case Gamepad::EventType::ON_RELEASE: return this->onRelease(std::move(listenerName), std::move(func)); + case Gamepad::EventType::ON_SHORT_RELEASE: + return this->onShortRelease(std::move(listenerName), std::move(func)); + default: + TODO("add error logging") + errno = EINVAL; + return false; + } +} + +bool Button::removeListener(std::string listenerName) const { + return this->onPressEvent.remove_listener(listenerName + "_user") || + this->onLongPressEvent.remove_listener(listenerName + "_user") || + this->onReleaseEvent.remove_listener(listenerName + "_user") || + this->onShortReleaseEvent.remove_listener(listenerName + "_user"); +} + +void Button::update(const bool is_held) { + this->rising_edge = !this->is_pressed && is_held; + this->falling_edge = this->is_pressed && !is_held; + this->is_pressed = is_held; + if (is_held) this->time_held += pros::millis() - this->last_update_time; + else this->time_released += pros::millis() - this->last_update_time; + + if (this->rising_edge) { + this->onPressEvent.fire(); + } else if (this->is_pressed && this->time_held >= this->long_press_threshold && + this->last_long_press_time <= pros::millis() - this->time_held) { + this->onLongPressEvent.fire(); + this->last_long_press_time = pros::millis(); + } else if (this->falling_edge) { + this->onReleaseEvent.fire(); + if (this->time_held < this->long_press_threshold) this->onShortReleaseEvent.fire(); + } + if (this->rising_edge) this->time_held = 0; + if (this->falling_edge) this->time_released = 0; + this->last_update_time = pros::millis(); +} + +void Controller::updateButton(pros::controller_digital_e_t button_id) { + Button Controller::*button = Controller::button_to_ptr(button_id); + bool is_held = this->controller.get_digital(button_id); + (this->*button).update(is_held); +} + +void Controller::update() { + for (int i = pros::E_CONTROLLER_DIGITAL_L1; i <= pros::E_CONTROLLER_DIGITAL_A; ++i) { + this->updateButton(static_cast(i)); + } + + this->m_LeftX = this->controller.get_analog(pros::E_CONTROLLER_ANALOG_LEFT_X); + this->m_LeftY = this->controller.get_analog(pros::E_CONTROLLER_ANALOG_LEFT_Y); + this->m_RightX = this->controller.get_analog(pros::E_CONTROLLER_ANALOG_RIGHT_X); + this->m_RightY = this->controller.get_analog(pros::E_CONTROLLER_ANALOG_RIGHT_Y); +} + +const Button& Controller::operator[](pros::controller_digital_e_t button) { + return this->*Controller::button_to_ptr(button); +} + +float Controller::operator[](pros::controller_analog_e_t axis) { + switch (axis) { + case pros::E_CONTROLLER_ANALOG_LEFT_X: return this->LeftX; + case pros::E_CONTROLLER_ANALOG_LEFT_Y: return this->LeftY; + case pros::E_CONTROLLER_ANALOG_RIGHT_X: return this->RightX; + case pros::E_CONTROLLER_ANALOG_RIGHT_Y: return this->RightY; + default: + TODO("add error logging") + errno = EINVAL; + return 0; + } +} + +std::string Controller::unique_name() { + static std::atomic i = 0; + return std::to_string(i++) + "_internal"; +} + +Button Controller::*Controller::button_to_ptr(pros::controller_digital_e_t button) { + switch (button) { + case pros::E_CONTROLLER_DIGITAL_L1: return &Controller::m_L1; + case pros::E_CONTROLLER_DIGITAL_L2: return &Controller::m_L2; + case pros::E_CONTROLLER_DIGITAL_R1: return &Controller::m_R1; + case pros::E_CONTROLLER_DIGITAL_R2: return &Controller::m_R2; + case pros::E_CONTROLLER_DIGITAL_UP: return &Controller::m_Up; + case pros::E_CONTROLLER_DIGITAL_DOWN: return &Controller::m_Down; + case pros::E_CONTROLLER_DIGITAL_LEFT: return &Controller::m_Left; + case pros::E_CONTROLLER_DIGITAL_RIGHT: return &Controller::m_Right; + case pros::E_CONTROLLER_DIGITAL_X: return &Controller::m_X; + case pros::E_CONTROLLER_DIGITAL_B: return &Controller::m_B; + case pros::E_CONTROLLER_DIGITAL_Y: return &Controller::m_Y; + case pros::E_CONTROLLER_DIGITAL_A: return &Controller::m_A; + default: + TODO("add error logging") + errno = EINVAL; + return &Controller::Fake; + } +} +} // namespace Gamepad diff --git a/src/main.cpp b/src/main.cpp index 1cc258a..2949002 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,20 +1,5 @@ #include "main.h" - -/** - * A callback function for LLEMU's center button. - * - * When this callback is fired, it will toggle line 2 of the LCD text between - * "I was pressed!" and nothing. - */ -void on_center_button() { - static bool pressed = false; - pressed = !pressed; - if (pressed) { - pros::lcd::set_text(2, "I was pressed!"); - } else { - pros::lcd::clear_line(2); - } -} +#include "gamepad/api.hpp" /** * Runs initialization code. This occurs as soon as the program is started. @@ -22,11 +7,27 @@ void on_center_button() { * All other competition modes are blocked by initialize; it is recommended * to keep execution time for this mode under a few seconds. */ -void initialize() { - pros::lcd::initialize(); - pros::lcd::set_text(1, "Hello PROS User!"); - pros::lcd::register_btn1_cb(on_center_button); +void downPress1() { printf("Down Press!\n"); } + +void upRelease1() { printf("Up Release!\n"); } + +void leftLongPress1() { printf("Left Long Press!\n"); } + +void leftShortRelease1() { printf("Left Short Release!\n"); } + +void initialize() { + // We can register functions to run when buttons are pressed + Gamepad::master.Down.onPress("downPress1", downPress1); + // ...or when they're released + Gamepad::master.Up.onRelease("downRelease1", upRelease1); + // There's also the longPress event + Gamepad::master.Left.onLongPress("leftLongPress1", leftLongPress1); + // We can have two functions on one button, + // just remember to give them different names + Gamepad::master.Left.onShortRelease("leftShortRelease", leftShortRelease1); + // And we can use lambda's too + Gamepad::master.X.onShortRelease("xShortRelease1", []() { printf("X Short Release!\n"); }); } /** @@ -74,21 +75,17 @@ void autonomous() {} * task, not resume it from where it left off. */ void opcontrol() { - pros::Controller master(pros::E_CONTROLLER_MASTER); - pros::MotorGroup left_mg({1, -2, 3}); // Creates a motor group with forwards ports 1 & 3 and reversed port 2 - pros::MotorGroup right_mg({-4, 5, -6}); // Creates a motor group with forwards port 4 and reversed ports 4 & 6 - - while (true) { - pros::lcd::print(0, "%d %d %d", (pros::lcd::read_buttons() & LCD_BTN_LEFT) >> 2, - (pros::lcd::read_buttons() & LCD_BTN_CENTER) >> 1, - - (pros::lcd::read_buttons() & LCD_BTN_RIGHT) >> 0); // Prints status of the emulated screen LCDs + pros::MotorGroup left_mg({1, -2, 3}); // Creates a motor group with forwards ports 1 & 3 and reversed port 2 + pros::MotorGroup right_mg({-4, 5, -6}); // Creates a motor group with forwards port 4 and reversed ports 4 & 6 - // Arcade control scheme - int dir = master.get_analog(ANALOG_LEFT_Y); // Gets amount forward/backward from left joystick - int turn = master.get_analog(ANALOG_RIGHT_X); // Gets the turn left/right from right joystick - left_mg.move(dir - turn); // Sets left motor voltage - right_mg.move(dir + turn); // Sets right motor voltage - pros::delay(20); // Run for 20 ms then update - } + while (true) { + // Remember to ALWAYS call update at the start of your while loop! + Gamepad::master.update(); + // We'll use the arcade control scheme + int dir = Gamepad::master.LeftY; // Gets amount forward/backward from left joystick + int turn = Gamepad::master.RightX; // Gets the turn left/right from right joystick + left_mg.move(dir - turn); // Sets left motor voltage + right_mg.move(dir + turn); // Sets right motor voltage + pros::delay(25); // Wait for 25 ms, then update the motor values again + } } \ No newline at end of file